feat(lane_change): additional debug markers

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp

@@ -21,6 +21,9 @@
 
 #include <geometry_msgs/msg/detail/polygon__struct.hpp>
 
+#include <lanelet2_core/Forward.h>
+
+#include <limits>
 #include <string>
 
 namespace behavior_path_planner::data::lane_change
@@ -34,7 +37,17 @@ struct Debug
   CollisionCheckDebugMap collision_check_objects_after_approval;
   LaneChangeTargetObjects filtered_objects;
   geometry_msgs::msg::Polygon execution_area;
+  geometry_msgs::msg::Pose ego_pose;
+  lanelet::ConstLanelets current_lanes;
+  lanelet::ConstLanelets target_lanes;
+  lanelet::ConstLanelets target_backward_lanes;
   double collision_check_object_debug_lifetime{0.0};
+  double distance_to_end_of_current_lane{std::numeric_limits<double>::max()};
+  double distance_to_lane_change_finished{std::numeric_limits<double>::max()};
+  double distance_to_abort_finished{std::numeric_limits<double>::max()};
+  bool is_able_to_return_to_current_lane{false};
+  bool is_stuck{false};
+  bool is_abort{false};
 
   void reset()
   {
@@ -44,7 +57,18 @@ struct Debug
     filtered_objects.current_lane.clear();
     filtered_objects.target_lane.clear();
     filtered_objects.other_lane.clear();
+    execution_area.points.clear();
+    current_lanes.clear();
+    target_lanes.clear();
+    target_backward_lanes.clear();
+
     collision_check_object_debug_lifetime = 0.0;
+    distance_to_end_of_current_lane = std::numeric_limits<double>::max();
+    distance_to_lane_change_finished = std::numeric_limits<double>::max();
+    distance_to_abort_finished = std::numeric_limits<double>::max();
+    is_able_to_return_to_current_lane = false;
+    is_stuck = false;
+    is_abort = false;
   }
 };
 }  // namespace behavior_path_planner::data::lane_change

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp

@@ -20,6 +20,7 @@
 #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 
 #include <geometry_msgs/msg/detail/polygon__struct.hpp>
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
 #include <visualization_msgs/msg/detail/marker_array__struct.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
@@ -42,7 +43,9 @@ MarkerArray showFilteredObjects(
   const ExtendedPredictedObjects & target_lane_objects,
   const ExtendedPredictedObjects & other_lane_objects, const std::string & ns);
 MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area);
-MarkerArray createDebugMarkerArray(const Debug & debug_data);
+MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose);
+MarkerArray createDebugMarkerArray(
+  const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose);
 
 }  // namespace marker_utils::lane_change_markers
 #endif  // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -83,15 +83,16 @@ void LaneChangeInterface::updateData()
   if (isWaitingApproval()) {
     module_type_->updateLaneChangeStatus();
   }
-  updateDebugMarker();
 
   module_type_->updateSpecialData();
   module_type_->resetStopPose();
+  updateDebugMarker();
 }
 
 void LaneChangeInterface::postProcess()
 {
   post_process_safety_status_ = module_type_->isApprovedPathSafe();
+  updateDebugMarker();
 }
 
 BehaviorModuleOutput LaneChangeInterface::plan()
@@ -186,6 +187,7 @@ bool LaneChangeInterface::canTransitSuccessState()
   auto log_debug_throttled = [&](std::string_view message) -> void {
     RCLCPP_DEBUG(getLogger(), "%s", message.data());
   };
+  updateDebugMarker();
 
   if (module_type_->specialExpiredCheck() && isWaitingApproval()) {
     log_debug_throttled("Run specialExpiredCheck.");
@@ -220,6 +222,7 @@ bool LaneChangeInterface::canTransitFailureState()
     RCLCPP_DEBUG(getLogger(), "%s", message.data());
   };
 
+  updateDebugMarker();
   log_debug_throttled(__func__);
 
   if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) {
@@ -319,7 +322,7 @@ void LaneChangeInterface::updateDebugMarker() const
 
   debug_marker_.markers.clear();
   using marker_utils::lane_change_markers::createDebugMarkerArray;
-  debug_marker_ = createDebugMarkerArray(module_type_->getDebugData());
+  debug_marker_ = createDebugMarkerArray(module_type_->getDebugData(), module_type_->getEgoPose());
 }
 
 MarkerArray LaneChangeInterface::getModuleVirtualWall()

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -539,15 +539,20 @@ bool NormalLaneChange::isNearEndOfCurrentLanes(
   const auto lane_change_buffer =
     utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals);
 
