refactor(behavior_path_planner): type aliasing for collision check de…

…bug type (autowarefoundation#4796)

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp

@@ -25,20 +25,13 @@
 namespace marker_utils::lane_change_markers
 {
 using behavior_path_planner::LaneChangePath;
-using marker_utils::CollisionCheckDebug;
 using visualization_msgs::msg::MarkerArray;
-MarkerArray showObjectInfo(
-  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
+MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 MarkerArray showAllValidLaneChangePath(
   const std::vector<LaneChangePath> & lanes, std::string && ns);
-MarkerArray showLerpedPose(
-  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
-MarkerArray showEgoPredictedPaths(
-  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
-MarkerArray showPolygon(
-  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
-MarkerArray showPolygonPose(
-  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
+MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
+MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
+MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 MarkerArray createLaneChangingVirtualWallMarker(
   const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
   const rclcpp::Time & now, const std::string & ns);

planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp

@@ -15,6 +15,7 @@
 #define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_
 
 #include "behavior_path_planner/data_manager.hpp"
+#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 
@@ -29,7 +30,6 @@
 #include <lanelet2_core/geometry/Lanelet.h>
 
 #include <string>
-#include <unordered_map>
 #include <vector>
 
 namespace marker_utils
@@ -39,6 +39,9 @@ using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using behavior_path_planner::DrivableLanes;
 using behavior_path_planner::ShiftLineArray;
+using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap;
+using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair;
+using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Polygon;
 using geometry_msgs::msg::Pose;
@@ -49,27 +52,6 @@ using tier4_autoware_utils::Polygon2d;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
-struct CollisionCheckDebug
-{
-  std::string unsafe_reason;                ///< Reason indicating unsafe situation.
-  Pose current_pose{};                      ///< Ego vehicle's current pose.
-  Twist current_twist{};                    ///< Ego vehicle's current velocity and rotation.
-  Twist object_twist{};                     ///< Detected object's velocity and rotation.
-  Pose expected_ego_pose{};                 ///< Predicted future pose of ego vehicle.
-  Pose expected_obj_pose{};                 ///< Predicted future pose of object.
-  double rss_longitudinal{0.0};             ///< Longitudinal RSS measure.
-  double inter_vehicle_distance{0.0};       ///< Distance between ego vehicle and object.
-  double extended_polygon_lon_offset{0.0};  ///< Longitudinal offset for extended polygon.
-  double extended_polygon_lat_offset{0.0};  ///< Lateral offset for extended polygon.
-  bool is_front{false};                     ///< True if object is in front of ego vehicle.
-  bool is_safe{false};                      ///< True if situation is deemed safe.
-  std::vector<Pose> lerped_path;            ///< Interpolated ego vehicle path.
-  std::vector<PredictedPath> ego_predicted_path{};  ///< Predicted future path of ego vehicle.
-  Polygon2d extended_ego_polygon{};                 ///< Ego vehicle's extended collision polygon.
-  Polygon2d extended_obj_polygon{};  ///< Detected object's extended collision polygon.
-};
-using CollisionCheckDebugMap = std::unordered_map<std::string, CollisionCheckDebug>;
-
 constexpr std::array<std::array<float, 3>, 10> colorsList()
 {
   constexpr std::array<float, 3> red = {1., 0., 0.};
@@ -85,12 +67,16 @@ constexpr std::array<std::array<float, 3>, 10> colorsList()
   return {red,     green,         blue,       yellow,       aqua,
           magenta, medium_orchid, light_pink, light_yellow, light_steel_blue};
 }
-
 inline int64_t bitShift(int64_t original_id)
 {
   return original_id << (sizeof(int32_t) * 8 / 2);
 }
 
+CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj);
+
+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);

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

@@ -43,11 +43,11 @@
 namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap;
 using data::lane_change::PathSafetyStatus;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
-using marker_utils::CollisionCheckDebugMap;
 using route_handler::Direction;
 using tier4_autoware_utils::StopWatch;
 using tier4_planning_msgs::msg::LaneChangeDebugMsg;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -26,15 +26,15 @@
 namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
+using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap;
 using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped;
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
 using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
-using marker_utils::CollisionCheckDebug;
-using marker_utils::CollisionCheckDebugMap;
 using route_handler::Direction;
 using tier4_planning_msgs::msg::LaneChangeDebugMsg;
 using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
@@ -141,7 +141,7 @@ class NormalLaneChange : public LaneChangeBase
   PathSafetyStatus isLaneChangePathSafe(
     const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects,
     const utils::path_safety_checker::RSSparams & rss_params,
-    std::unordered_map<std::string, CollisionCheckDebug> & debug_data) const;
+    CollisionCheckDebugMap & debug_data) const;
 
   rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr());
 };

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -48,7 +48,7 @@ using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::TransformStamped;
 
-using marker_utils::CollisionCheckDebug;
+using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
 
 struct ObjectParameter
 {

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp

@@ -19,13 +19,18 @@
 
 #include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
 #include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/twist.hpp>
 
+#include <string>
+#include <unordered_map>
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner::utils::path_safety_checker
 {
 
 using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::Twist;
 using tier4_autoware_utils::Polygon2d;
 
 struct PoseWithVelocity
@@ -171,6 +176,28 @@ struct SafetyCheckParams
   bool publish_debug_marker{false};  ///< Option to publish debug markers.
 };
 
+struct CollisionCheckDebug
+{
+  std::string unsafe_reason;                ///< Reason indicating unsafe situation.
+  Pose current_pose{};                      ///< Ego vehicle's current pose.
+  Twist current_twist{};                    ///< Ego vehicle's current velocity and rotation.
+  Twist object_twist{};                     ///< Detected object's velocity and rotation.
+  Pose expected_ego_pose{};                 ///< Predicted future pose of ego vehicle.
+  Pose expected_obj_pose{};                 ///< Predicted future pose of object.
+  double rss_longitudinal{0.0};             ///< Longitudinal RSS measure.
+  double inter_vehicle_distance{0.0};       ///< Distance between ego vehicle and object.
+  double extended_polygon_lon_offset{0.0};  ///< Longitudinal offset for extended polygon.
+  double extended_polygon_lat_offset{0.0};  ///< Lateral offset for extended polygon.
+  bool is_front{false};                     ///< True if object is in front of ego vehicle.
+  bool is_safe{false};                      ///< True if situation is deemed safe.
+  std::vector<Pose> lerped_path;            ///< Interpolated ego vehicle path.
+  Polygon2d extended_ego_polygon{};         ///< Ego vehicle's extended collision polygon.
+  Polygon2d extended_obj_polygon{};         ///< Detected object's extended collision polygon.
+};
+using CollisionCheckDebugPair = std::pair<std::string, CollisionCheckDebug>;
+using CollisionCheckDebugMap =
+  std::unordered_map<CollisionCheckDebugPair::first_type, CollisionCheckDebugPair::second_type>;
+
 }  // namespace behavior_path_planner::utils::path_safety_checker
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_

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

@@ -43,9 +43,9 @@ namespace behavior_path_planner::utils::path_safety_checker
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_perception_msgs::msg::Shape;
+using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
-using marker_utils::CollisionCheckDebug;
 using tier4_autoware_utils::Point2d;
 using tier4_autoware_utils::Polygon2d;
 using vehicle_info_util::VehicleInfo;

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

@@ -36,8 +36,7 @@ using tier4_autoware_utils::createDefaultMarker;
 using tier4_autoware_utils::createMarkerColor;
 using tier4_autoware_utils::createMarkerScale;
 
-MarkerArray showObjectInfo(
-  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns)
+MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
 {
   Marker obj_marker = createDefaultMarker(
     "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
@@ -105,8 +104,7 @@ MarkerArray showAllValidLaneChangePath(const std::vector<LaneChangePath> & lanes
   return marker_array;
 }
 
-MarkerArray showLerpedPose(
-  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns)
+MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
 {
   MarkerArray marker_array;
   int32_t id{0};
@@ -128,46 +126,7 @@ MarkerArray showLerpedPose(
   return marker_array;
 }
 
-MarkerArray showEgoPredictedPaths(
-  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns)
-{
-  if (obj_debug_vec.empty()) {
-    return MarkerArray{};
-  }
-
-  MarkerArray marker_array;
-  constexpr auto colors = colorsList();
-  constexpr float scale_val = 0.2;
-  const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
-  marker_array.markers.reserve(obj_debug_vec.size());
-
-  int32_t id{0};
-  for (const auto & [uuid, info] : obj_debug_vec) {
-    const auto loop_size = std::min(info.ego_predicted_path.size(), colors.size());
-
-    for (std::size_t idx = 0; idx < loop_size; ++idx) {
-      const auto & path = info.ego_predicted_path.at(idx).path;
-      const auto & color = colors.at(idx);
-
-      Marker marker = createDefaultMarker(
-        "map", current_time, ns, ++id, Marker::LINE_STRIP,
-        createMarkerScale(scale_val, scale_val, scale_val),
-        createMarkerColor(color[0], color[1], color[2], 0.9));
-
-      marker.points.reserve(path.size());
-
-      for (const auto & point : path) {
-        marker.points.push_back(point.position);
-      }
-
-      marker_array.markers.push_back(marker);
-    }
-  }
-  return marker_array;
-}
-
-MarkerArray showPolygon(
-  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns)
+MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
 {
   if (obj_debug_vec.empty()) {
     return MarkerArray{};
@@ -235,8 +194,7 @@ MarkerArray showPolygon(
   return marker_array;
 }
 
-MarkerArray showPolygonPose(
-  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns)
+MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
 {
   constexpr auto colors = colorsList();
   const auto loop_size = std::min(colors.size(), obj_debug_vec.size());

planning/behavior_path_planner/src/marker_utils/utils.cpp

@@ -14,6 +14,7 @@
 
 #include "behavior_path_planner/marker_utils/utils.hpp"
 
+#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
 
@@ -23,6 +24,7 @@ namespace marker_utils
 {
 using behavior_path_planner::ShiftLine;
 using behavior_path_planner::utils::calcPathArcLengthArray;
+using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
 using std_msgs::msg::ColorRGBA;
 using tier4_autoware_utils::calcOffsetPose;
 using tier4_autoware_utils::createDefaultMarker;
@@ -32,6 +34,27 @@ using tier4_autoware_utils::createMarkerScale;
 using tier4_autoware_utils::createPoint;
 using visualization_msgs::msg::Marker;
 
+CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj)
+{
+  CollisionCheckDebug debug;
+  debug.current_pose = obj.initial_pose.pose;
+  debug.current_twist = obj.initial_twist.twist;
+  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);
+}
+
 MarkerArray createPoseMarkerArray(
   const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g,
   const float & b)

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

@@ -45,7 +45,7 @@
 namespace behavior_path_planner
 {
 
-using marker_utils::CollisionCheckDebug;
+using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
 using motion_utils::calcLongitudinalOffsetPose;
 using motion_utils::calcSignedArcLength;
 using motion_utils::findNearestIndex;