fix(avoidance): avoid merging vehicle if it's NOT on ego lane.

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -420,6 +420,9 @@ struct ObjectData  // avoidance target
   // is parked vehicle on road shoulder
   bool is_parked{false};
 
+  // is driving on ego current lane
+  bool is_on_ego_lane{false};
+
   // object direction.
   Direction direction{Direction::NONE};
 

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -478,6 +478,14 @@ bool isWithinIntersection(
     object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon())));
 }
 
+bool isOnEgoLane(const ObjectData & object)
+{
+  const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
+  return boost::geometry::within(
+    lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
+    object.overhang_lanelet.polygon2d().basicPolygon());
+}
+
 bool isParallelToEgoLane(const ObjectData & object, const double threshold)
 {
   const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
@@ -525,6 +533,10 @@ bool isParkedVehicle(
   using lanelet::utils::to2D;
   using lanelet::utils::conversion::toLaneletPoint;
 
+  if (object.is_within_intersection) {
+    return false;
+  }
+
   const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
   const auto centerline_pos =
     lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pos).position;
@@ -652,6 +664,10 @@ bool isForceAvoidanceTarget(
     return false;
   }
 
+  if (!object.is_on_ego_lane) {
+    return true;
+  }
+
   const auto & ego_pose = planner_data->self_odometry->pose.pose;
 
   // force avoidance for stopped vehicle
@@ -778,24 +794,16 @@ bool isSatisfiedWithVehicleCondition(
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {
-  using boost::geometry::within;
-  using lanelet::utils::to2D;
-  using lanelet::utils::conversion::toLaneletPoint;
-
   object.behavior = getObjectBehavior(object, parameters);
-  object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler);
+  object.is_on_ego_lane = isOnEgoLane(object);
 
   // from here condition check for vehicle type objects.
   if (isForceAvoidanceTarget(object, data, planner_data, parameters)) {
     return true;
   }
 
   // check vehicle shift ratio
-  const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
-  const auto on_ego_driving_lane = within(
-    to2D(toLaneletPoint(object_pos)).basicPoint(),
-    object.overhang_lanelet.polygon2d().basicPolygon());
-  if (on_ego_driving_lane) {
+  if (object.is_on_ego_lane) {
     if (object.is_parked) {
       return true;
     } else {
@@ -1755,6 +1763,8 @@ void filterTargetObjects(
     }
 
     if (filtering_utils::isVehicleTypeObject(o)) {
+      o.is_within_intersection =
+        filtering_utils::isWithinIntersection(o, planner_data->route_handler);
       o.is_parked = filtering_utils::isParkedVehicle(o, planner_data->route_handler, parameters);
       o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters);