feat(behavior_path_planner): output drivable lanes marker (#4373)

* feat(behavior_path_planner): output drivable lanes marker

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(avoidance): output drivable lanes marker

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -296,6 +296,8 @@ class SceneModuleInterface
 
   MarkerArray getDebugMarkers() const { return debug_marker_; }
 
+  MarkerArray getDrivableLanesMarkers() const { return drivable_lanes_marker_; }
+
   virtual MarkerArray getModuleVirtualWall() { return MarkerArray(); }
 
   ModuleStatus getCurrentStatus() const { return current_state_; }
@@ -343,6 +345,12 @@ class SceneModuleInterface
     dead_pose_ = boost::none;
   }
 
+  void setDrivableLanes(const std::vector<DrivableLanes> & drivable_lanes)
+  {
+    drivable_lanes_marker_ =
+      marker_utils::createDrivableLanesMarkerArray(drivable_lanes, "drivable_lanes");
+  }
+
   rclcpp::Logger getLogger() const { return logger_; }
 
   void setIsSimultaneousExecutableAsApprovedModule(const bool enable)
@@ -496,6 +504,8 @@ class SceneModuleInterface
   mutable MarkerArray info_marker_;
 
   mutable MarkerArray debug_marker_;
+
+  mutable MarkerArray drivable_lanes_marker_;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp

@@ -68,6 +68,7 @@ class SceneModuleManagerInterface
     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);
+    pub_drivable_lanes_ = node->create_publisher<MarkerArray>("~/drivable_lanes/" + name, 20);
   }
 
   virtual ~SceneModuleManagerInterface() = default;
@@ -170,6 +171,7 @@ class SceneModuleManagerInterface
 
     MarkerArray info_markers{};
     MarkerArray debug_markers{};
+    MarkerArray drivable_lanes_markers{};
 
     const auto marker_offset = std::numeric_limits<uint8_t>::max();
 
@@ -185,16 +187,23 @@ class SceneModuleManagerInterface
         debug_markers.markers.push_back(marker);
       }
 
+      for (auto & marker : m->getDrivableLanesMarkers().markers) {
+        marker.id += marker_id;
+        drivable_lanes_markers.markers.push_back(marker);
+      }
+
       marker_id += marker_offset;
     }
 
     if (registered_modules_.empty() && idling_module_ptr_ != nullptr) {
       appendMarkerArray(idling_module_ptr_->getInfoMarkers(), &info_markers);
       appendMarkerArray(idling_module_ptr_->getDebugMarkers(), &debug_markers);
+      appendMarkerArray(idling_module_ptr_->getDrivableLanesMarkers(), &drivable_lanes_markers);
     }
 
     pub_info_marker_->publish(info_markers);
     pub_debug_marker_->publish(debug_markers);
+    pub_drivable_lanes_->publish(drivable_lanes_markers);
   }
 
   bool exist(const SceneModulePtr & module_ptr) const
@@ -271,6 +280,8 @@ class SceneModuleManagerInterface
 
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_virtual_wall_;
 
+  rclcpp::Publisher<MarkerArray>::SharedPtr pub_drivable_lanes_;
+
   std::string name_;
 
   std::shared_ptr<PlannerData> planner_data_;

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2512,6 +2512,7 @@ BehaviorModuleOutput AvoidanceModule::plan()
 
   // Drivable area generation.
   generateExtendedDrivableArea(output);
+  setDrivableLanes(output.drivable_area_info.drivable_lanes);
 
   updateRegisteredRTCStatus(spline_shift_path.path);