refactor(behavior_path_planner): visualize debug information of safet…

…y check against dynamic obstacles (autowarefoundation#4892)

* visualize debug info for safety check

Signed-off-by: kyoichi-sugahara <[email protected]>

* Update planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

Co-authored-by: Kosuke Takeuchi <[email protected]>

* fix build error

Signed-off-by: kyoichi-sugahara <[email protected]>

---------

Signed-off-by: kyoichi-sugahara <[email protected]>
Co-authored-by: Kosuke Takeuchi <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp

@@ -27,6 +27,7 @@
 namespace behavior_path_planner
 {
 using autoware_auto_perception_msgs::msg::PredictedObjects;
+using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap;
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
 using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane;
 /*
@@ -38,6 +39,8 @@ struct StartGoalPlannerData
   PredictedObjects filtered_objects;
   TargetObjectsOnLane target_objects_on_lane;
   std::vector<PoseWithVelocityStamped> ego_predicted_path;
+  // collision check debug map
+  CollisionCheckDebugMap collision_check;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp

@@ -18,6 +18,7 @@
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
+#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp"
 #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp"
 #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp"
 
@@ -31,6 +32,7 @@ namespace behavior_path_planner::utils::start_goal_planner_common
 {
 
 using behavior_path_planner::StartPlannerParameters;
+using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap;
 using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams;
 using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams;
 using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams;
@@ -85,6 +87,13 @@ void updatePathProperty(
   std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
   const std::pair<double, double> & pairs_terminal_velocity_and_accel);
 
+void initializeCollisionCheckDebugMap(CollisionCheckDebugMap & collision_check_debug_map);
+
+void updateSafetyCheckTargetObjectsData(
+  StartGoalPlannerData & data, const PredictedObjects & filtered_objects,
+  const TargetObjectsOnLane & target_objects_on_lane,
+  const std::vector<PoseWithVelocityStamped> & ego_predicted_path);
+
 std::pair<double, double> getPairsTerminalVelocityAndAccel(
   const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel,
   const size_t current_path_idx);

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -1901,10 +1901,9 @@ bool AvoidanceModule::isSafePath(
 
         safe_count_ = 0;
         return false;
-      } else {
-        marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true);
       }
     }
+    marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true);
   }
 
   safe_count_++;

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -1494,13 +1494,12 @@ bool GoalPlannerModule::isSafePath() const
     *route_handler, left_side_parking_, parameters_->backward_goal_search_length,
     parameters_->forward_goal_search_length);
   const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points);
-  const auto & common_param = planner_data_->parameters;
   const std::pair<double, double> terminal_velocity_and_accel =
     utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel(
       status_.pull_over_path->pairs_terminal_velocity_and_accel, status_.current_path_idx);
   RCLCPP_DEBUG(
-    getLogger(), "pairs_terminal_velocity_and_accel: %f, %f", terminal_velocity_and_accel.first,
-    terminal_velocity_and_accel.second);
+    getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f",
+    terminal_velocity_and_accel.first, terminal_velocity_and_accel.second);
   RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx);
   utils::start_goal_planner_common::updatePathProperty(
     ego_predicted_path_params_, terminal_velocity_and_accel);
@@ -1509,32 +1508,52 @@ bool GoalPlannerModule::isSafePath() const
       ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity,
       ego_seg_idx);
 
-  const auto filtered_objects = utils::path_safety_checker::filterObjects(
+  // filtering objects with velocity, position and class
+  const auto & filtered_objects = utils::path_safety_checker::filterObjects(
     dynamic_object, route_handler, pull_over_lanes, current_pose.position,
     objects_filtering_params_);
 
-  const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane(
+  // filtering objects based on the current position's lane
+  const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane(
     pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_);
 
   const double hysteresis_factor =
     status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate;
 
-  updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path);
+  utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData(
+    goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path);
+  utils::start_goal_planner_common::initializeCollisionCheckDebugMap(
+    goal_planner_data_.collision_check);
 
+  bool is_safe_dynamic_objects = true;
+  // Check for collisions with each predicted path of the object
   for (const auto & object : target_objects_on_lane.on_current_lane) {
+    auto current_debug_data = marker_utils::createObjectDebug(object);
+
+    bool is_safe_dynamic_object = true;
+
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       object, objects_filtering_params_->check_all_predicted_path);
+
+    // If a collision is detected, mark the object as unsafe and break the loop
     for (const auto & obj_path : obj_predicted_paths) {
-      CollisionCheckDebug collision{};
       if (!utils::path_safety_checker::checkCollision(
-            pull_over_path, ego_predicted_path, object, obj_path, common_param,
-            safety_check_params_->rss_params, hysteresis_factor, collision)) {
-        return false;
+            pull_over_path, ego_predicted_path, object, obj_path, planner_data_->parameters,
+            safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) {
+        marker_utils::updateCollisionCheckDebugMap(
+          goal_planner_data_.collision_check, current_debug_data, false);
+        is_safe_dynamic_objects = false;
+        is_safe_dynamic_object = false;
+        break;
       }
     }
+    if (is_safe_dynamic_object) {
+      marker_utils::updateCollisionCheckDebugMap(
+        goal_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object);
+    }
   }
 
-  return true;
+  return is_safe_dynamic_objects;
 }
 
 void GoalPlannerModule::setDebugData()

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -928,15 +928,6 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
   return turn_signal;
 }
 
-void StartPlannerModule::updateSafetyCheckTargetObjectsData(
-  const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane,
-  const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const
-{
-  start_planner_data_.filtered_objects = filtered_objects;
-  start_planner_data_.target_objects_on_lane = target_objects_on_lane;
-  start_planner_data_.ego_predicted_path = ego_predicted_path;
-}
-
 bool StartPlannerModule::isSafePath() const
 {
   // TODO(Sugahara): should safety check for backward path
@@ -950,46 +941,72 @@ bool StartPlannerModule::isSafePath() const
   const auto & route_handler = planner_data_->route_handler;
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
+
   const auto current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, backward_path_length, std::numeric_limits<double>::max(),
     /*forward_only_in_route*/ true);
+
+  // for ego predicted path
   const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_out_path.points);
