refactor(behavior_path_planner): move updateCollisionCheckDebugMap to…

… safety checker (#5670) refactor(goal_planner): move updateCollisionCheckDebugMap to safety checker Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Nov 24, 2023 · 9a88fc6 · 9a88fc6
1 parent 957331e
commit 9a88fc6
Show file tree

Hide file tree

Showing 9 changed files with 30 additions and 26 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp
@@ -56,9 +56,6 @@ inline int64_t bitShift(int64_t original_id)
   return original_id << (sizeof(int32_t) * 8 / 2);
 }
 
-void updateCollisionCheckDebugMap(
-  CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe);
-
 MarkerArray createPoseMarkerArray(
   const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g,
   const float & b);

diff --git a/...ior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/...ior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp
@@ -134,6 +134,8 @@ bool checkPolygonsIntersects(
 
 // debug
 CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj);
+void updateCollisionCheckDebugMap(
+  CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe);
 
 }  // namespace behavior_path_planner::utils::path_safety_checker
 

diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp
@@ -42,19 +42,6 @@ using tier4_autoware_utils::createMarkerScale;
 using tier4_autoware_utils::createPoint;
 using visualization_msgs::msg::Marker;
 
-void updateCollisionCheckDebugMap(
-  CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe)
-{
-  auto & [key, element] = object_debug;
-  element.is_safe = is_safe;
-  if (debug_map.find(key) != debug_map.end()) {
-    debug_map[key] = element;
-    return;
-  }
-
-  debug_map.insert(object_debug);
-}
-
 MarkerArray createPoseMarkerArray(
   const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g,
   const float & b)

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -2028,14 +2028,15 @@ bool AvoidanceModule::isSafePath(
       if (!utils::path_safety_checker::checkCollision(
             shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params,
             hysteresis_factor, current_debug_data.second)) {
-        marker_utils::updateCollisionCheckDebugMap(
+        utils::path_safety_checker::updateCollisionCheckDebugMap(
           debug.collision_check, current_debug_data, false);
 
         safe_count_ = 0;
         return false;
       }
     }
-    marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true);
+    utils::path_safety_checker::updateCollisionCheckDebugMap(
+      debug.collision_check, current_debug_data, true);
   }
 
   safe_count_++;

diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -1753,7 +1753,7 @@ bool GoalPlannerModule::checkSafetyWithRSS(
           planned_path, ego_predicted_path, object, obj_path, planner_data_->parameters,
           safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second);
 
-        marker_utils::updateCollisionCheckDebugMap(
+        utils::path_safety_checker::updateCollisionCheckDebugMap(
           goal_planner_data_.collision_check, current_debug_data, !has_collision);
 
         return has_collision;

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
@@ -1728,7 +1728,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
         current_debug_data.second);
 
       if (collided_polygons.empty()) {
-        marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe);
+        utils::path_safety_checker::updateCollisionCheckDebugMap(
+          debug_data, current_debug_data, is_safe);
         continue;
       }
 
@@ -1738,20 +1739,23 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
         utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes);
 
       if (!collision_in_current_lanes && !collision_in_target_lanes) {
-        marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe);
+        utils::path_safety_checker::updateCollisionCheckDebugMap(
+          debug_data, current_debug_data, is_safe);
         continue;
       }
 
       is_safe = false;
       path_safety_status.is_safe = false;
-      marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe);
+      utils::path_safety_checker::updateCollisionCheckDebugMap(
+        debug_data, current_debug_data, is_safe);
       const auto & obj_pose = obj.initial_pose.pose;
       const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape);
       path_safety_status.is_object_coming_from_rear |=
         !utils::path_safety_checker::isTargetObjectFront(
           path, current_pose, common_parameters.vehicle_info, obj_polygon);
     }
-    marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe);
+    utils::path_safety_checker::updateCollisionCheckDebugMap(
+      debug_data, current_debug_data, is_safe);
   }
 
   return path_safety_status;

diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -1104,15 +1104,15 @@ bool StartPlannerModule::isSafePath() const
       if (!utils::path_safety_checker::checkCollision(
             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(
+        utils::path_safety_checker::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(
+      utils::path_safety_checker::updateCollisionCheckDebugMap(
         start_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object);
     }
   }

diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp
@@ -989,7 +989,7 @@ 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);
+    utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false);
     return true;
   }
 

diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp
@@ -406,4 +406,17 @@ CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj)
   return {tier4_autoware_utils::toHexString(obj.uuid), debug};
 }
 
+void updateCollisionCheckDebugMap(
+  CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe)
+{
+  auto & [key, element] = object_debug;
+  element.is_safe = is_safe;
+  if (debug_map.find(key) != debug_map.end()) {
+    debug_map[key] = element;
+    return;
+  }
+
+  debug_map.insert(object_debug);
+}
+
 }  // namespace behavior_path_planner::utils::path_safety_checker