feat(behavior): output important marker

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Apr 28, 2023 · 94e8403 · 94e8403
1 parent 24bce1e
commit 94e8403
Show file tree

Hide file tree

Showing 5 changed files with 35 additions and 10 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
@@ -97,12 +97,12 @@ class PlannerManager
   }
 
   /**
-   * @brief publish all registered modules' debug markers.
+   * @brief publish all registered modules' markers.
    */
-  void publishDebugMarker() const
+  void publishMarker() const
   {
     std::for_each(
-      manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->publishDebugMarker(); });
+      manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->publishMarker(); });
   }
 
   /**

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
@@ -79,6 +79,11 @@ class SceneModuleInterface
       pub_debug_marker_ = node.create_publisher<MarkerArray>(ns, 20);
     }
 
+    {
+      const auto ns = std::string("~/info/") + utils::convertToSnakeCase(name);
+      pub_info_marker_ = node.create_publisher<MarkerArray>(ns, 20);
+    }
+
     {
       const auto ns = std::string("~/virtual_wall/") + utils::convertToSnakeCase(name);
       pub_virtual_wall_ = node.create_publisher<MarkerArray>(ns, 20);
@@ -277,6 +282,8 @@ class SceneModuleInterface
   virtual void setData(const std::shared_ptr<const PlannerData> & data) { planner_data_ = data; }
 
 #ifdef USE_OLD_ARCHITECTURE
+  void publishInfoMarker() { pub_info_marker_->publish(info_marker_); }
+
   void publishDebugMarker() { pub_debug_marker_->publish(debug_marker_); }
 
   void publishVirtualWall()
@@ -322,7 +329,9 @@ class SceneModuleInterface
 
   PlanResult getPathReference() const { return path_reference_; }
 
-  MarkerArray getDebugMarkers() { return debug_marker_; }
+  MarkerArray getInfoMarkers() const { return info_marker_; }
+
+  MarkerArray getDebugMarkers() const { return debug_marker_; }
 
   ModuleStatus getCurrentStatus() const { return current_state_; }
 
@@ -375,6 +384,7 @@ class SceneModuleInterface
   rclcpp::Logger logger_;
 
 #ifdef USE_OLD_ARCHITECTURE
+  rclcpp::Publisher<MarkerArray>::SharedPtr pub_info_marker_;
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_debug_marker_;
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_virtual_wall_;
 #endif
@@ -477,6 +487,8 @@ class SceneModuleInterface
 
   mutable boost::optional<Pose> dead_pose_{boost::none};
 
+  mutable MarkerArray info_marker_;
+
   mutable MarkerArray debug_marker_;
 };
 

diff --git a/...ath_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/...ath_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp
@@ -64,6 +64,7 @@ class SceneModuleManagerInterface
         rtc_type, std::make_shared<RTCInterface>(node, rtc_interface_name));
     }
 
+    pub_info_marker_ = node->create_publisher<MarkerArray>("~/info/" + name, 20);
     pub_debug_marker_ = node->create_publisher<MarkerArray>("~/debug/" + name, 20);
     pub_virtual_wall_ = node->create_publisher<MarkerArray>("~/virtual_wall/" + name, 20);
   }
@@ -153,28 +154,37 @@ class SceneModuleManagerInterface
     pub_virtual_wall_->publish(markers);
   }
 
-  void publishDebugMarker() const
+  void publishMarker() const
   {
     using tier4_autoware_utils::appendMarkerArray;
 
-    MarkerArray markers{};
+    MarkerArray info_markers{};
+    MarkerArray debug_markers{};
 
     const auto marker_offset = std::numeric_limits<uint8_t>::max();
 
     uint32_t marker_id = marker_offset;
     for (const auto & m : registered_modules_) {
+      for (auto & marker : m->getInfoMarkers().markers) {
+        marker.id += marker_id;
+        info_markers.markers.push_back(marker);
+      }
+
       for (auto & marker : m->getDebugMarkers().markers) {
         marker.id += marker_id;
-        markers.markers.push_back(marker);
+        debug_markers.markers.push_back(marker);
       }
+
       marker_id += marker_offset;
     }
 
     if (registered_modules_.empty() && idling_module_ != nullptr) {
-      appendMarkerArray(idling_module_->getDebugMarkers(), &markers);
+      appendMarkerArray(idling_module_->getInfoMarkers(), &info_markers);
+      appendMarkerArray(idling_module_->getDebugMarkers(), &debug_markers);
     }
 
-    pub_debug_marker_->publish(markers);
+    pub_info_marker_->publish(info_markers);
+    pub_debug_marker_->publish(debug_markers);
   }
 
   bool exist(const SceneModulePtr & module_ptr) const
@@ -229,6 +239,8 @@ class SceneModuleManagerInterface
 
   rclcpp::Logger logger_;
 
+  rclcpp::Publisher<MarkerArray>::SharedPtr pub_info_marker_;
+
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_debug_marker_;
 
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_virtual_wall_;

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -1162,7 +1162,7 @@ void BehaviorPathPlannerNode::run()
 
 #ifndef USE_OLD_ARCHITECTURE
   planner_manager_->print();
-  planner_manager_->publishDebugMarker();
+  planner_manager_->publishMarker();
   planner_manager_->publishVirtualWall();
   lk_manager.unlock();  // release planner_manager_
 #endif

diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp
@@ -101,6 +101,7 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr<PlannerData>
   resetNotRunningModulePathCandidate();
 
   std::for_each(scene_modules_.begin(), scene_modules_.end(), [](const auto & m) {
+    m->publishInfoMarker();
     m->publishDebugMarker();
     m->publishVirtualWall();
     if (!m->isExecutionRequested()) {