From 3ca00b2533b3d5d92bb6a93bbab15e55258d2e74 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 30 Nov 2023 16:47:03 +0900 Subject: [PATCH] feat(lane_change): remove debug message (#5638) Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner_node.hpp | 4 --- .../lane_change/avoidance_by_lane_change.hpp | 3 -- .../scene_module/lane_change/base_class.hpp | 9 ------ .../scene_module/lane_change/interface.hpp | 18 ++++------- .../scene_module/lane_change/normal.hpp | 5 --- .../scene_module/scene_module_visitor.hpp | 19 ----------- .../src/behavior_path_planner_node.cpp | 7 ---- .../scene_module/lane_change/interface.cpp | 32 ------------------- .../src/scene_module/scene_module_visitor.cpp | 5 --- 9 files changed, 6 insertions(+), 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 index 1a9b23be00a3f..ccda2ff35e682 100644 --- 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 #include #include @@ -35,7 +34,6 @@ #include #include #include -#include #include #include #include @@ -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::SharedPtr debug_maximum_drivable_area_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; - rclcpp::Publisher::SharedPtr debug_lane_change_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_turn_signal_info_publisher_; /** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp index 4b548c86e55bb..67fd2c1ba0c77 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp +++ b/planning/behavior_path_planner/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 -#include 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/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 03a512a23989d..53c42deca6215 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_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 #include -#include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp" #include #include #include -#include -#include #include #include #include -#include 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/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index dad46494bc350..d2892691126eb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_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 -#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 #include #include @@ -40,16 +36,14 @@ #include #include #include -#include -#include 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 get_debug_msg_array() const; - - void acceptVisitor(const std::shared_ptr & visitor) const override; + void acceptVisitor( + [[maybe_unused]] const std::shared_ptr & 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 prev_approved_path_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index e82c7f493ae9d..61caf7b2b393f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/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 -#include -#include #include #include @@ -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/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp index 03672aa59f6b9..08779e8baf757 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp @@ -15,12 +15,7 @@ #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 namespace behavior_path_planner @@ -28,37 +23,23 @@ 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 getAvoidanceModuleDebugMsg() const; - std::shared_ptr getLaneChangeModuleDebugMsg() const; protected: - mutable std::shared_ptr lane_change_visitor_; - mutable std::shared_ptr ext_request_lane_change_visitor_; - mutable std::shared_ptr external_request_lane_change_bt_visitor_; mutable std::shared_ptr 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 index aa3dcb37eb76f..d1069e789edd2 100644 --- 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("~/output/is_reroute_available", 1); debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); - debug_lane_change_msg_array_publisher_ = - create_publisher("~/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 index 1dddad3433668..f84eaa39844dc 100644 --- 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 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(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 & 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 & 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 index 42c8ae15919b1..6831e4b6b32ee 100644 --- 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 SceneModuleVisitor::getAvoidanceModuleDe { return avoidance_visitor_; } - -std::shared_ptr SceneModuleVisitor::getLaneChangeModuleDebugMsg() const -{ - return lane_change_visitor_; -} } // namespace behavior_path_planner