-  auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes);
+  const auto distance_to_lane_change_end = std::invoke([&]() {
+    auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes);
 
-  if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) {
-    distance_to_end = std::min(
-      distance_to_end,
-      utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes));
-  }
+    if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) {
+      distance_to_end = std::min(
+        distance_to_end,
+        utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes));
+    }
+
+    return std::max(0.0, distance_to_end) - lane_change_buffer;
+  });
 
-  return (std::max(0.0, distance_to_end) - lane_change_buffer) < threshold;
+  lane_change_debug_.distance_to_end_of_current_lane = distance_to_lane_change_end;
+  return distance_to_lane_change_end < threshold;
 }
 
 bool NormalLaneChange::hasFinishedLaneChange() const
@@ -565,29 +570,34 @@ bool NormalLaneChange::hasFinishedLaneChange() const
   }
 
   const auto reach_lane_change_end = dist_to_lane_change_end + finish_judge_buffer < 0.0;
+
+  lane_change_debug_.distance_to_lane_change_finished =
+    dist_to_lane_change_end + finish_judge_buffer;
+
   if (!reach_lane_change_end) {
     return false;
   }
 
   const auto arc_length = lanelet::utils::getArcCoordinates(status_.target_lanes, current_pose);
   const auto reach_target_lane =
     std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold;
-  if (!reach_target_lane) {
-    return false;
-  }
 
-  return true;
+  lane_change_debug_.distance_to_lane_change_finished = arc_length.distance;
+
+  return reach_target_lane;
 }
 
 bool NormalLaneChange::isAbleToReturnCurrentLane() const
 {
   if (status_.lane_change_path.path.points.size() < 2) {
+    lane_change_debug_.is_able_to_return_to_current_lane = false;
     return false;
   }
 
   if (!utils::isEgoWithinOriginalLane(
         status_.current_lanes, getEgoPose(), planner_data_->parameters,
         lane_change_parameters_->cancel.overhang_tolerance)) {
+    lane_change_debug_.is_able_to_return_to_current_lane = false;
     return false;
   }
 
@@ -605,12 +615,15 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const
     dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1);
     if (dist > estimated_travel_dist) {
       const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose;
-      return utils::isEgoWithinOriginalLane(
+      auto is_ego_within_original_lane = utils::isEgoWithinOriginalLane(
         status_.current_lanes, estimated_pose, planner_data_->parameters,
         lane_change_parameters_->cancel.overhang_tolerance);
+      lane_change_debug_.is_able_to_return_to_current_lane = is_ego_within_original_lane;
+      return is_ego_within_original_lane;
     }
   }
 
+  lane_change_debug_.is_able_to_return_to_current_lane = true;
   return true;
 }
 
@@ -651,17 +664,18 @@ bool NormalLaneChange::isAbleToStopSafely() const
 bool NormalLaneChange::hasFinishedAbort() const
 {
   if (!abort_path_) {
+    lane_change_debug_.is_abort = true;
     return true;
   }
 
   const auto distance_to_finish = calcSignedArcLength(
     abort_path_->path.points, getEgoPosition(), abort_path_->info.shift_line.end.position);
+  lane_change_debug_.distance_to_abort_finished = distance_to_finish;
 
-  if (distance_to_finish < 0.0) {
-    return true;
-  }
+  const auto has_finished_abort = distance_to_finish < 0.0;
+  lane_change_debug_.is_abort = has_finished_abort;
 
-  return false;
+  return has_finished_abort;
 }
 
 bool NormalLaneChange::isAbortState() const
@@ -678,6 +692,7 @@ bool NormalLaneChange::isAbortState() const
     return false;
   }
 
+  lane_change_debug_.is_abort = true;
   return true;
 }
 int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const
@@ -832,6 +847,10 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
   const auto target_backward_lanes = utils::lane_change::getBackwardLanelets(
     route_handler, target_lanes, current_pose, backward_length);
 
+  lane_change_debug_.current_lanes = current_lanes;
+  lane_change_debug_.target_lanes = target_lanes;
+  lane_change_debug_.target_backward_lanes = target_backward_lanes;
+
   // filter objects to get target object index
   const auto target_obj_index =
     filterObject(objects, current_lanes, target_lanes, target_backward_lanes);
