From 7fb4c1bd29c657847f366eb8853c016729f753a2 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 6 Jan 2025 22:01:40 +0900 Subject: [PATCH] feat(behavior_velocity_planner): remove virtual traffic light dependency from behavior_velocity_planner and behavior_velocity_planner_common (#9746) * feat: remove-virtual-traffic-light-dependency-from-plugin-manager Signed-off-by: Takayuki Murooka * build passed Signed-off-by: Takayuki Murooka * fix test Signed-off-by: Takayuki Murooka * fix test Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix typo Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../behavior_velocity_planner/node.hpp | 4 -- .../package.xml | 1 - .../src/node.cpp | 3 -- .../src/test_utils.cpp | 2 - .../planner_data.hpp | 2 - .../scene_module_interface.hpp | 26 ------------ .../package.xml | 1 - .../src/manager.cpp | 40 ++++++++++++++++++- .../src/manager.hpp | 18 ++++++++- .../src/scene.cpp | 25 ++++++++++-- .../src/scene.hpp | 14 +++++++ .../test/test_node_interface.cpp | 11 ++++- 12 files changed, 100 insertions(+), 47 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index 8a52ae70231d0..472538406c382 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -81,10 +81,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - autoware::universe_utils::InterProcessPollingSubscriber< - tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> - sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_occupancy_grid_{this, "~/input/occupancy_grid"}; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index eca49a09c8a56..c40d80c2bf998 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -56,7 +56,6 @@ tf2_geometry_msgs tf2_ros tier4_planning_msgs - tier4_v2x_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index 1e3e691076b8a..443bfebe2a3eb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -271,9 +271,6 @@ bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock) planner_data_.route_handler_ = std::make_shared(*map_data); } - // optional data - getData(planner_data_.virtual_traffic_light_states, sub_virtual_traffic_light_states_); - // planner_data_.external_velocity_limit is std::optional type variable. const auto external_velocity_limit = sub_external_velocity_limit_.takeData(); if (external_velocity_limit) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp index ee1bb8f89fc46..8b9f39e97d22b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp @@ -113,8 +113,6 @@ void publishMandatoryTopics( test_target_node, "behavior_velocity_planner_node/input/traffic_signals"); test_manager->publishMaxVelocity( test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps"); - test_manager->publishVirtualTrafficLightState( - test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); test_manager->publishOccupancyGrid( test_target_node, "behavior_velocity_planner_node/input/occupancy_grid"); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp index 983aaec2acf4f..f8b37999cb6bf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -30,7 +30,6 @@ #include #include #include -#include #include #include @@ -64,7 +63,6 @@ struct PlannerData std::map traffic_light_id_map_raw_; std::map traffic_light_id_map_last_observed_; std::optional external_velocity_limit; - tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; bool is_simulation = false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index 9dbed9009a93e..d8d640a078267 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -32,7 +32,6 @@ #include #include #include -#include #include #include @@ -95,17 +94,6 @@ class SceneModuleInterface planner_data_ = planner_data; } - std::optional getInfrastructureCommand() - { - return infrastructure_command_; - } - - void setInfrastructureCommand( - const std::optional & command) - { - infrastructure_command_ = command; - } - void setTimeKeeper(const std::shared_ptr & time_keeper) { time_keeper_ = time_keeper; @@ -123,7 +111,6 @@ class SceneModuleInterface rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; - std::optional infrastructure_command_; autoware::motion_utils::VelocityFactorInterface velocity_factor_; std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; @@ -161,9 +148,6 @@ class SceneModuleManagerInterface std::string("~/virtual_wall/") + module_name, 5); pub_velocity_factor_ = node.create_publisher( std::string("/planning/velocity_factors/") + module_name, 1); - pub_infrastructure_commands_ = - node.create_publisher( - "~/output/infrastructure_commands", 1); processing_time_publisher_ = std::make_shared(&node, "~/debug"); @@ -201,9 +185,6 @@ class SceneModuleManagerInterface velocity_factor_array.header.frame_id = "map"; velocity_factor_array.header.stamp = clock_->now(); - tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; - infrastructure_command_array.stamp = clock_->now(); - for (const auto & scene_module : scene_modules_) { scene_module->resetVelocityFactor(); scene_module->setPlannerData(planner_data_); @@ -215,10 +196,6 @@ class SceneModuleManagerInterface velocity_factor_array.factors.emplace_back(velocity_factor); } - if (const auto command = scene_module->getInfrastructureCommand()) { - infrastructure_command_array.commands.push_back(*command); - } - for (const auto & marker : scene_module->createDebugMarkerArray().markers) { debug_marker_array.markers.push_back(marker); } @@ -227,7 +204,6 @@ class SceneModuleManagerInterface } pub_velocity_factor_->publish(velocity_factor_array); - pub_infrastructure_commands_->publish(infrastructure_command_array); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { tier4_planning_msgs::msg::PathWithLaneId debug_path; @@ -299,8 +275,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_debug_path_; rclcpp::Publisher::SharedPtr pub_velocity_factor_; - rclcpp::Publisher::SharedPtr - pub_infrastructure_commands_; std::shared_ptr processing_time_publisher_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 002c3362260cc..9f920dff8f166 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -45,7 +45,6 @@ tf2_geometry_msgs tf2_ros tier4_planning_msgs - tier4_v2x_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index a51cf9211c21f..3815c83d3b6ab 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -21,6 +21,8 @@ #include #include +#include + #include #include @@ -53,6 +55,14 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node p.check_timeout_after_stop_line = getOrDeclareParameter(node, ns + ".check_timeout_after_stop_line"); } + + sub_virtual_traffic_light_states_ = autoware::universe_utils::InterProcessPollingSubscriber< + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>:: + create_subscription(&node, "~/input/virtual_traffic_light_states"); + + pub_infrastructure_commands_ = + node.create_publisher( + "~/output/infrastructure_commands", 1); } void VirtualTrafficLightModuleManager::launchNewModules( @@ -89,17 +99,43 @@ void VirtualTrafficLightModuleManager::launchNewModules( } } -std::function &)> +std::function &)> VirtualTrafficLightModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [id_set](const std::shared_ptr & scene_module) { + return [id_set](const std::shared_ptr & scene_module) { return id_set.count(scene_module->getModuleId()) == 0; }; } + +void VirtualTrafficLightModuleManager::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path) +{ + // NOTE: virtual traffic light specific implementation + // Since the argument of modifyPathVelocity cannot be changed, the specific information + // of virtual traffic light states is set here. + const auto virtual_traffic_light_states = sub_virtual_traffic_light_states_->takeData(); + for (const auto & scene_module : scene_modules_) { + scene_module->setVirtualTrafficLightStates(virtual_traffic_light_states); + } + + SceneModuleManagerInterface::modifyPathVelocity(path); + + // NOTE: virtual traffic light specific implementation + // publish infrastructure_command_array + tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; + infrastructure_command_array.stamp = clock_->now(); + + for (const auto & scene_module : scene_modules_) { + if (const auto command = scene_module->getInfrastructureCommand()) { + infrastructure_command_array.commands.push_back(*command); + } + } + pub_infrastructure_commands_->publish(infrastructure_command_array); +} } // namespace autoware::behavior_velocity_planner #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index 8e0abc98c90de..aecef0851d8ab 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -20,16 +20,20 @@ #include #include #include +#include #include #include +#include +#include #include #include namespace autoware::behavior_velocity_planner { -class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface<> +class VirtualTrafficLightModuleManager +: public SceneModuleManagerInterface { public: explicit VirtualTrafficLightModuleManager(rclcpp::Node & node); @@ -38,10 +42,20 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface<> private: VirtualTrafficLightModule::PlannerParam planner_param_; + + void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( + std::function &)> getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + autoware::universe_utils::InterProcessPollingSubscriber< + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr + sub_virtual_traffic_light_states_; + + rclcpp::Publisher::SharedPtr + pub_infrastructure_commands_; }; class VirtualTrafficLightModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 206fb14b6d41c..86239816ed6f2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -333,12 +333,12 @@ bool VirtualTrafficLightModule::isNearAnyEndLine(const size_t end_line_idx) std::optional VirtualTrafficLightModule::findCorrespondingState() { - // No message - if (!planner_data_->virtual_traffic_light_states) { + // Note: This variable is set by virtual traffic light's manager. + if (!virtual_traffic_light_states_) { return {}; } - for (const auto & state : planner_data_->virtual_traffic_light_states->states) { + for (const auto & state : virtual_traffic_light_states_->states) { if (state.id == map_data_.instrument_id) { return state; } @@ -457,4 +457,23 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( module_data_.stop_head_pose_at_end_line = calcHeadPose(stop_pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); } + +std::optional +VirtualTrafficLightModule::getInfrastructureCommand() const +{ + return infrastructure_command_; +} + +void VirtualTrafficLightModule::setInfrastructureCommand( + const std::optional & command) +{ + infrastructure_command_ = command; +} + +void VirtualTrafficLightModule::setVirtualTrafficLightStates( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr + virtual_traffic_light_states) +{ + virtual_traffic_light_states_ = virtual_traffic_light_states; +} } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index b031ba5b2bb2b..7f37e7078a431 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -24,6 +25,9 @@ #include #include +#include +#include + #include #include @@ -84,13 +88,23 @@ class VirtualTrafficLightModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; + std::optional getInfrastructureCommand() const; + void setInfrastructureCommand( + const std::optional & command); + + void setVirtualTrafficLightStates( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr + virtual_traffic_light_states); + private: const int64_t lane_id_; const lanelet::autoware::VirtualTrafficLight & reg_elem_; const lanelet::ConstLanelet lane_; const PlannerParam planner_param_; + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states_; State state_{State::NONE}; tier4_v2x_msgs::msg::InfrastructureCommand command_; + std::optional infrastructure_command_; MapData map_data_; ModuleData module_data_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp index 9cb6bbfd40639..ab2fd5847c5be 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp @@ -26,10 +26,17 @@ namespace autoware::behavior_velocity_planner TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) { rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "virtual_traffic_light", + "autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"}}; + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); - auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + test_manager->publishVirtualTrafficLightState( + test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); // test with nominal path_with_lane_id ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); @@ -51,6 +58,8 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + test_manager->publishVirtualTrafficLightState( + test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));