-  const auto & common_param = planner_data_->parameters;
   const std::pair<double, double> terminal_velocity_and_accel =
     utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel(
       status_.pull_out_path.pairs_terminal_velocity_and_accel, status_.current_path_idx);
+  RCLCPP_DEBUG(
+    getLogger(), "pairs_terminal_velocity_and_accel for start_planner: %f, %f",
+    terminal_velocity_and_accel.first, terminal_velocity_and_accel.second);
+  RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx);
   utils::start_goal_planner_common::updatePathProperty(
     ego_predicted_path_params_, terminal_velocity_and_accel);
   const auto ego_predicted_path =
     behavior_path_planner::utils::path_safety_checker::createPredictedPath(
       ego_predicted_path_params_, pull_out_path.points, current_pose, current_velocity,
       ego_seg_idx);
 
-  const auto filtered_objects = utils::path_safety_checker::filterObjects(
+  // filtering objects with velocity, position and class
+  const auto & filtered_objects = utils::path_safety_checker::filterObjects(
     dynamic_object, route_handler, current_lanes, current_pose.position, objects_filtering_params_);
 
-  const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane(
+  // filtering objects based on the current position's lane
+  const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane(
     current_lanes, route_handler, filtered_objects, objects_filtering_params_);
 
   const double hysteresis_factor =
     status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate;
 
-  updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path);
+  utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData(
+    start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path);
+  utils::start_goal_planner_common::initializeCollisionCheckDebugMap(
+    start_planner_data_.collision_check);
 
+  bool is_safe_dynamic_objects = true;
+  // Check for collisions with each predicted path of the object
   for (const auto & object : target_objects_on_lane.on_current_lane) {
+    auto current_debug_data = marker_utils::createObjectDebug(object);
+
+    bool is_safe_dynamic_object = true;
+
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       object, objects_filtering_params_->check_all_predicted_path);
+
+    // If a collision is detected, mark the object as unsafe and break the loop
     for (const auto & obj_path : obj_predicted_paths) {
-      CollisionCheckDebug collision{};
       if (!utils::path_safety_checker::checkCollision(
-            pull_out_path, ego_predicted_path, object, obj_path, common_param,
-            safety_check_params_->rss_params, hysteresis_factor, collision)) {
-        return false;
+            pull_out_path, ego_predicted_path, object, obj_path, planner_data_->parameters,
+            safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) {
+        marker_utils::updateCollisionCheckDebugMap(
+          start_planner_data_.collision_check, current_debug_data, false);
+        is_safe_dynamic_objects = false;
+        is_safe_dynamic_object = false;
+        break;
       }
     }
+    if (is_safe_dynamic_object) {
+      marker_utils::updateCollisionCheckDebugMap(
+        start_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object);
+    }
   }
 
-  return true;
+  return is_safe_dynamic_objects;
 }
 
 bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const
@@ -1114,6 +1131,9 @@ void StartPlannerModule::setDebugData() const
   using marker_utils::createPathMarkerArray;
   using marker_utils::createPoseMarkerArray;
   using marker_utils::createPredictedPathMarkerArray;
+  using marker_utils::showPolygon;
+  using marker_utils::showPredictedPath;
+  using marker_utils::showSafetyCheckInfo;
   using tier4_autoware_utils::createDefaultMarker;
   using tier4_autoware_utils::createMarkerColor;
   using tier4_autoware_utils::createMarkerScale;
@@ -1143,6 +1163,25 @@ void StartPlannerModule::setDebugData() const
       start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9));
   }
 
+  // safety check
+  {
+    add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info"));
+    add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path"));
+    add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation"));
+  }
+
+  if (start_planner_data_.ego_predicted_path.size() > 0) {
+    const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath(
+      start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution);
+    add(createPredictedPathMarkerArray(
+      ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9));
+  }
+
+  if (start_planner_data_.filtered_objects.objects.size() > 0) {
+    add(createObjectsMarkerArray(
+      start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9));
+  }
+
   // Visualize planner type text
   const auto header = planner_data_->route_handler->getRouteHeader();
   {

planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp

@@ -142,6 +142,21 @@ void updatePathProperty(
   ego_predicted_path_params->acceleration = acceleration;
 }
 
+void initializeCollisionCheckDebugMap(CollisionCheckDebugMap & collision_check_debug_map)
+{
+  collision_check_debug_map.clear();
+}
+
+void updateSafetyCheckTargetObjectsData(
+  StartGoalPlannerData & data, const PredictedObjects & filtered_objects,
+  const TargetObjectsOnLane & target_objects_on_lane,
+  const std::vector<PoseWithVelocityStamped> & ego_predicted_path)
+{
+  data.filtered_objects = filtered_objects;
+  data.target_objects_on_lane = target_objects_on_lane;
+  data.ego_predicted_path = ego_predicted_path;
+}
+
 std::pair<double, double> getPairsTerminalVelocityAndAccel(
   const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel,
   const size_t current_path_idx)