From 6ea6823f410a02b0c98364467f7082ca10c3c907 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Sat, 20 Jan 2024 15:07:26 +0900 Subject: [PATCH] feat(behavior_velocity_planner): add enable_all_modules_auto_mode argument to launch files for behavior velocity planner modules (#6094) * set default value for enable_all_modules_auto_mode Signed-off-by: kyoichi-sugahara * fix enable_rtc configuration in scene_module_manager_interface.hpp Signed-off-by: kyoichi-sugahara * Refactor scene module managers to use getEnableRTC function Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../behavior_planning.launch.xml | 1 + .../src/manager.cpp | 5 +---- .../src/manager.cpp | 3 +-- .../src/manager.cpp | 3 +-- .../src/manager.cpp | 5 ++--- .../src/manager.cpp | 3 +-- .../scene_module_interface.hpp | 17 +++++++++++++++++ .../src/manager.cpp | 3 +-- 8 files changed, 25 insertions(+), 15 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index d7da81c969da2..a297697234ef2 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -237,6 +237,7 @@ + diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index 09d1c5a7c3270..d114ab0c65da9 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -16,7 +16,6 @@ #include #include -#include #include @@ -28,12 +27,10 @@ namespace behavior_velocity_planner { -using tier4_autoware_utils::getOrDeclareParameter; BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.use_pass_judge_line = diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index b8190cb6124e7..dc3ce32be8505 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -31,8 +31,7 @@ using tier4_autoware_utils::getOrDeclareParameter; CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".common.enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".common.enable_rtc")) { const std::string ns(getModuleName()); diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index e7ec6b37f7f20..834ffc03e5dde 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -35,8 +35,7 @@ using tier4_autoware_utils::getOrDeclareParameter; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index c92f16dd7b474..cd5995b7f013c 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -35,11 +35,10 @@ using tier4_autoware_utils::getOrDeclareParameter; IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc.intersection")), + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection")), occlusion_rtc_interface_( &node, "intersection_occlusion", - getOrDeclareParameter( - node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) { const std::string ns(getModuleName()); auto & ip = intersection_param_; diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index d5e9eeddb6377..c4192750af1d5 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -35,8 +35,7 @@ using tier4_autoware_utils::getOrDeclareParameter; NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); auto & pp = planner_param_; 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 fcde16d8a871c..d8b042ec880e4 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 @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -52,6 +53,7 @@ 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_autoware_utils::getOrDeclareParameter; using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; @@ -251,6 +253,21 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void publishObjectsOfInterestMarker(); void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) + { + bool enable_rtc = true; + + try { + enable_rtc = getOrDeclareParameter(node, "enable_all_modules_auto_mode") + ? false + : getOrDeclareParameter(node, param_name); + } catch (const std::exception & e) { + enable_rtc = getOrDeclareParameter(node, param_name); + } + + return enable_rtc; + } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index fa1a516c107b0..89817f3342dbd 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -31,8 +31,7 @@ using tier4_autoware_utils::getOrDeclareParameter; TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin");