feat(lane_change): remove debug message (autowarefoundation#5638)

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
tier4 · Dec 15, 2023 · cb9aa5f · cb9aa5f
1 parent bf11a68
commit cb9aa5f
Show file tree

Hide file tree

Showing 9 changed files with 6 additions and 96 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/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_;
 
   /**

diff --git a/...anner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp b/...anner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp
@@ -18,15 +18,12 @@
 #include "behavior_path_planner/scene_module/lane_change/normal.hpp"
 
 #include <memory>
-#include <utility>
 
 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

diff --git a/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
@@ -14,8 +14,6 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_
 
-#include "behavior_path_planner/marker_utils/lane_change/debug.hpp"
-#include "behavior_path_planner/marker_utils/utils.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
 #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
@@ -27,18 +25,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 +43,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
 {

diff --git a/...ehavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/...ehavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp
@@ -15,7 +15,6 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_
 
-#include "behavior_path_planner/marker_utils/lane_change/debug.hpp"
 #include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp"
 #include "behavior_path_planner/scene_module/lane_change/base_class.hpp"
 #include "behavior_path_planner/scene_module/lane_change/external_request.hpp"
@@ -28,9 +27,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>
@@ -40,16 +36,14 @@
 #include <memory>
 #include <string>
 #include <unordered_map>
-#include <utility>
-#include <vector>
 
 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 +80,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 +136,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_;
 

diff --git a/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
@@ -14,12 +14,9 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_
 
-#include "behavior_path_planner/marker_utils/utils.hpp"
 #include "behavior_path_planner/scene_module/lane_change/base_class.hpp"
 
 #include <memory>
-#include <string>
-#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -36,8 +33,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
 {

diff --git a/...behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp b/...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

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/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(

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
@@ -328,27 +328,6 @@ void LaneChangeInterface::setObjectDebugVisualization() const
   }
 }
 
-std::shared_ptr<LaneChangeDebugMsgArray> LaneChangeInterface::get_debug_msg_array() const
-{
-  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);
-  }
-  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_);
-}
-
 MarkerArray LaneChangeInterface::getModuleVirtualWall()
 {
   using marker_utils::lane_change_markers::createLaneChangingVirtualWallMarker;
@@ -415,12 +394,6 @@ void LaneChangeInterface::updateSteeringFactorPtr(
     {output.start_distance_to_path_change, output.finish_distance_to_path_change},
     PlanningBehavior::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)
@@ -502,11 +475,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,

diff --git a/planning/behavior_path_planner/src/scene_module/scene_module_visitor.cpp b/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