diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index ba3d3d5d7a4d4..d88c93712b1e3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -17,6 +17,7 @@ #include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "planner_manager.hpp" @@ -72,6 +73,7 @@ using tier4_planning_msgs::msg::RerouteAvailability; using tier4_planning_msgs::msg::Scenario; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; +using DebugPublisher = autoware::universe_utils::DebugPublisher; class BehaviorPathPlannerNode : public rclcpp::Node { @@ -186,6 +188,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node // debug rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_turn_signal_info_publisher_; + std::unique_ptr debug_start_planner_evaluation_table_publisher_ptr_; /** * @brief publish reroute availability diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 568d432e1faa3..38c5aa0085fb9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -30,6 +31,7 @@ namespace autoware::behavior_path_planner { using autoware::vehicle_info_utils::VehicleInfoUtils; using tier4_planning_msgs::msg::PathChangeModuleId; +using DebugStringMsg = autoware_internal_debug_msgs::msg::StringStamped; BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options) : Node("behavior_path_planner", node_options), @@ -55,9 +57,13 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod create_publisher("~/output/modified_goal", durable_qos); reroute_availability_publisher_ = create_publisher("~/output/is_reroute_available", 1); + debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); + debug_start_planner_evaluation_table_publisher_ptr_ = + std::make_unique(this, "~/debug/start_planner_evaluation_table"); + debug_turn_signal_info_publisher_ = create_publisher("~/debug/turn_signal_info", 1); bound_publisher_ = create_publisher("~/debug/bound", 1); @@ -608,6 +614,11 @@ void BehaviorPathPlannerNode::publishSceneModuleDebugMsg( if (avoidance_debug_message) { debug_avoidance_msg_array_publisher_->publish(*avoidance_debug_message); } + const auto start_planner_debug_message = debug_messages_data_ptr->getStartPlannerModuleDebugMsg(); + if (start_planner_debug_message) { + debug_start_planner_evaluation_table_publisher_ptr_->publish( + "start_planner_evaluation_table", *(start_planner_debug_message)); + } } void BehaviorPathPlannerNode::publishPathCandidate( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp index e2d5d276070ee..f3921e4778593 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ #include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" +#include #include namespace autoware::behavior_path_planner @@ -31,16 +32,20 @@ class SideShiftModule; using tier4_planning_msgs::msg::AvoidanceDebugMsg; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; +using DebugStringMsg = autoware_internal_debug_msgs::msg::StringStamped; class SceneModuleVisitor { public: void visitAvoidanceModule(const StaticObstacleAvoidanceModule * module) const; + void visitStartPlannerModule(const StartPlannerModule * module) const; std::shared_ptr getAvoidanceModuleDebugMsg() const; + std::shared_ptr getStartPlannerModuleDebugMsg() const; protected: mutable std::shared_ptr avoidance_visitor_; + mutable std::shared_ptr start_planner_visitor_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp index e6ccfc74866c4..45c4f17c32698 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp @@ -24,4 +24,10 @@ std::shared_ptr SceneModuleVisitor::getAvoidanceModuleDe { return avoidance_visitor_; } + +std::shared_ptr SceneModuleVisitor::getStartPlannerModuleDebugMsg() const +{ + return start_planner_visitor_; +} + } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index 5d8331318484d..4a2ca54e7d38c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -26,7 +26,6 @@ #include #include - namespace autoware::behavior_path_planner { @@ -51,29 +50,34 @@ struct PlannerDebugData { public: PlannerType planner_type; - std::vector conditions_evaluation; - double required_margin{0.0}; double backward_distance{0.0}; + double required_margin{0.0}; + std::vector conditions_evaluation; - auto header_str() const + static std::string double_to_str(double value, int precision = 1) { - std::stringstream ss; - ss << std::left << std::setw(20) << "| Planner type " << std::setw(20) << "| Required margin " - << std::setw(20) << "| Backward distance " << std::setw(25) << "| Condition evaluation |" - << "\n"; - return ss.str(); + std::ostringstream oss; + oss << std::fixed << std::setprecision(precision) << value; + return oss.str(); } - auto str() const + static std::string to_planner_type_name(PlannerType pt) { - std::stringstream ss; - for (const auto & result : conditions_evaluation) { - ss << std::left << std::setw(23) << magic_enum::enum_name(planner_type) << std::setw(23) - << (std::to_string(required_margin) + "[m]") << std::setw(23) - << (std::to_string(backward_distance) + "[m]") << std::setw(25) << result << "\n"; + // Adding whitespace for column width alignment in RViz display + switch (pt) { + case PlannerType::NONE: + return "NONE "; + case PlannerType::SHIFT: + return "SHIFT "; + case PlannerType::GEOMETRIC: + return "GEOMETRIC "; + case PlannerType::STOP: + return "STOP "; + case PlannerType::FREESPACE: + return "FREESPACE "; + default: + return "UNKNOWN"; } - ss << std::setw(40); - return ss.str(); } }; struct StartPlannerDebugData diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index 9b1186cb5fc66..3978738798386 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -26,10 +26,12 @@ #include "autoware/behavior_path_start_planner_module/geometric_pull_out.hpp" #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" #include "autoware/behavior_path_start_planner_module/shift_pull_out.hpp" +#include "data_structs.hpp" #include #include +#include #include #include @@ -52,6 +54,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckPa using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using geometry_msgs::msg::PoseArray; using PriorityOrder = std::vector>>; +using DebugStringMsg = autoware_internal_debug_msgs::msg::StringStamped; struct PullOutStatus { @@ -146,16 +149,16 @@ class StartPlannerModule : public SceneModuleInterface } void resetStatus(); - void acceptVisitor( - [[maybe_unused]] const std::shared_ptr & visitor) const override - { - } + void acceptVisitor(const std::shared_ptr & visitor) const override; // Condition to disable simultaneous execution bool isDrivingForward() const { return status_.driving_forward; } bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } + std::string get_planner_evaluation_table() const { return planner_evaluation_table_; } + private: + friend class SceneModuleVisitor; struct StartPlannerData { StartPlannerParameters parameters; @@ -283,6 +286,7 @@ ego pose. std::vector> start_planners_; PullOutStatus status_; mutable StartPlannerDebugData debug_data_; + std::string planner_evaluation_table_; // Keeps track of lanelets that should be ignored when calculating the turnSignalInfo for this // module's output. If the ego vehicle is in this lanelet, the calculation is skipped. @@ -352,6 +356,10 @@ ego pose. const StartPlannerParameters & parameters, const std::shared_ptr & planner_data, const PullOutStatus & pull_out_status); + std::string create_planner_evaluation_table( + const std::vector & planner_debug_data_vector) const; + void set_planner_evaluation_table(const std::vector & debug_data_vector); + void setDebugData(); void logPullOutStatus(rclcpp::Logger::Level log_level = rclcpp::Logger::Level::Info) const; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index ae05bcf4c084e..5ad519af20cad 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -894,16 +894,6 @@ void StartPlannerModule::planWithPriority( if (start_pose_candidates.empty()) return; - auto get_accumulated_debug_stream = [](const std::vector & debug_data_vector) { - std::stringstream ss; - if (debug_data_vector.empty()) return ss; - ss << debug_data_vector.front().header_str(); - for (const auto & debug_data : debug_data_vector) { - ss << debug_data.str(); - } - return ss; - }; - const PriorityOrder order_priority = determinePriorityOrder(search_priority, start_pose_candidates.size()); @@ -918,20 +908,13 @@ void StartPlannerModule::planWithPriority( collision_check_margin, debug_data_vector)) { debug_data_.selected_start_pose_candidate_index = index; debug_data_.margin_for_start_pose_candidate = collision_check_margin; - if (parameters_->print_debug_info) { - const auto ss = get_accumulated_debug_stream(debug_data_vector); - DEBUG_PRINT("\nPull out path search results:\n%s", ss.str().c_str()); - } + set_planner_evaluation_table(debug_data_vector); return; } } } } - - if (parameters_->print_debug_info) { - const auto ss = get_accumulated_debug_stream(debug_data_vector); - DEBUG_PRINT("\nPull out path search results:\n%s", ss.str().c_str()); - } + set_planner_evaluation_table(debug_data_vector); updateStatusIfNoSafePathFound(); } @@ -973,7 +956,7 @@ bool StartPlannerModule::findPullOutPath( planner->setCollisionCheckMargin(collision_check_margin); PlannerDebugData debug_data{ - planner->getPlannerType(), {}, collision_check_margin, backwards_distance}; + planner->getPlannerType(), backwards_distance, collision_check_margin, {}}; const auto pull_out_path = planner->plan(start_pose_candidate, goal_pose, planner_data_, debug_data); @@ -1580,12 +1563,9 @@ std::optional StartPlannerModule::planFreespacePath( for (const auto & p : center_line_path.points) { const Pose end_pose = p.point.pose; - PlannerDebugData debug_data{freespace_planner_->getPlannerType(), {}, 0.0, 0.0}; + PlannerDebugData debug_data{freespace_planner_->getPlannerType(), 0.0, 0.0, {}}; auto freespace_path = freespace_planner_->plan(current_pose, end_pose, planner_data, debug_data); - DEBUG_PRINT( - "\nFreespace Pull out path search results\n%s%s", debug_data.header_str().c_str(), - debug_data.str().c_str()); if (!freespace_path) { continue; } @@ -1628,6 +1608,83 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } +std::string StartPlannerModule::create_planner_evaluation_table( + const std::vector & planner_debug_data_vector) const +{ + if (planner_debug_data_vector.empty()) { + return ""; + } + + const std::string header_planner_type = "Planner type "; + const std::string header_required_margin = "Required margin [m]"; + const std::string header_backward_distance = "Backward distance [m]"; + const std::string header_condition_eval = "Condition"; + + std::ostringstream oss; + oss << "-----------------------------------------------------------------------------------------" + "----------------" + << "\n"; + oss << "| " << std::left << header_planner_type << " | " << header_required_margin << " | " + << header_backward_distance << " | " << header_condition_eval << " \n"; + oss << "-----------------------------------------------------------------------------------------" + "----------------" + << "\n"; + + for (const auto & d : planner_debug_data_vector) { + const std::string pt_str = PlannerDebugData::to_planner_type_name(d.planner_type); + const std::string rm_str = + PlannerDebugData::double_to_str(d.required_margin, 1) + " "; + const std::string bd_str = PlannerDebugData::double_to_str(d.backward_distance, 1) + + " "; + + if (d.conditions_evaluation.empty()) { + oss << "| " << std::left << pt_str << " | " << rm_str << " | " << bd_str << " | " + << "Unexpected empty condition evaluation" + << " \n"; + } else { + for (size_t i = 0; i < d.conditions_evaluation.size(); ++i) { + const std::string cond_with_index = + "#" + std::to_string(i + 1) + ": " + d.conditions_evaluation[i]; + + oss << "| " << std::left << pt_str << " | " << rm_str << " | " << bd_str << " | " + << cond_with_index << " \n"; + } + } + } + + return oss.str(); +} + +void StartPlannerModule::set_planner_evaluation_table( + const std::vector & debug_data_vector) +{ + planner_evaluation_table_.clear(); + if (debug_data_vector.empty()) { + return; + } + + planner_evaluation_table_ = create_planner_evaluation_table(debug_data_vector); +} + +void StartPlannerModule::acceptVisitor(const std::shared_ptr & visitor) const +{ + if (visitor) { + visitor->visitStartPlannerModule(this); + } +} + +void SceneModuleVisitor::visitStartPlannerModule(const StartPlannerModule * module) const +{ + auto debug_msg = std::make_shared(); + auto debug_info = module->get_planner_evaluation_table(); + if (debug_info.empty()) return; + + debug_msg->stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); + debug_msg->data = debug_info; + + start_planner_visitor_ = debug_msg; +} + void StartPlannerModule::setDebugData() { using autoware::universe_utils::createDefaultMarker;