From 931729c55646f0c0661e9e4d75d366e72c5e72d2 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 15 Dec 2023 09:42:32 +0900 Subject: [PATCH] feat(behavior_velocity_planner_common): add objects_of_interest_marker_intereface to behavior_velocity_planner (#5875) * feat(behavior_velocity_planner_common): add objects_of_interest_marker_intereface to behavior_velocity_planner Signed-off-by: Fumiya Watanabe * fix(behavior_velocity_planner_common): add functions to publish objects of interest marker Signed-off-by: Fumiya Watanabe * feat(no_stopping_area): insert object data Signed-off-by: Fumiya Watanabe * refactor(behavior_velocity_planner_common): rename function Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../src/scene_no_stopping_area.cpp | 4 +++ .../scene_module_interface.hpp | 30 +++++++++++++++++++ .../package.xml | 1 + .../src/scene_module_interface.cpp | 18 ++++++++++- 4 files changed, 52 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 47d4985ae643e..e049d02ffe9b5 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -230,6 +230,8 @@ 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 @@ -237,6 +239,8 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( 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(); diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index ced2e267cc025..fcde16d8a871c 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -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 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 infrastructure_command_; std::optional first_stop_path_point_index_; VelocityFactorInterface velocity_factor_; + std::vector 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 & points) const; }; @@ -200,6 +226,8 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface RTCInterface rtc_interface_; std::unordered_map 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; }; diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 187720ff92a15..784b7cabfe9ad 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -32,6 +32,7 @@ motion_utils motion_velocity_smoother nav_msgs + objects_of_interest_marker_interface pcl_conversions rclcpp rclcpp_components diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp index 49a52fcd60df2..6f596be92ec8b 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/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) {