@@ -2051,6 +2070,7 @@ bool NormalLaneChange::isVehicleStuck(
 bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const
 {
   if (current_lanes.empty()) {
+    lane_change_debug_.is_stuck = false;
     return false;  // can not check
   }
 
@@ -2070,7 +2090,10 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan
                                   getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN;
   RCLCPP_DEBUG(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc);
 
-  return isVehicleStuck(current_lanes, detection_distance);
+  auto is_vehicle_stuck = isVehicleStuck(current_lanes, detection_distance);
+
+  lane_change_debug_.is_stuck = is_vehicle_stuck;
+  return is_vehicle_stuck;
 }
 
 void NormalLaneChange::setStopPose(const Pose & stop_pose)

planning/behavior_path_lane_change_module/src/utils/markers.cpp

@@ -17,15 +17,18 @@
 #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
 
 #include <behavior_path_lane_change_module/utils/markers.hpp>
+#include <lanelet2_extension/visualization/visualization.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
 #include <visualization_msgs/msg/detail/marker__struct.hpp>
 #include <visualization_msgs/msg/detail/marker_array__struct.hpp>
 
 #include <cstdint>
 #include <cstdlib>
+#include <sstream>
 #include <string>
 #include <vector>
 
@@ -122,8 +125,39 @@ MarkerArray showFilteredObjects(
   return marker_array;
 }
 
-MarkerArray createDebugMarkerArray(const Debug & debug_data)
+MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose)
 {
+  auto default_text_marker = [&]() {
+    return createDefaultMarker(
+      "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "execution_info", 0, Marker::TEXT_VIEW_FACING,
+      createMarkerScale(0.5, 0.5, 0.5), colors::white());
+  };
+
+  MarkerArray marker_array;
+
+  auto safety_check_info_text = default_text_marker();
+  safety_check_info_text.pose = ego_pose;
+  safety_check_info_text.pose.position.z += 4.0;
+
+  std::ostringstream ss;
+
+  ss << "\nDistToEndOfCurrentLane: " << std::setprecision(5)
+     << debug_data.distance_to_end_of_current_lane
+     << "\nDistToLaneChangeFinished: " << debug_data.distance_to_lane_change_finished
+     << (debug_data.is_stuck ? "\nVehicleStuck" : "")
+     << (debug_data.is_able_to_return_to_current_lane ? "\nAbleToReturnToCurrentLane" : "")
+     << (debug_data.is_abort ? "\nAborting" : "")
+     << "\nDistanceToAbortFinished: " << debug_data.distance_to_abort_finished;
+
+  safety_check_info_text.text = ss.str();
+  marker_array.markers.push_back(safety_check_info_text);
+  return marker_array;
+}
+
+MarkerArray createDebugMarkerArray(
+  const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose)
+{
+  using lanelet::visualization::laneletsAsTriangleMarkerArray;
   using marker_utils::showPolygon;
   using marker_utils::showPredictedPath;
   using marker_utils::showSafetyCheckInfo;
@@ -141,6 +175,20 @@ MarkerArray createDebugMarkerArray(const Debug & debug_data)
     tier4_autoware_utils::appendMarkerArray(added, &debug_marker);
   };
 
+  if (!debug_data.execution_area.points.empty()) {
+    add(createPolygonMarkerArray(
+      debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1));
+  }
+
+  add(showExecutionInfo(debug_data, ego_pose));
+
+  // lanes
+  add(laneletsAsTriangleMarkerArray(
+    "current_lanes", debug_data.current_lanes, colors::light_yellow(0.2)));
+  add(laneletsAsTriangleMarkerArray("target_lanes", debug_data.target_lanes, colors::aqua(0.2)));
+  add(laneletsAsTriangleMarkerArray(
+    "target_backward_lanes", debug_data.target_backward_lanes, colors::blue(0.2)));
+
   add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths"));
   add(showFilteredObjects(
     debug_filtered_objects.current_lane, debug_filtered_objects.target_lane,
@@ -162,11 +210,6 @@ MarkerArray createDebugMarkerArray(const Debug & debug_data)
       "ego_and_target_polygon_relation_after_approval"));
   }
 
-  if (!debug_data.execution_area.points.empty()) {
-    add(createPolygonMarkerArray(
-      debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1));
-  }
-
   return debug_marker;
 }
 }  // namespace marker_utils::lane_change_markers