feat(behavior_path_planner): safety check msgs for debug

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

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp

@@ -17,6 +17,10 @@
 
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
+#include <boost/uuid/uuid.hpp>
+#include <boost/uuid/uuid_io.hpp>
+
+#include <algorithm>
 #include <random>
 #include <string>
 
@@ -40,6 +44,20 @@ inline std::string toHexString(const unique_identifier_msgs::msg::UUID & id)
   }
   return ss.str();
 }
+
+inline boost::uuids::uuid rosToBoostUUID(const unique_identifier_msgs::msg::UUID & ros_uuid)
+{
+  boost::uuids::uuid boost_uuid{};
+  std::copy(ros_uuid.uuid.begin(), ros_uuid.uuid.end(), boost_uuid.begin());
+  return boost_uuid;
+}
+
+inline unique_identifier_msgs::msg::UUID boostToRosUUID(const boost::uuids::uuid & boost_uuid)
+{
+  unique_identifier_msgs::msg::UUID ros_uuid;
+  std::copy(boost_uuid.begin(), boost_uuid.end(), ros_uuid.uuid.begin());
+  return ros_uuid;
+}
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -21,7 +21,6 @@
 #include "behavior_path_planner/steering_factor_interface.hpp"
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
 
-#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp"
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
@@ -35,7 +34,6 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <tier4_planning_msgs/msg/approval.hpp>
 #include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
-#include <tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp>
 #include <tier4_planning_msgs/msg/path_change_module.hpp>
 #include <tier4_planning_msgs/msg/reroute_availability.hpp>
 #include <tier4_planning_msgs/msg/scenario.hpp>
@@ -66,7 +64,6 @@ using nav_msgs::msg::Odometry;
 using rcl_interfaces::msg::SetParametersResult;
 using steering_factor_interface::SteeringFactorInterface;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
-using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
 using tier4_planning_msgs::msg::LateralOffset;
 using tier4_planning_msgs::msg::RerouteAvailability;
 using tier4_planning_msgs::msg::Scenario;
@@ -171,7 +168,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   // debug
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_maximum_drivable_area_publisher_;
   rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
