feat(behavior_velocity_planner_common): add objects_of_interest_marke…

…r_intereface to behavior_velocity_planner (autowarefoundation#5875)

* feat(behavior_velocity_planner_common): add objects_of_interest_marker_intereface to behavior_velocity_planner

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

* fix(behavior_velocity_planner_common): add functions to publish objects of interest marker

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

* feat(no_stopping_area): insert object data

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

* refactor(behavior_velocity_planner_common): rename function

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

---------

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

planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp

@@ -230,13 +230,17 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea(
     }
     const auto obj_v = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x);
     if (obj_v > planner_param_.stuck_vehicle_vel_thr) {
+      setObjectsOfInterestData(
+        object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::GREEN);
       continue;  // not stop vehicle
     }
     // check if the footprint is in the stuck detect area
     const Polygon2d obj_footprint = tier4_autoware_utils::toPolygon2d(object);
     const bool is_in_stuck_area = !bg::disjoint(obj_footprint, poly);
     if (is_in_stuck_area) {
       RCLCPP_DEBUG(logger_, "stuck vehicle found.");
+      setObjectsOfInterestData(
+        object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED);
       for (const auto & p : obj_footprint.outer()) {
         geometry_msgs::msg::Point point;
         point.x = p.x();

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp

@@ -19,6 +19,7 @@
 #include <behavior_velocity_planner_common/velocity_factor_interface.hpp>
 #include <builtin_interfaces/msg/time.hpp>
 #include <motion_utils/marker/virtual_wall_marker_creator.hpp>
+#include <objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp>
 #include <rtc_interface/rtc_interface.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
 
@@ -47,6 +48,8 @@ namespace behavior_velocity_planner
 
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using builtin_interfaces::msg::Time;
+using objects_of_interest_marker_interface::ColorName;
+using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface;
 using rtc_interface::RTCInterface;
 using tier4_autoware_utils::DebugPublisher;
 using tier4_debug_msgs::msg::Float64Stamped;
@@ -55,6 +58,19 @@ using tier4_planning_msgs::msg::StopReason;
 using tier4_rtc_msgs::msg::Module;
 using unique_identifier_msgs::msg::UUID;
 
+struct ObjectOfInterest
+{
+  geometry_msgs::msg::Pose pose;
+  autoware_auto_perception_msgs::msg::Shape shape;
+  ColorName color;
+  ObjectOfInterest(
+    const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape,
+    const ColorName & color_name)
+  : pose(pose), shape(shape), color(color_name)
+  {
+  }
+};
+
 class SceneModuleInterface
 {
 public:
@@ -94,6 +110,8 @@ class SceneModuleInterface
 
   void resetVelocityFactor() { velocity_factor_.reset(); }
   VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); }
+  std::vector<ObjectOfInterest> getObjectsOfInterestData() const { return objects_of_interest_; }
+  void clearObjectsOfInterestData() { objects_of_interest_.clear(); }
 
 protected:
   const int64_t module_id_;
@@ -107,6 +125,7 @@ class SceneModuleInterface
   std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> infrastructure_command_;
   std::optional<int> first_stop_path_point_index_;
   VelocityFactorInterface velocity_factor_;
+  std::vector<ObjectOfInterest> objects_of_interest_;
 
   void setSafe(const bool safe)
   {
@@ -118,6 +137,13 @@ class SceneModuleInterface
   void setDistance(const double distance) { distance_ = distance; }
   void syncActivation() { setActivation(isSafe()); }
 
+  void setObjectsOfInterestData(
+    const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape,
+    const ColorName & color_name)
+  {
+    objects_of_interest_.emplace_back(pose, shape, color_name);
+  }
+
   size_t findEgoSegmentIndex(
     const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points) const;
 };
@@ -200,6 +226,8 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
   RTCInterface rtc_interface_;
   std::unordered_map<int64_t, UUID> map_uuid_;
 
+  ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_;
+
   virtual void sendRTC(const Time & stamp);
 
   virtual void setActivation();
@@ -220,6 +248,8 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
 
   void removeUUID(const int64_t & module_id);
 
+  void publishObjectsOfInterestMarker();
+
   void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 };
 

planning/behavior_velocity_planner_common/package.xml

@@ -32,6 +32,7 @@
   <depend>motion_utils</depend>
   <depend>motion_velocity_smoother</depend>
   <depend>nav_msgs</depend>
+  <depend>objects_of_interest_marker_interface</depend>
   <depend>pcl_conversions</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

planning/behavior_velocity_planner_common/src/scene_module_interface.cpp

@@ -192,7 +192,9 @@ void SceneModuleManagerInterface::unregisterModule(
 
 SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC(
   rclcpp::Node & node, const char * module_name, const bool enable_rtc)
-: SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name, enable_rtc)
+: SceneModuleManagerInterface(node, module_name),
+  rtc_interface_(&node, module_name, enable_rtc),
+  objects_of_interest_marker_interface_(&node, module_name)
 {
 }
 
@@ -202,6 +204,7 @@ void SceneModuleManagerInterfaceWithRTC::plan(
   setActivation();
   modifyPathVelocity(path);
   sendRTC(path->header.stamp);
+  publishObjectsOfInterestMarker();
 }
 
 void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp)
@@ -244,6 +247,19 @@ void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id)
   }
 }
 
+void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker()
+{
+  for (const auto & scene_module : scene_modules_) {
+    const auto objects = scene_module->getObjectsOfInterestData();
+    for (const auto & obj : objects) {
+      objects_of_interest_marker_interface_.insertObjectData(obj.pose, obj.shape, obj.color);
+    }
+    scene_module->clearObjectsOfInterestData();
+  }
+
+  objects_of_interest_marker_interface_.publishMarkerArray();
+}
+
 void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {