fix(avoidance): take objects on same root lanelet into consideration of safety check (#5828)

fix(avoidance): use same root adjacent 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

@@ -638,6 +638,9 @@ struct DebugData
   // tmp for plot
   PathWithLaneId center_line;
 
+  // safety check area
+  lanelet::ConstLanelets safety_check_lanes;
+
   // collision check debug map
   utils::path_safety_checker::CollisionCheckDebugMap collision_check;
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

@@ -153,7 +153,8 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck(
 
 std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
   const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift);
+  const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift,
+  DebugData & debug);
 
 std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -618,6 +618,8 @@ MarkerArray createDebugMarkerArray(
       createMarkerColor(0.16, 1.0, 0.69, 0.2)));
     add(laneletsAsTriangleMarkerArray(
       "current_lanes", data.current_lanelets, createMarkerColor(1.0, 1.0, 1.0, 0.2)));
+    add(laneletsAsTriangleMarkerArray(
+      "safety_check_lanes", debug.safety_check_lanes, createMarkerColor(1.0, 0.0, 0.42, 0.2)));
   }
 
   return msg;

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -738,7 +738,7 @@ bool AvoidanceModule::isSafePath(
   const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate;
 
   const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects(
-    avoid_data_, planner_data_, parameters_, is_right_shift.value());
+    avoid_data_, planner_data_, parameters_, is_right_shift.value(), debug);
 
   for (const auto & object : safety_check_target_objects) {
     auto current_debug_data = utils::path_safety_checker::createObjectDebug(object);

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -1654,12 +1654,12 @@ lanelet::ConstLanelets getAdjacentLane(
 
   lanelet::ConstLanelets lanes{};
   for (const auto & lane : ego_succeeding_lanes) {
-    const auto opt_left_lane = rh->getLeftLanelet(lane);
+    const auto opt_left_lane = rh->getLeftLanelet(lane, true, false);
     if (!is_right_shift && opt_left_lane) {
       lanes.push_back(opt_left_lane.value());
     }
 
-    const auto opt_right_lane = rh->getRightLanelet(lane);
+    const auto opt_right_lane = rh->getRightLanelet(lane, true, false);
     if (is_right_shift && opt_right_lane) {
       lanes.push_back(opt_right_lane.value());
     }
@@ -1675,7 +1675,8 @@ lanelet::ConstLanelets getAdjacentLane(
 
 std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
   const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift)
+  const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift,
+  DebugData & debug)
 {
   const auto & p = parameters;
   const auto check_right_lanes =
@@ -1733,6 +1734,9 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
         utils::path_safety_checker::isCentroidWithinLanelet);
       append(targets);
     }
+
+    debug.safety_check_lanes.insert(
+      debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end());
   }
 
   // check left lanes
@@ -1752,6 +1756,9 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
         utils::path_safety_checker::isCentroidWithinLanelet);
       append(targets);
     }
+
+    debug.safety_check_lanes.insert(
+      debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end());
   }
 
   // check current lanes
@@ -1771,6 +1778,9 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
         utils::path_safety_checker::isCentroidWithinLanelet);
       append(targets);
     }
+
+    debug.safety_check_lanes.insert(
+      debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end());
   }
 
   return target_objects;