-  rclcpp::Publisher<LaneChangeDebugMsgArray>::SharedPtr debug_lane_change_msg_array_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_turn_signal_info_publisher_;
 
   /**

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

@@ -25,8 +25,6 @@ namespace behavior_path_planner
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
-using tier4_planning_msgs::msg::LaneChangeDebugMsg;
-using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
 using AvoidanceDebugData = DebugData;
 
 class AvoidanceByLaneChange : public NormalLaneChange

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

@@ -27,18 +27,13 @@
 #include <magic_enum.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp"
-#include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp"
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 
-#include <algorithm>
-#include <chrono>
 #include <memory>
 #include <string>
 #include <utility>
-#include <vector>
 
 namespace behavior_path_planner
 {
@@ -50,8 +45,6 @@ using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 using route_handler::Direction;
 using tier4_autoware_utils::StopWatch;
-using tier4_planning_msgs::msg::LaneChangeDebugMsg;
-using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
 
 class LaneChangeBase
 {

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

@@ -28,9 +28,6 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp"
-#include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp"
-#include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp"
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
@@ -48,8 +45,8 @@ namespace behavior_path_planner
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
-using tier4_planning_msgs::msg::LaneChangeDebugMsg;
-using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
+using objects_of_interest_marker_interface::ColorName;
+using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface;
 
 class LaneChangeInterface : public SceneModuleInterface
 {
@@ -86,9 +83,10 @@ class LaneChangeInterface : public SceneModuleInterface
 
   CandidateOutput planCandidate() const override;
 
-  std::shared_ptr<LaneChangeDebugMsgArray> get_debug_msg_array() const;
-
-  void acceptVisitor(const std::shared_ptr<SceneModuleVisitor> & visitor) const override;
+  void acceptVisitor(
+    [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override
+  {
+  }
 
   void updateModuleParams(const std::any & parameters) override;
 
@@ -141,7 +139,6 @@ class LaneChangeInterface : public SceneModuleInterface
     const CandidateOutput & output, const LaneChangePath & selected_path) const;
 
   mutable MarkerArray virtual_wall_marker_;
-  mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_;
 
   std::unique_ptr<PathWithLaneId> prev_approved_path_;
 

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

@@ -36,8 +36,6 @@ using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 using route_handler::Direction;
-using tier4_planning_msgs::msg::LaneChangeDebugMsg;
-using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
 
 class NormalLaneChange : public LaneChangeBase
 {

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp

@@ -15,50 +15,31 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_VISITOR_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_VISITOR_HPP_
 
-#include "tier4_planning_msgs/msg/avoidance_debug_msg.hpp"
-#include "tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp"
 #include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp"
-#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp"
-#include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp"
-#include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp"
 
 #include <memory>
 namespace behavior_path_planner
 {
 // Forward Declaration
 class AvoidanceModule;
 class AvoidanceByLCModule;
-class LaneChangeModule;
 class ExternalRequestLaneChangeModule;
 class LaneChangeInterface;
-class LaneChangeBTInterface;
-class LaneFollowingModule;
 class StartPlannerModule;
 class GoalPlannerModule;
 class SideShiftModule;
 
 using tier4_planning_msgs::msg::AvoidanceDebugMsg;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
-using tier4_planning_msgs::msg::LaneChangeDebugMsg;
-using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
 
 class SceneModuleVisitor
 {
 public:
-  void visitLaneChangeModule(const LaneChangeModule * module) const;
-  void visitExternalRequestLaneChangeModule(const ExternalRequestLaneChangeModule * module) const;
-  void visitLaneChangeInterface(const LaneChangeInterface * interface) const;
-  void visitLaneChangeBTInterface(const LaneChangeBTInterface * module) const;
   void visitAvoidanceModule(const AvoidanceModule * module) const;
-  void visitAvoidanceByLCModule(const AvoidanceByLCModule * module) const;
 
   std::shared_ptr<AvoidanceDebugMsgArray> getAvoidanceModuleDebugMsg() const;
-  std::shared_ptr<LaneChangeDebugMsgArray> getLaneChangeModuleDebugMsg() const;
 
 protected:
-  mutable std::shared_ptr<LaneChangeDebugMsgArray> lane_change_visitor_;
-  mutable std::shared_ptr<LaneChangeDebugMsgArray> ext_request_lane_change_visitor_;
-  mutable std::shared_ptr<LaneChangeDebugMsgArray> external_request_lane_change_bt_visitor_;
   mutable std::shared_ptr<AvoidanceDebugMsgArray> avoidance_visitor_;
 };
 }  // namespace behavior_path_planner

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

@@ -21,6 +21,9 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 
+#include <boost/uuid/uuid.hpp>
+#include <boost/uuid/uuid_hash.hpp>
+
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -214,7 +217,7 @@ struct CollisionCheckDebug
   Polygon2d extended_obj_polygon{};  ///< Detected object's extended collision polygon.
   autoware_auto_perception_msgs::msg::Shape obj_shape;  ///< object's shape.
 };
-using CollisionCheckDebugPair = std::pair<std::string, CollisionCheckDebug>;
+using CollisionCheckDebugPair = std::pair<boost::uuids::uuid, CollisionCheckDebug>;
 using CollisionCheckDebugMap =
   std::unordered_map<CollisionCheckDebugPair::first_type, CollisionCheckDebugPair::second_type>;
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -79,8 +79,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     create_publisher<RerouteAvailability>("~/output/is_reroute_available", 1);
   debug_avoidance_msg_array_publisher_ =
     create_publisher<AvoidanceDebugMsgArray>("~/debug/avoidance_debug_message_array", 1);
-  debug_lane_change_msg_array_publisher_ =
-    create_publisher<LaneChangeDebugMsgArray>("~/debug/lane_change_debug_message_array", 1);
 
   if (planner_data_->parameters.visualize_maximum_drivable_area) {
     debug_maximum_drivable_area_publisher_ =
@@ -785,11 +783,6 @@ void BehaviorPathPlannerNode::publishSceneModuleDebugMsg(
   if (avoidance_debug_message) {
     debug_avoidance_msg_array_publisher_->publish(*avoidance_debug_message);
   }
-
-  const auto lane_change_debug_message = debug_messages_data_ptr->getLaneChangeModuleDebugMsg();
-  if (lane_change_debug_message) {
-    debug_lane_change_msg_array_publisher_->publish(*lane_change_debug_message);
-  }
 }
 
 void BehaviorPathPlannerNode::publishPathCandidate(

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

@@ -20,7 +20,12 @@
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 #include "behavior_path_planner/utils/lane_change/utils.hpp"
 
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
+
+#include <geometry_msgs/msg/detail/point32__struct.hpp>
+#include <geometry_msgs/msg/detail/polygon__struct.hpp>
 
 #include <algorithm>
 #include <memory>
@@ -320,25 +325,16 @@ void LaneChangeInterface::setObjectDebugVisualization() const
   }
 }
 
-std::shared_ptr<LaneChangeDebugMsgArray> LaneChangeInterface::get_debug_msg_array() const
+void LaneChangeInterface::setObjectsOfInterestData(const bool is_approved)
 {
-  const auto debug_data = module_type_->getDebugData();
-  LaneChangeDebugMsgArray debug_msg_array;
-  debug_msg_array.lane_change_info.reserve(debug_data.size());
-  for (const auto & [uuid, debug_data] : debug_data) {
-    LaneChangeDebugMsg debug_msg;
-    debug_msg.object_id = uuid;
-    debug_msg.allow_lane_change = debug_data.is_safe;
-    debug_msg.is_front = debug_data.is_front;
-    debug_msg.failed_reason = debug_data.unsafe_reason;
-    debug_msg.velocity =
-      std::hypot(debug_data.object_twist.linear.x, debug_data.object_twist.linear.y);
-    debug_msg_array.lane_change_info.push_back(debug_msg);
+  const auto debug_data =
+    is_approved ? module_type_->getAfterApprovalDebugData() : module_type_->getDebugData();
+  for (const auto & [uuid, data] : debug_data) {
+    const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED;
+    objects_of_interest_marker_interface_.insertObjectData(
+      data.current_obj_pose, data.obj_shape, color);
   }
-  lane_change_debug_msg_array_ = debug_msg_array;
-
-  lane_change_debug_msg_array_.header.stamp = clock_->now();
-  return std::make_shared<LaneChangeDebugMsgArray>(lane_change_debug_msg_array_);
+  return;
 }
 
 MarkerArray LaneChangeInterface::getModuleVirtualWall()
@@ -407,12 +403,6 @@ void LaneChangeInterface::updateSteeringFactorPtr(
     {output.start_distance_to_path_change, output.finish_distance_to_path_change},
     SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, "");
 }
-void LaneChangeInterface::acceptVisitor(const std::shared_ptr<SceneModuleVisitor> & visitor) const
-{
-  if (visitor) {
-    visitor->visitLaneChangeInterface(this);
-  }
-}
 
 TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo(
   const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info)
@@ -494,11 +484,6 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo(
   return original_turn_signal_info;
 }
 
-void SceneModuleVisitor::visitLaneChangeInterface(const LaneChangeInterface * interface) const
-{
-  lane_change_visitor_ = interface->get_debug_msg_array();
-}
-
 AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface(
   const std::string & name, rclcpp::Node & node,
   const std::shared_ptr<LaneChangeParameters> & parameters,

planning/behavior_path_planner/src/scene_module/scene_module_visitor.cpp

@@ -22,9 +22,4 @@ std::shared_ptr<AvoidanceDebugMsgArray> SceneModuleVisitor::getAvoidanceModuleDe
 {
   return avoidance_visitor_;
 }
-
-std::shared_ptr<LaneChangeDebugMsgArray> SceneModuleVisitor::getLaneChangeModuleDebugMsg() const
-{
-  return lane_change_visitor_;
-}
 }  // namespace behavior_path_planner

planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp

@@ -568,7 +568,7 @@ CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj)
   debug.extended_obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape);
   debug.obj_shape = obj.shape;
   debug.current_twist = obj.initial_twist.twist;
-  return {tier4_autoware_utils::toHexString(obj.uuid), debug};
+  return {tier4_autoware_utils::rosToBoostUUID(obj.uuid), debug};
 }
 
 void updateCollisionCheckDebugMap(