feat(out_of_lane): ignore objects coming from behind ego (autowarefou…

…ndation#7891) Signed-off-by: Maxime CLEMENT <[email protected]>
wuyitongkiriv · Jul 22, 2024 · ab841bc · ab841bc
1 parent fd8bc18
commit ab841bc
Show file tree

Hide file tree

Showing 5 changed files with 14 additions and 3 deletions.
diff --git a/...elocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/...elocity_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

diff --git a/...city_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/...city_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;

diff --git a/...n_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/...n_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);
 

diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/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

diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/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;