fix(dynamic_avoidance): avoidance against forked object's path (#5724)

Signed-off-by: Takayuki Murooka <[email protected]>
autowarefoundation · Nov 30, 2023 · c180bf6 · c180bf6
1 parent 6e2ab96
commit c180bf6
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 7 deletions.
diff --git a/...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
@@ -327,8 +327,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     const std::vector<PathPointWithLaneId> & ego_path, const geometry_msgs::msg::Pose & obj_pose,
     const autoware_auto_perception_msgs::msg::Shape & obj_shape) const;
   double calcValidStartLengthToAvoid(
-    const std::vector<PathPointWithLaneId> & path_points_for_object_polygon,
-    const size_t obj_seg_idx, const PredictedPath & obj_path,
+    const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose,
     const autoware_auto_perception_msgs::msg::Shape & obj_shape) const;
   MinMaxValue calcMinMaxLongitudinalOffsetToAvoid(
     const std::vector<PathPointWithLaneId> & path_points_for_object_polygon,

diff --git a/...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
@@ -887,7 +887,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid(
 
   if (obj_vel < 0) {
     const double valid_start_length_to_avoid =
-      calcValidStartLengthToAvoid(path_points_for_object_polygon, obj_seg_idx, obj_path, obj_shape);
+      calcValidStartLengthToAvoid(obj_path, obj_pose, obj_shape);
     return MinMaxValue{
       std::max(obj_lon_offset.min_value - start_length_to_avoid, valid_start_length_to_avoid),
       obj_lon_offset.max_value + end_length_to_avoid};
@@ -898,18 +898,22 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid(
 }
 
 double DynamicAvoidanceModule::calcValidStartLengthToAvoid(
-  const std::vector<PathPointWithLaneId> & path_points_for_object_polygon, const size_t obj_seg_idx,
-  const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape) const
+  const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose,
+  const autoware_auto_perception_msgs::msg::Shape & obj_shape) const
 {
+  const auto prev_module_path_points = getPreviousModuleOutput().path->points;
+  const size_t obj_seg_idx =
+    motion_utils::findNearestSegmentIndex(prev_module_path_points, obj_pose.position);
+
   const size_t valid_obj_path_end_idx = [&]() {
     int ego_path_idx = obj_seg_idx + 1;
     for (size_t obj_path_idx = 0; obj_path_idx < obj_path.path.size(); ++obj_path_idx) {
       bool are_paths_close{false};
       for (; 0 < ego_path_idx; --ego_path_idx) {
         const double dist_to_segment = calcDistanceToSegment(
           obj_path.path.at(obj_path_idx).position,
-          path_points_for_object_polygon.at(ego_path_idx).point.pose.position,
-          path_points_for_object_polygon.at(ego_path_idx - 1).point.pose.position);
+          prev_module_path_points.at(ego_path_idx).point.pose.position,
+          prev_module_path_points.at(ego_path_idx - 1).point.pose.position);
         if (
           dist_to_segment < planner_data_->parameters.vehicle_width / 2.0 +
                               parameters_->lat_offset_from_obstacle +