feat(out_of_lane): also apply lat buffer between the lane and stop po…

…se (autowarefoundation#7918)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml

@@ -27,7 +27,8 @@
       action:  # action to insert in the trajectory if an object causes a conflict at an overlap
         skip_if_over_max_decel: true  # if true, do not take an action that would cause more deceleration than the maximum allowed
         precision: 0.1  # [m] precision when inserting a stop pose in the trajectory
-        distance_buffer: 1.5  # [m] buffer distance to try to keep between the ego footprint and lane
+        longitudinal_distance_buffer: 1.5  # [m] safety distance buffer to keep in front of the ego vehicle
+        lateral_distance_buffer: 1.0  # [m] safety distance buffer to keep on the side of the ego vehicle
         min_duration: 1.0  # [s] minimum duration needed before a decision can be canceled
         slowdown:
           distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp

@@ -70,9 +70,11 @@ std::optional<TrajectoryPoint> calculate_last_in_lane_pose(
 
 std::optional<SlowdownToInsert> calculate_slowdown_point(
   const EgoData & ego_data, const std::vector<Slowdown> & decisions,
-  const std::optional<SlowdownToInsert> prev_slowdown_point, PlannerParam params)
+  const std::optional<SlowdownToInsert> & prev_slowdown_point, PlannerParam params)
 {
-  params.extra_front_offset += params.dist_buffer;
+  params.extra_front_offset += params.lon_dist_buffer;
+  params.extra_right_offset += params.lat_dist_buffer;
+  params.extra_left_offset += params.lat_dist_buffer;
   const auto base_footprint = make_base_footprint(params);
 
   // search for the first slowdown decision for which a stop point can be inserted

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp

@@ -53,6 +53,6 @@ std::optional<TrajectoryPoint> calculate_last_in_lane_pose(
 /// @return optional slowdown point to insert in the trajectory
 std::optional<SlowdownToInsert> calculate_slowdown_point(
   const EgoData & ego_data, const std::vector<Slowdown> & decisions,
-  const std::optional<SlowdownToInsert> prev_slowdown_point, PlannerParam params);
+  const std::optional<SlowdownToInsert> & prev_slowdown_point, PlannerParam params);
 }  // namespace autoware::motion_velocity_planner::out_of_lane
 #endif  // CALCULATE_SLOWDOWN_POINTS_HPP_

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -93,7 +93,9 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node)
     getOrDeclareParameter<bool>(node, ns_ + ".action.skip_if_over_max_decel");
   pp.precision = getOrDeclareParameter<double>(node, ns_ + ".action.precision");
   pp.min_decision_duration = getOrDeclareParameter<double>(node, ns_ + ".action.min_duration");
-  pp.dist_buffer = getOrDeclareParameter<double>(node, ns_ + ".action.distance_buffer");
+  pp.lon_dist_buffer =
+    getOrDeclareParameter<double>(node, ns_ + ".action.longitudinal_distance_buffer");
+  pp.lat_dist_buffer = getOrDeclareParameter<double>(node, ns_ + ".action.lateral_distance_buffer");
   pp.slow_velocity = getOrDeclareParameter<double>(node, ns_ + ".action.slowdown.velocity");
   pp.slow_dist_threshold =
     getOrDeclareParameter<double>(node, ns_ + ".action.slowdown.distance_threshold");
@@ -139,7 +141,8 @@ void OutOfLaneModule::update_parameters(const std::vector<rclcpp::Parameter> & p
   updateParam(parameters, ns_ + ".action.skip_if_over_max_decel", pp.skip_if_over_max_decel);
   updateParam(parameters, ns_ + ".action.precision", pp.precision);
   updateParam(parameters, ns_ + ".action.min_duration", pp.min_decision_duration);
-  updateParam(parameters, ns_ + ".action.distance_buffer", pp.dist_buffer);
+  updateParam(parameters, ns_ + ".action.longitudinal_distance_buffer", pp.lon_dist_buffer);
+  updateParam(parameters, ns_ + ".action.lateral_distance_buffer", pp.lat_dist_buffer);
   updateParam(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity);
   updateParam(parameters, ns_ + ".action.slowdown.distance_threshold", pp.slow_dist_threshold);
   updateParam(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold);

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp

@@ -61,7 +61,8 @@ struct PlannerParam
   double overlap_min_dist;      // [m] min distance inside another lane to consider an overlap
   // action to insert in the trajectory if an object causes a conflict at an overlap
   bool skip_if_over_max_decel;  // if true, skip the action if it causes more than the max decel
-  double dist_buffer;
+  double lon_dist_buffer;       // [m] safety distance buffer to keep in front of the ego vehicle
+  double lat_dist_buffer;       // [m] safety distance buffer to keep on the side of the ego vehicle
   double slow_velocity;
   double slow_dist_threshold;
   double stop_dist_threshold;