fix(dynamic_avoidance): ignore objects on LC target lane (autowarefoundation#4137)

* fix(dynamic_avoidance): ignore objects on LC target lane

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update config

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml

@@ -16,6 +16,8 @@
 
         successive_num_to_entry_dynamic_avoidance_condition: 5
 
+        min_obj_lat_offset_to_ego_path: 0.3 # [m]
+
       drivable_area_generation:
         lat_offset_from_obstacle: 0.8 # [m]
         max_lat_offset_to_avoid: 0.5 # [m]

planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp

@@ -48,6 +48,7 @@ struct DynamicAvoidanceParameters
   bool avoid_pedestrian{false};
   double min_obstacle_vel{0.0};
   int successive_num_to_entry_dynamic_avoidance_condition{0};
+  double min_obj_lat_offset_to_ego_path{0.0};
 
   // drivable area generation
   double lat_offset_from_obstacle{0.0};

planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp

@@ -325,7 +325,8 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const
   const auto objects_in_right_lanes = getObjectsInLanes(input_objects, right_lanes);
   const auto objects_in_left_lanes = getObjectsInLanes(input_objects, left_lanes);
 
-  // 4. check if object will cut into the ego lane or cut out to the next lane.
+  // 4. check if object will not cut into the ego lane or cut out to the next lane,
+  //    or close to the ego's path considering ego's lane change.
   // NOTE: The oncoming object will be ignored.
   constexpr double epsilon_path_lat_diff = 0.3;
   std::vector<DynamicAvoidanceObjectCandidate> output_objects_candidate;
@@ -380,6 +381,15 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const
         continue;
       }
 
+      // Ignore object that is close to the ego's path.
+      const double lat_offset_to_path =
+        motion_utils::calcLateralOffset(prev_module_path->points, object.pose.position);
+      if (
+        std::abs(lat_offset_to_path) < planner_data_->parameters.vehicle_width / 2.0 +
+                                         parameters_->min_obj_lat_offset_to_ego_path) {
+        continue;
+      }
+
       // get previous object if it exists
       const auto prev_target_object_candidate = DynamicAvoidanceObjectCandidate::getObjectFromUuid(
         prev_target_objects_candidate_, object.uuid);

planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp

@@ -42,6 +42,8 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager(
     p.min_obstacle_vel = node->declare_parameter<double>(ns + "min_obstacle_vel");
     p.successive_num_to_entry_dynamic_avoidance_condition =
       node->declare_parameter<int>(ns + "successive_num_to_entry_dynamic_avoidance_condition");
+    p.min_obj_lat_offset_to_ego_path =
+      node->declare_parameter<double>(ns + "min_obj_lat_offset_to_ego_path");
   }
 
   {  // drivable_area_generation
@@ -92,6 +94,8 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
     updateParam<int>(
       parameters, ns + "successive_num_to_entry_dynamic_avoidance_condition",
       p->successive_num_to_entry_dynamic_avoidance_condition);
+    updateParam<double>(
+      parameters, ns + "min_obj_lat_offset_to_ego_path", p->min_obj_lat_offset_to_ego_path);
   }
 
   {  // drivable_area_generation