feat(out_of_lane): add objects.extra_width parameter (autowarefoundation#10923)

Signed-off-by: Maxime CLEMENT <[email protected]>

Author: maxime-clem

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md

@@ -63,6 +63,7 @@ In the debug visualization, the filtered predicted paths are shown in green and
 ### 5. Time to collisions
 
 For each out of lane area, we calculate the times when a dynamic object will overlap the area based on its filtered predicted paths.
+To make it more likely to detect collision in the other lanes, the width of the dynamic object can be increased using the parameter `objects.extra_width`.
 
 In the case where parameter `mode` is set to `threshold` and the calculated time is less than `threshold.time_threshold` parameter, then we decide to avoid the out of lane area.
 

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

@@ -17,6 +17,7 @@
         cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights
         ignore_behind_ego: false # if true, objects behind the ego vehicle are ignored
         validate_predicted_paths_on_lanelets: true  # if true, an out of lane collision is only considered if the predicted path fully follows a sequence of lanelets that include the out of lane lanelet
+        extra_width: 1.0  # [m] extra width around detected objects, making it more likely to detect an out of lane collision
 
       action:  # action to insert in the trajectory if an object causes a conflict at an overlap
         use_map_stop_lines: true  # if true, try to stop at stop lines defined in the vector map

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp

@@ -219,13 +219,15 @@ void calculate_object_path_time_collisions(
 void calculate_objects_time_collisions(
   OutOfLaneData & out_of_lane_data,
   const std::vector<autoware_perception_msgs::msg::PredictedObject> & objects,
-  const route_handler::RouteHandler & route_handler,
-  const bool validate_predicted_paths_on_lanelets)
+  const route_handler::RouteHandler & route_handler, const PlannerParam & params)
 {
   for (const auto & object : objects) {
+    auto shape = object.shape;
+    shape.dimensions.y += params.objects_extra_width * 0.5;
     for (const auto & path : object.kinematics.predicted_paths) {
       calculate_object_path_time_collisions(
-        out_of_lane_data, path, object.shape, route_handler, validate_predicted_paths_on_lanelets);
+        out_of_lane_data, path, object.shape, route_handler,
+        params.validate_predicted_paths_on_lanelets);
     }
   }
 }

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp

@@ -40,8 +40,7 @@ void calculate_object_path_time_collisions(
 void calculate_objects_time_collisions(
   OutOfLaneData & out_of_lane_data,
   const std::vector<autoware_perception_msgs::msg::PredictedObject> & objects,
-  const route_handler::RouteHandler & route_handler,
-  const bool validate_predicted_paths_on_lanelets);
+  const route_handler::RouteHandler & route_handler, const PlannerParam & params);
 
 /// @brief calculate the collisions to avoid
 /// @details either uses the time to collision or just the time when the object will arrive at the

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -99,6 +99,7 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node)
     get_or_declare_parameter<bool>(node, ns_ + ".objects.ignore_behind_ego");
   pp.validate_predicted_paths_on_lanelets =
     get_or_declare_parameter<bool>(node, ns_ + ".objects.validate_predicted_paths_on_lanelets");
+  pp.objects_extra_width = get_or_declare_parameter<double>(node, ns_ + ".objects.extra_width");
 
   pp.precision = get_or_declare_parameter<double>(node, ns_ + ".action.precision");
   pp.use_map_stop_lines = get_or_declare_parameter<bool>(node, ns_ + ".action.use_map_stop_lines");
@@ -147,6 +148,7 @@ void OutOfLaneModule::update_parameters(const std::vector<rclcpp::Parameter> & p
   update_param(
     parameters, ns_ + ".objects.validate_predicted_paths_on_lanelets",
     pp.validate_predicted_paths_on_lanelets);
+  update_param(parameters, ns_ + ".objects.extra_width", pp.objects_extra_width);
 
   update_param(parameters, ns_ + ".action.precision", pp.precision);
   update_param(parameters, ns_ + ".action.use_map_stop_lines", pp.use_map_stop_lines);
@@ -398,12 +400,11 @@ VelocityPlanningResult OutOfLaneModule::plan(
 
   stopwatch.tic("calculate_time_collisions");
   out_of_lane::calculate_objects_time_collisions(
-    out_of_lane_data, objects.objects, *planner_data->route_handler,
-    params_.validate_predicted_paths_on_lanelets);
+    out_of_lane_data, objects.objects, *planner_data->route_handler, params_);
   const auto calculate_time_collisions_us = stopwatch.toc("calculate_time_collisions");
 
   stopwatch.tic("calculate_times");
-  const auto is_stopping = previous_slowdown_pose_ ? true : false;
+  const auto is_stopping = previous_slowdown_pose_.has_value();
   out_of_lane::calculate_collisions_to_avoid(
     out_of_lane_data, ego_data.trajectory_points, params_, is_stopping);
   const auto calculate_times_us = stopwatch.toc("calculate_times");

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp

@@ -62,6 +62,7 @@ struct PlannerParam
     validate_predicted_paths_on_lanelets;  // if true, an out of lane collision is only considered
                                            // if the predicted path fully follows a sequence of
                                            // lanelets that include the out of lane lanelet
+  double objects_extra_width;              // [m] extra width to apply to the object footprints
 
   // action to insert in the trajectory if an object causes a collision at an overlap
   double lon_dist_buffer;      // [m] safety distance buffer to keep in front of the ego vehicle
@@ -148,7 +149,7 @@ struct SlowdownPose
 
   SlowdownPose() = default;
   SlowdownPose(
-    const double arc_length, const rclcpp::Time start_time, const geometry_msgs::msg::Pose & pose,
+    const double arc_length, const rclcpp::Time & start_time, const geometry_msgs::msg::Pose & pose,
     const bool is_active)
   : arc_length(arc_length), start_time(start_time), pose(pose), is_active(is_active)
   {