fix(lane_change): enrich lane change debug markers

planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md

@@ -318,6 +318,33 @@ The following parameters are configurable in `lane_change.param.yaml`.
 
 ## Debug Marker & Visualization
 
-To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner lane_change.publish_debug_marker true` (no restart is needed) or simply set the `publish_debug_marker` to `true` in the `lane_change.param.yaml` for permanent effect (restart is needed). Then add the marker `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanechange` in `rviz2`.
+To enable the debug marker, execute (no restart is needed)
 
-![debug](../image/lane_change/lane_change-debug.png)
+```shell
+ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner lane_change.publish_debug_marker true
+
+```
+
+or simply set the `publish_debug_marker` to `true` in the `lane_change.param.yaml` for permanent effect (restart is needed).
+
+Then add the marker
+
+```shell
+/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_left
+```
+
+in `rviz2`.
+
+![debug](../image/lane_change/lane_change-debug-1.png)
+
+![debug2](../image/lane_change/lane_change-debug-2.png)
+
+![debug3](../image/lane_change/lane_change-debug-3.png)
+
+Available information
+
+1. Ego to object relation, plus safety check information
+2. Ego vehicle interpolated pose up to the latest safety check position.
+3. Object is safe or not, shown by the color of the polygon (Green = Safe, Red = unsafe)
+4. Valid candidate paths.
+5. Position when lane changing start and end.

planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp

@@ -57,6 +57,10 @@ struct CollisionCheckDebug
   Pose expected_ego_pose{};
   Pose expected_obj_pose{};
   Pose relative_to_ego{};
+  double rss_longitudinal{0.0};
+  double ego_to_obj_margin{0.0};
+  double longitudinal_offset{0.0};
+  double lateral_offset{0.0};
   bool is_front{false};
   bool allow_lane_change{false};
   std::vector<Pose> lerped_path;

planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp

@@ -62,9 +62,10 @@ bool isTargetObjectFront(
 
 Polygon2d createExtendedPolygon(
   const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info,
-  const double lon_length, const double lat_margin);
+  const double lon_length, const double lat_margin, CollisionCheckDebug & debug);
 Polygon2d createExtendedPolygon(
-  const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin);
+  const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin,
+  CollisionCheckDebug & debug);
 
 double calcRssDistance(
   const double front_object_velocity, const double rear_object_velocity,

planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "behavior_path_planner/marker_util/debug_utilities.hpp"
 #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
 
 #include <behavior_path_planner/marker_util/lane_change/debug.hpp>
@@ -24,6 +25,7 @@
 #include <cstdint>
 #include <cstdlib>
 #include <iomanip>
+#include <sstream>
 #include <string>
 #include <vector>
 
@@ -37,7 +39,7 @@ using tier4_autoware_utils::createMarkerScale;
 MarkerArray showObjectInfo(
   const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns)
 {
-  Marker marker = createDefaultMarker(
+  Marker obj_marker = createDefaultMarker(
     "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
     createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(0.0, 1.0, 1.0, 0.999));
 
@@ -46,19 +48,24 @@ MarkerArray showObjectInfo(
 
   marker_array.markers.reserve(obj_debug_vec.size());
 
+  int idx{0};
+
   for (const auto & [uuid, info] : obj_debug_vec) {
-    marker.id = ++id;
-    marker.pose = info.current_pose;
+    obj_marker.id = ++id;
+    obj_marker.pose = info.current_pose;
 
     std::ostringstream ss;
 
-    ss << info.failed_reason << "\nLon: " << std::setprecision(4) << info.relative_to_ego.position.x
-       << "\nLat: " << info.relative_to_ego.position.y
-       << "\nPosition: " << (info.is_front ? "front" : "back");
+    ss << "Idx: " << ++idx << "\nReason: " << info.failed_reason
+       << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal
+       << "\nEgo to obj: " << info.ego_to_obj_margin << "\nLateral offset: " << info.lateral_offset
+       << "\nLongitudinal offset: " << info.longitudinal_offset
+       << "\nPosition: " << (info.is_front ? "front" : "back")
+       << "\nSafe: " << (info.allow_lane_change ? "Yes" : "No");
 
-    marker.text = ss.str();
+    obj_marker.text = ss.str();
 
-    marker_array.markers.push_back(marker);
+    marker_array.markers.push_back(obj_marker);
   }
   return marker_array;
 }
@@ -167,23 +174,28 @@ MarkerArray showPolygon(
 
   constexpr float scale_val{0.2};
   int32_t id{0};
+  const auto now = rclcpp::Clock{RCL_ROS_TIME}.now();
   Marker ego_marker = createDefaultMarker(
-    "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP,
-    createMarkerScale(scale_val, scale_val, scale_val), createMarkerColor(1.0, 1.0, 1.0, 0.9));
+    "map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val),
+    createMarkerColor(1.0, 1.0, 1.0, 0.9));
   Marker obj_marker = ego_marker;
+
+  auto text_marker = createDefaultMarker(
+    "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+    createMarkerScale(1.5, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 1.0));
+
   MarkerArray marker_array;
 
-  constexpr auto colors = colorsList();
-  const auto loop_size = std::min(obj_debug_vec.size(), colors.size());
+  constexpr auto red_color = colorsList().at(0);
+  constexpr auto green_color = colorsList().at(1);
+  const auto reserve_size = obj_debug_vec.size();
 
-  marker_array.markers.reserve(loop_size * 2);
-  size_t idx{0};
+  marker_array.markers.reserve(reserve_size * 4);
+
+  int32_t idx = {0};
 
   for (const auto & [uuid, info] : obj_debug_vec) {
-    if (idx >= loop_size) {
-      break;
-    }
-    const auto & color = colors.at(idx);
+    const auto & color = info.allow_lane_change ? green_color : red_color;
     const auto & ego_polygon = info.ego_polygon.outer();
     ego_marker.id = ++id;
     ego_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8);
@@ -193,6 +205,14 @@ MarkerArray showPolygon(
     }
     marker_array.markers.push_back(ego_marker);
 
+    std::ostringstream ss;
+    text_marker.id = ego_marker.id;
+    ss << ++idx;
+    text_marker.text = ss.str();
+    text_marker.pose = info.expected_ego_pose;
+
+    marker_array.markers.push_back(text_marker);
+
     const auto & obj_polygon = info.obj_polygon.outer();
     obj_marker.id = ++id;
     obj_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8);
@@ -201,7 +221,11 @@ MarkerArray showPolygon(
       obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0));
     }
     marker_array.markers.push_back(obj_marker);
-    ++idx;
+
+    text_marker.id = obj_marker.id;
+    text_marker.pose = info.expected_obj_pose;
+    marker_array.markers.push_back(text_marker);
+
     ego_marker.points.clear();
     obj_marker.points.clear();
   }

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -74,6 +74,7 @@ std::pair<bool, bool> NormalLaneChange::getSafePath(LaneChangePath & safe_path)
   LaneChangePaths valid_paths{};
   const auto found_safe_path =
     getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths);
+  debug_valid_path_ = valid_paths;
 
   if (valid_paths.empty()) {
     return {false, false};

planning/behavior_path_planner/src/utils/lane_change/utils.cpp

@@ -396,8 +396,8 @@ PathSafetyStatus isLaneChangePathSafe(
     return std::make_pair(tier4_autoware_utils::toHexString(obj.object_id), debug);
   };
 
-  const auto appendDebugInfo =
-    [&debug_data](std::pair<std::string, CollisionCheckDebug> & obj, bool && is_allowed) {
+  const auto updateDebugInfo =
+    [&debug_data](std::pair<std::string, CollisionCheckDebug> & obj, bool is_allowed) {
       const auto & key = obj.first;
       auto & element = obj.second;
       element.allow_lane_change = is_allowed;
@@ -438,7 +438,7 @@ PathSafetyStatus isLaneChangePathSafe(
             lane_change_path.duration.prepare,
             lane_change_parameter.prepare_segment_ignore_object_velocity_thresh)) {
         path_safety_status.is_safe = false;
-        appendDebugInfo(current_debug_data, false);
+        updateDebugInfo(current_debug_data, path_safety_status.is_safe);
         if (isObjectIndexIncluded(i, dynamic_objects_indices.target_lane)) {
           const auto & obj_pose = obj.kinematics.initial_pose_with_covariance.pose;
           const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape);
@@ -448,7 +448,7 @@ PathSafetyStatus isLaneChangePathSafe(
         }
       }
     }
-    appendDebugInfo(current_debug_data, true);
+    updateDebugInfo(current_debug_data, path_safety_status.is_safe);
   }
 
   if (!lane_change_parameter.use_predicted_path_outside_lanelet) {
@@ -469,13 +469,13 @@ PathSafetyStatus isLaneChangePathSafe(
           lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, front_decel,
           rear_decel, current_debug_data.second)) {
       path_safety_status.is_safe = false;
-      appendDebugInfo(current_debug_data, false);
+      updateDebugInfo(current_debug_data, path_safety_status.is_safe);
       const auto & obj_pose = obj.kinematics.initial_pose_with_covariance.pose;
       const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape);
       path_safety_status.is_object_coming_from_rear |= !utils::safety_check::isTargetObjectFront(
         path, current_pose, common_parameter.vehicle_info, obj_polygon);
     }
-    appendDebugInfo(current_debug_data, true);
+    updateDebugInfo(current_debug_data, path_safety_status.is_safe);
   }
   return path_safety_status;
 }