feat(out_of_lane): ignore objects coming from behind ego (autowarefoundation#7891) (#1437)

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

Author: maxime-clem

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

@@ -19,6 +19,7 @@
         predicted_path_min_confidence : 0.1  # when using predicted paths, ignore the ones whose confidence is lower than this value.
         distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane
         cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights
+        ignore_behind_ego: true # if true, objects behind the ego vehicle are ignored
 
       overlap:
         minimum_distance: 0.0  # [m] minimum distance inside a lanelet for an overlap to be considered

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp

@@ -111,6 +111,14 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects(
       }) != object.classification.end();
     if (is_pedestrian) continue;
 
+    const auto is_coming_from_behind =
+      motion_utils::calcSignedArcLength(
+        ego_data.trajectory_points, ego_data.first_trajectory_idx,
+        object.kinematics.initial_pose_with_covariance.pose.position) < 0.0;
+    if (params.objects_ignore_behind_ego && is_coming_from_behind) {
+      continue;
+    }
+
     auto filtered_object = object;
     const auto is_invalid_predicted_path = [&](const auto & predicted_path) {
       const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence;

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -36,7 +36,6 @@
 #include <map>
 #include <memory>
 #include <string>
-#include <utility>
 #include <vector>
 
 namespace autoware::motion_velocity_planner
@@ -85,6 +84,8 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node)
   pp.objects_dist_buffer = getOrDeclareParameter<double>(node, ns_ + ".objects.distance_buffer");
   pp.objects_cut_predicted_paths_beyond_red_lights =
     getOrDeclareParameter<bool>(node, ns_ + ".objects.cut_predicted_paths_beyond_red_lights");
+  pp.objects_ignore_behind_ego =
+    getOrDeclareParameter<bool>(node, ns_ + ".objects.ignore_behind_ego");
 
   pp.overlap_min_dist = getOrDeclareParameter<double>(node, ns_ + ".overlap.minimum_distance");
   pp.overlap_extra_length = getOrDeclareParameter<double>(node, ns_ + ".overlap.extra_length");
@@ -132,7 +133,7 @@ void OutOfLaneModule::update_parameters(const std::vector<rclcpp::Parameter> & p
   updateParam(
     parameters, ns_ + ".objects.cut_predicted_paths_beyond_red_lights",
     pp.objects_cut_predicted_paths_beyond_red_lights);
-
+  updateParam(parameters, ns_ + ".objects.ignore_behind_ego", pp.objects_ignore_behind_ego);
   updateParam(parameters, ns_ + ".overlap.minimum_distance", pp.overlap_min_dist);
   updateParam(parameters, ns_ + ".overlap.extra_length", pp.overlap_extra_length);
 

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp

@@ -56,6 +56,7 @@ struct PlannerParam
   double objects_min_confidence;  // minimum confidence to consider a predicted path
   double objects_dist_buffer;  // [m] distance buffer used to determine if a collision will occur in
                                // the other lane
+  bool objects_ignore_behind_ego;  // if true, objects behind the ego vehicle are ignored
 
   double overlap_extra_length;  // [m] extra length to add around an overlap range
   double overlap_min_dist;      // [m] min distance inside another lane to consider an overlap

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp

@@ -418,7 +418,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocityPlannerNode::on_set_param
   updateParam(parameters, "ego_nearest_dist_threshold", planner_data_.ego_nearest_dist_threshold);
   updateParam(parameters, "ego_nearest_yaw_threshold", planner_data_.ego_nearest_yaw_threshold);
 
-  set_velocity_smoother_params();
+  // set_velocity_smoother_params(); TODO(Maxime): fix update parameters of the velocity smoother
 
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;