feat(behavior_path_planner): output debug marker from each module (au…

…towarefoundation#1711) * feat(behavior_path_planner): output debug marker from each module Signed-off-by: satoshi-ota <[email protected]> * fix(behavior_path_planner): remove unused publisher Signed-off-by: satoshi-ota <[email protected]> * fix(behavior_path_planner): remove unused interface function Signed-off-by: satoshi-ota <[email protected]> Signed-off-by: satoshi-ota <[email protected]>
YoheiMishina · Aug 29, 2022 · e668dfc · e668dfc
1 parent 231b7ed
commit e668dfc
Show file tree

Hide file tree

Showing 5 changed files with 12 additions and 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
@@ -148,8 +148,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   // debug
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_drivable_area_lanelets_publisher_;
   rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
-  rclcpp::Publisher<MarkerArray>::SharedPtr debug_marker_publisher_;
-  void publishDebugMarker(const std::vector<MarkerArray> & 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
@@ -49,7 +49,6 @@ class BehaviorTreeManager
 
   BehaviorModuleOutput run(const std::shared_ptr<PlannerData> & data);
   std::vector<std::shared_ptr<SceneModuleStatus>> getModulesStatus();
-  std::vector<MarkerArray> getDebugMarkers();
   AvoidanceDebugMsgArray getAvoidanceDebugMsgArray();
 
 private:

diff --git a/...havior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/...havior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp
@@ -30,6 +30,7 @@
 
 #include <behaviortree_cpp_v3/basic_types.h>
 
+#include <algorithm>
 #include <limits>
 #include <memory>
 #include <random>
@@ -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<MarkerArray>(ns, 20);
   }
 
   virtual ~SceneModuleInterface() = default;
@@ -212,14 +219,14 @@ class SceneModuleInterface
    */
   void setData(const std::shared_ptr<const PlannerData> & 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<const PlannerData> 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<MarkerArray>::SharedPtr pub_debug_marker_;
   mutable AvoidanceDebugMsgArray::SharedPtr debug_avoidance_msg_array_ptr_{};
+  mutable MarkerArray debug_marker_;
 
   std::shared_ptr<RTCInterface> 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
@@ -68,9 +68,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   debug_avoidance_msg_array_publisher_ =
     create_publisher<AvoidanceDebugMsgArray>("~/debug/avoidance_debug_message_array", 1);
 
-  // Debug
-  debug_marker_publisher_ = create_publisher<MarkerArray>("~/debug/markers", 1);
-
   if (planner_data_->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) {
     debug_drivable_area_lanelets_publisher_ =
       create_publisher<MarkerArray>("~/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<MarkerArray> & 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<std::mutex> 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
@@ -98,6 +98,7 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr<PlannerData>
   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<MarkerArray> BehaviorTreeManager::getDebugMarkers()
-{
-  std::vector<MarkerArray> data;
-  for (const auto & module : scene_modules_) {
-    data.push_back(module->getDebugMarker());
-  }
-  return data;
-}
-
 AvoidanceDebugMsgArray BehaviorTreeManager::getAvoidanceDebugMsgArray()
 {
   const auto avoidance_module = std::find_if(