fix(lane_change): add visualization for passParkedObject

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
zulfaqar-azmi-t4 · Oct 12, 2023 · a22ab3d · a22ab3d
1 parent d5709ab
commit a22ab3d
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 3 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp
@@ -52,6 +52,7 @@ using data::lane_change::PathSafetyStatus;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
+using path_safety_checker::CollisionCheckDebugMap;
 using route_handler::Direction;
 using tier4_autoware_utils::Polygon2d;
 
@@ -170,7 +171,8 @@ bool isParkedObject(
 bool passParkedObject(
   const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
   const std::vector<ExtendedPredictedObject> & objects, const double minimum_lane_change_length,
-  const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters);
+  const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters,
+  CollisionCheckDebugMap & object_debug);
 
 boost::optional<size_t> getLeadingStaticObjectIdx(
   const RouteHandler & route_handler, const LaneChangePath & lane_change_path,

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -1224,7 +1224,7 @@ bool NormalLaneChange::getLaneChangePaths(
 
         if (utils::lane_change::passParkedObject(
               route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer,
-              is_goal_in_route, *lane_change_parameters_)) {
+              is_goal_in_route, *lane_change_parameters_, object_debug_)) {
           return false;
         }
         candidate_paths->push_back(*candidate_path);

diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp
@@ -930,7 +930,8 @@ bool isParkedObject(
 bool passParkedObject(
   const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
   const std::vector<ExtendedPredictedObject> & objects, const double minimum_lane_change_length,
-  const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters)
+  const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters,
+  CollisionCheckDebugMap & object_debug)
 {
   const auto & object_check_min_road_shoulder_width =
     lane_change_parameters.object_check_min_road_shoulder_width;
@@ -953,6 +954,7 @@ bool passParkedObject(
   }
 
   const auto & leading_obj = objects.at(*leading_obj_idx);
+  auto debug = marker_utils::createObjectDebug(leading_obj);
   const auto leading_obj_poly =
     tier4_autoware_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape);
   if (leading_obj_poly.outer().empty()) {
@@ -976,6 +978,8 @@ bool passParkedObject(
 
   // If there are still enough length after the target object, we delay the lane change
   if (min_dist_to_end_of_current_lane > minimum_lane_change_length) {
+    debug.second.unsafe_reason = "delay lane change";
+    marker_utils::updateCollisionCheckDebugMap(object_debug, debug, false);
     return true;
   }