From e668dfc246c6461cabcfffbf51c07950f56e4382 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 29 Aug 2022 13:47:57 +0900 Subject: [PATCH] feat(behavior_path_planner): output debug marker from each module (#1711) * feat(behavior_path_planner): output debug marker from each module Signed-off-by: satoshi-ota * fix(behavior_path_planner): remove unused publisher Signed-off-by: satoshi-ota * fix(behavior_path_planner): remove unused interface function Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota --- .../behavior_path_planner_node.hpp | 2 -- .../behavior_tree_manager.hpp | 1 - .../scene_module/scene_module_interface.hpp | 14 +++++++++++--- .../src/behavior_path_planner_node.cpp | 13 ------------- .../src/behavior_tree_manager.cpp | 10 +--------- 5 files changed, 12 insertions(+), 28 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 418d169a70377..b06397c385f2f 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 @@ -148,8 +148,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node // debug rclcpp::Publisher::SharedPtr debug_drivable_area_lanelets_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; - rclcpp::Publisher::SharedPtr debug_marker_publisher_; - void publishDebugMarker(const std::vector & debug_markers); }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp index 36dcdd01a0c27..31a47496e6da6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp @@ -49,7 +49,6 @@ class BehaviorTreeManager BehaviorModuleOutput run(const std::shared_ptr & data); std::vector> getModulesStatus(); - std::vector getDebugMarkers(); AvoidanceDebugMsgArray getAvoidanceDebugMsgArray(); private: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index ea8931a7e9efc..9be0be6ee6746 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -30,6 +30,7 @@ #include +#include #include #include #include @@ -97,6 +98,12 @@ class SceneModuleInterface is_waiting_approval_{false}, current_state_{BT::NodeStatus::IDLE} { + std::string module_ns; + module_ns.resize(name.size()); + std::transform(name.begin(), name.end(), module_ns.begin(), tolower); + + const auto ns = std::string("~/debug/") + module_ns; + pub_debug_marker_ = node.create_publisher(ns, 20); } virtual ~SceneModuleInterface() = default; @@ -212,14 +219,14 @@ class SceneModuleInterface */ void setData(const std::shared_ptr & data) { planner_data_ = data; } + void publishDebugMarker() { pub_debug_marker_->publish(debug_marker_); } + std::string name() const { return name_; } rclcpp::Logger getLogger() const { return logger_; } std::shared_ptr planner_data_; - MarkerArray getDebugMarker() { return debug_marker_; } - AvoidanceDebugMsgArray::SharedPtr getAvoidanceDebugMsgArray() { if (debug_avoidance_msg_array_ptr_) { @@ -234,9 +241,10 @@ class SceneModuleInterface rclcpp::Logger logger_; protected: - MarkerArray debug_marker_; rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr pub_debug_marker_; mutable AvoidanceDebugMsgArray::SharedPtr debug_avoidance_msg_array_ptr_{}; + mutable MarkerArray debug_marker_; std::shared_ptr rtc_interface_ptr_; UUID uuid_; 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 3f678b315a1de..3386f515a44e5 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -68,9 +68,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); - // Debug - debug_marker_publisher_ = create_publisher("~/debug/markers", 1); - if (planner_data_->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { debug_drivable_area_lanelets_publisher_ = create_publisher("~/drivable_area_boundary", 1); @@ -596,7 +593,6 @@ void BehaviorPathPlannerNode::run() // for debug debug_avoidance_msg_array_publisher_->publish(bt_manager_->getAvoidanceDebugMsgArray()); - publishDebugMarker(bt_manager_->getDebugMarkers()); if (planner_data->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { const auto drivable_area_lines = marker_utils::createFurthestLineStringMarkerArray( @@ -652,15 +648,6 @@ bool BehaviorPathPlannerNode::skipSmoothGoalConnection( return false; } -void BehaviorPathPlannerNode::publishDebugMarker(const std::vector & debug_markers) -{ - MarkerArray msg{}; - for (const auto & markers : debug_markers) { - tier4_autoware_utils::appendMarkerArray(markers, &msg); - } - debug_marker_publisher_->publish(msg); -} - void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg) { std::lock_guard lock(mutex_pd_); diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp index 2f63c708b7b48..76bee98960016 100644 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp @@ -98,6 +98,7 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr RCLCPP_DEBUG(logger_, "BehaviorPathPlanner::run end status = %s", BT::toStr(res).c_str()); std::for_each(scene_modules_.begin(), scene_modules_.end(), [](const auto & m) { + m->publishDebugMarker(); if (!m->isExecutionRequested()) { m->onExit(); } @@ -112,15 +113,6 @@ BehaviorTreeManager::getModulesStatus() return modules_status_; } -std::vector BehaviorTreeManager::getDebugMarkers() -{ - std::vector data; - for (const auto & module : scene_modules_) { - data.push_back(module->getDebugMarker()); - } - return data; -} - AvoidanceDebugMsgArray BehaviorTreeManager::getAvoidanceDebugMsgArray() { const auto avoidance_module = std::find_if(