fix(dynamic_avoidance): ignore objects on LC target lane (autowarefou…

…ndation#632)

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

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/behavior_path_planner_node.cpp

@@ -723,6 +723,8 @@ DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam()
     p.min_obstacle_vel = declare_parameter<double>(ns + "min_obstacle_vel");
     p.successive_num_to_entry_dynamic_avoidance_condition =
       declare_parameter<int>(ns + "successive_num_to_entry_dynamic_avoidance_condition");
+    p.min_obj_lat_offset_to_ego_path =
+      declare_parameter<double>(ns + "min_obj_lat_offset_to_ego_path");
   }
 
   {  // drivable_area_generation

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

@@ -341,7 +341,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;
@@ -396,6 +397,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

@@ -53,6 +53,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