feat(behavior_path_planner): add objects of interest marker interface (…

…autowarefoundation#5695)

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -275,6 +275,8 @@ class PlannerManager
 
     module_ptr->publishRTCStatus();
 
+    module_ptr->publishObjectsOfInterestMarker();
+
     processing_time_.at(module_ptr->name()) += stop_watch_.toc(module_ptr->name(), true);
 
     return result;
@@ -298,6 +300,7 @@ class PlannerManager
   {
     module_ptr->onExit();
     module_ptr->publishRTCStatus();
+    module_ptr->publishObjectsOfInterestMarker();
     module_ptr.reset();
   }
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -49,7 +49,9 @@ class AvoidanceModule : public SceneModuleInterface
 public:
   AvoidanceModule(
     const std::string & name, rclcpp::Node & node, std::shared_ptr<AvoidanceParameters> parameters,
-    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+    std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+      objects_of_interest_marker_interface_ptr_map);
 
   CandidateOutput planCandidate() const override;
   BehaviorModuleOutput plan() override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp

@@ -37,7 +37,9 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface
 
   std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
-    return std::make_unique<AvoidanceModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
+    return std::make_unique<AvoidanceModule>(
+      name_, *node_, parameters_, rtc_interface_ptr_map_,
+      objects_of_interest_marker_interface_ptr_map_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp

@@ -268,7 +268,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface
   DynamicAvoidanceModule(
     const std::string & name, rclcpp::Node & node,
     std::shared_ptr<DynamicAvoidanceParameters> parameters,
-    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+    std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+      objects_of_interest_marker_interface_ptr_map);
 
   void updateModuleParams(const std::any & parameters) override
   {

planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp

@@ -37,7 +37,8 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface
   std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
     return std::make_unique<DynamicAvoidanceModule>(
-      name_, *node_, parameters_, rtc_interface_ptr_map_);
+      name_, *node_, parameters_, rtc_interface_ptr_map_,
+      objects_of_interest_marker_interface_ptr_map_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

@@ -261,7 +261,9 @@ class GoalPlannerModule : public SceneModuleInterface
   GoalPlannerModule(
     const std::string & name, rclcpp::Node & node,
     const std::shared_ptr<GoalPlannerParameters> & parameters,
-    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+    std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+      objects_of_interest_marker_interface_ptr_map);
 
   void updateModuleParams(const std::any & parameters) override
   {

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp

@@ -35,7 +35,9 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface
 
   std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
-    return std::make_unique<GoalPlannerModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
+    return std::make_unique<GoalPlannerModule>(
+      name_, *node_, parameters_, rtc_interface_ptr_map_,
+      objects_of_interest_marker_interface_ptr_map_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp

@@ -25,7 +25,6 @@
 #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
 #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp"
 #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
-#include "objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -49,8 +48,6 @@ namespace behavior_path_planner
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
-using objects_of_interest_marker_interface::ColorName;
-using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface;
 using tier4_planning_msgs::msg::LaneChangeDebugMsg;
 using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
 
@@ -59,7 +56,9 @@ class LaneChangeInterface : public SceneModuleInterface
 public:
   LaneChangeInterface(
     const std::string & name, rclcpp::Node & node, std::shared_ptr<LaneChangeParameters> parameters,
-    const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map,
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+    std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+      objects_of_interest_marker_interface_ptr_map,
     std::unique_ptr<LaneChangeBase> && module_type);
 
   LaneChangeInterface(const LaneChangeInterface &) = delete;
@@ -136,13 +135,6 @@ class LaneChangeInterface : public SceneModuleInterface
 
   void setObjectDebugVisualization() const;
 
-  void setObjectsOfInterestData(const bool is_approved);
-
-  void publishObjectsOfInterestData()
-  {
-    objects_of_interest_marker_interface_.publishMarkerArray();
-  }
-
   void updateSteeringFactorPtr(const BehaviorModuleOutput & output);
 
   void updateSteeringFactorPtr(
@@ -152,7 +144,6 @@ class LaneChangeInterface : public SceneModuleInterface
   mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_;
 
   std::unique_ptr<PathWithLaneId> prev_approved_path_;
-  ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_;
 
   void clearAbortApproval() { is_abort_path_approved_ = false; }
 
@@ -168,7 +159,9 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface
     const std::string & name, rclcpp::Node & node,
     const std::shared_ptr<LaneChangeParameters> & parameters,
     const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
-    const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map);
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+    std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+      objects_of_interest_marker_interface_ptr_map);
 
   bool isExecutionRequested() const override;
 

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

@@ -26,6 +26,7 @@
 #include <motion_utils/marker/marker_helper.hpp>
 #include <motion_utils/trajectory/path_with_lane_id.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
+#include <objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <route_handler/route_handler.hpp>
 #include <rtc_interface/rtc_interface.hpp>
@@ -56,6 +57,8 @@ namespace behavior_path_planner
 {
 using autoware_adapi_v1_msgs::msg::SteeringFactor;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using objects_of_interest_marker_interface::ColorName;
+using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface;
 using rtc_interface::RTCInterface;
 using steering_factor_interface::SteeringFactorInterface;
 using tier4_autoware_utils::calcOffsetPose;
@@ -81,11 +84,15 @@ class SceneModuleInterface
 public:
   SceneModuleInterface(
     const std::string & name, rclcpp::Node & node,
-    std::unordered_map<std::string, std::shared_ptr<RTCInterface>> rtc_interface_ptr_map)
+    std::unordered_map<std::string, std::shared_ptr<RTCInterface>> rtc_interface_ptr_map,
+    std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>>
+      objects_of_interest_marker_interface_ptr_map)
   : name_{name},
     logger_{node.get_logger().get_child(name)},
     clock_{node.get_clock()},
     rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)),
+    objects_of_interest_marker_interface_ptr_map_(
+      std::move(objects_of_interest_marker_interface_ptr_map)),
     steering_factor_interface_ptr_(
       std::make_unique<SteeringFactorInterface>(&node, utils::convertToSnakeCase(name)))
   {
@@ -191,6 +198,15 @@ class SceneModuleInterface
     }
   }
 
+  void publishObjectsOfInterestMarker()
+  {
+    for (const auto & [module_name, ptr] : objects_of_interest_marker_interface_ptr_map_) {
+      if (ptr) {
+        ptr->publishMarkerArray();
+      }
+    }
+  }
+
   void publishSteeringFactor()
   {
     if (!steering_factor_interface_ptr_) {
@@ -415,6 +431,17 @@ class SceneModuleInterface
     }
   }
 
+  void setObjectsOfInterestData(
+    const geometry_msgs::msg::Pose & obj_pose,
+    const autoware_auto_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name)
+  {
+    for (const auto & [module_name, ptr] : objects_of_interest_marker_interface_ptr_map_) {
+      if (ptr) {
+        ptr->insertObjectData(obj_pose, obj_shape, color_name);
+      }
+    }
+  }
+
   /**
    * @brief Return SUCCESS if plan is not needed or plan is successfully finished,
    *        FAILURE if plan has failed, RUNNING if plan is on going.
@@ -569,6 +596,9 @@ class SceneModuleInterface
 
   std::unordered_map<std::string, std::shared_ptr<RTCInterface>> rtc_interface_ptr_map_;
 
+  std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>>
+    objects_of_interest_marker_interface_ptr_map_;
+
   std::unique_ptr<SteeringFactorInterface> steering_factor_interface_ptr_;
 
   mutable boost::optional<Pose> stop_pose_{boost::none};

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

@@ -65,6 +65,8 @@ class SceneModuleManagerInterface
         rtc_type == "" ? snake_case_name : snake_case_name + "_" + rtc_type;
       rtc_interface_ptr_map_.emplace(
         rtc_type, std::make_shared<RTCInterface>(node, rtc_interface_name, enable_rtc_));
+      objects_of_interest_marker_interface_ptr_map_.emplace(
+        rtc_type, std::make_shared<ObjectsOfInterestMarkerInterface>(node, rtc_interface_name));
     }
 
     pub_info_marker_ = node->create_publisher<MarkerArray>("~/info/" + name, 20);
@@ -305,6 +307,9 @@ class SceneModuleManagerInterface
 
   std::unordered_map<std::string, std::shared_ptr<RTCInterface>> rtc_interface_ptr_map_;
 
+  std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>>
+    objects_of_interest_marker_interface_ptr_map_;
+
   bool enable_simultaneous_execution_as_approved_module_{false};
 
   bool enable_simultaneous_execution_as_candidate_module_{false};

planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp

@@ -36,7 +36,9 @@ class SideShiftModuleManager : public SceneModuleManagerInterface
 
   std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
-    return std::make_unique<SideShiftModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
+    return std::make_unique<SideShiftModule>(
+      name_, *node_, parameters_, rtc_interface_ptr_map_,
+      objects_of_interest_marker_interface_ptr_map_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp

@@ -43,7 +43,9 @@ class SideShiftModule : public SceneModuleInterface
   SideShiftModule(
     const std::string & name, rclcpp::Node & node,
     const std::shared_ptr<SideShiftParameters> & parameters,
-    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+    std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+      objects_of_interest_marker_interface_ptr_map);
 
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp

@@ -35,7 +35,9 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface
 
   std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
-    return std::make_unique<StartPlannerModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
+    return std::make_unique<StartPlannerModule>(
+      name_, *node_, parameters_, rtc_interface_ptr_map_,
+      objects_of_interest_marker_interface_ptr_map_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp

@@ -79,7 +79,9 @@ class StartPlannerModule : public SceneModuleInterface
   StartPlannerModule(
     const std::string & name, rclcpp::Node & node,
     const std::shared_ptr<StartPlannerParameters> & parameters,
-    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+    std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+      objects_of_interest_marker_interface_ptr_map);
 
   void updateModuleParams(const std::any & parameters) override
   {

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

@@ -151,8 +151,10 @@ lanelet::BasicLineString3d toLineString3d(const std::vector<Point> & bound)
 
 AvoidanceModule::AvoidanceModule(
   const std::string & name, rclcpp::Node & node, std::shared_ptr<AvoidanceParameters> parameters,
-  const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map)
-: SceneModuleInterface{name, node, rtc_interface_ptr_map},
+  const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+  std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+    objects_of_interest_marker_interface_ptr_map)
+: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map},  // NOLINT
   parameters_{parameters},
   helper_{parameters}
 {

planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp

@@ -248,8 +248,10 @@ double calcDistanceToSegment(
 DynamicAvoidanceModule::DynamicAvoidanceModule(
   const std::string & name, rclcpp::Node & node,
   std::shared_ptr<DynamicAvoidanceParameters> parameters,
-  const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map)
-: SceneModuleInterface{name, node, rtc_interface_ptr_map},
+  const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+  std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+    objects_of_interest_marker_interface_ptr_map)
+: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map},  // NOLINT
   parameters_{std::move(parameters)},
   target_objects_manager_{TargetObjectsManager(
     parameters_->successive_num_to_entry_dynamic_avoidance_condition,

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -54,8 +54,10 @@ namespace behavior_path_planner
 GoalPlannerModule::GoalPlannerModule(
   const std::string & name, rclcpp::Node & node,
   const std::shared_ptr<GoalPlannerParameters> & parameters,
-  const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map)
-: SceneModuleInterface{name, node, rtc_interface_ptr_map},
+  const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+  std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+    objects_of_interest_marker_interface_ptr_map)
+: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map},  // NOLINT
   parameters_{parameters},
   vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()},
   thread_safe_data_{mutex_, clock_}