From 0e0b12b530411d8ccbcf8528b5b8b88c487a0ef3 Mon Sep 17 00:00:00 2001
From: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Date: Fri, 19 Jan 2024 20:41:28 +0900
Subject: [PATCH 1/2] feat(behavior_path_planner): add
 enable_all_modules_auto_mode argument to launch files for behavior path
 planner modules (#6093)

* Add enable_all_modules_auto_mode argument to launch files

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* set default value for enable_all_modules_auto_mode

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* fix enable_rtc configuration in scene_module_manager_interface.hpp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 launch/tier4_planning_launch/launch/planning.launch.xml  | 6 +++++-
 .../launch/scenario_planning/lane_driving.launch.xml     | 3 +++
 .../behavior_planning/behavior_planning.launch.xml       | 2 ++
 .../scenario_planning/scenario_planning.launch.xml       | 5 ++++-
 .../interface/scene_module_manager_interface.hpp         | 9 ++++++++-
 5 files changed, 22 insertions(+), 3 deletions(-)

diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml
index bf81125286256..e02ba883d5115 100644
--- a/launch/tier4_planning_launch/launch/planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/planning.launch.xml
@@ -7,6 +7,8 @@
   <arg name="freespace_planner_param_path"/>
   <!-- planning validator -->
   <arg name="planning_validator_param_path"/>
+  <!-- Auto mode setting-->
+  <arg name="enable_all_modules_auto_mode"/>
 
   <group>
     <push-ros-namespace namespace="planning"/>
@@ -21,7 +23,9 @@
     <!-- scenario planning module -->
     <group>
       <push-ros-namespace namespace="scenario_planning"/>
-      <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/scenario_planning.launch.xml"/>
+      <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/scenario_planning.launch.xml">
+        <arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+      </include>
     </group>
 
     <!-- planning validator -->
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml
index 73e466afc0bda..bae084f80b51e 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml
@@ -1,4 +1,6 @@
 <launch>
+  <arg name="enable_all_modules_auto_mode"/>
+
   <!-- lane_driving scenario -->
   <group>
     <push-ros-namespace namespace="lane_driving"/>
@@ -8,6 +10,7 @@
       <group>
         <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml">
           <arg name="container_type" value="component_container_mt"/>
+          <arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
           <!-- This condition should be true if run_out module is enabled and its detection method is Points -->
           <arg name="launch_compare_map_pipeline" value="false"/>
         </include>
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 1c7643359c496..d7da81c969da2 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
@@ -4,6 +4,7 @@
   <arg name="input_virtual_traffic_light_topic_name" default="/awapi/tmp/virtual_traffic_light_states"/>
   <arg name="input_vector_map_topic_name" default="/map/vector_map"/>
   <arg name="input_pointcloud_map_topic_name" default="/map/pointcloud_map"/>
+  <arg name="enable_all_modules_auto_mode"/>
 
   <arg name="launch_avoidance_module" default="true"/>
   <arg name="launch_avoidance_by_lane_change_module" default="true"/>
@@ -195,6 +196,7 @@
       <param from="$(var vehicle_param_file)"/>
       <param from="$(var nearest_search_param_path)"/>
       <param name="launch_modules" value="$(var behavior_path_planner_launch_modules)"/>
+      <param name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
       <param from="$(var behavior_path_planner_side_shift_module_param_path)"/>
       <param from="$(var behavior_path_planner_avoidance_module_param_path)"/>
       <param from="$(var behavior_path_planner_avoidance_by_lc_module_param_path)"/>
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
index 66c90ef2ff833..e673d28804480 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
@@ -1,4 +1,5 @@
 <launch>
+  <arg name="enable_all_modules_auto_mode"/>
   <!-- scenario selector -->
   <group>
     <include file="$(find-pkg-share scenario_selector)/launch/scenario_selector.launch.xml">
@@ -50,7 +51,9 @@
   <group>
     <!-- lane driving -->
     <group>
-      <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving.launch.xml"/>
+      <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving.launch.xml">
+        <arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+      </include>
     </group>
     <!-- parking -->
     <group>
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp
index 43ecd17fa1844..e8a28ee684ce6 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp
@@ -268,7 +268,14 @@ class SceneModuleManagerInterface
     // init manager configuration
     {
       std::string ns = name_ + ".";
-      config_.enable_rtc = getOrDeclareParameter<bool>(*node, ns + "enable_rtc");
+      try {
+        config_.enable_rtc = getOrDeclareParameter<bool>(*node, "enable_all_modules_auto_mode")
+                               ? false
+                               : getOrDeclareParameter<bool>(*node, ns + "enable_rtc");
+      } catch (const std::exception & e) {
+        config_.enable_rtc = getOrDeclareParameter<bool>(*node, ns + "enable_rtc");
+      }
+
       config_.enable_simultaneous_execution_as_approved_module =
         getOrDeclareParameter<bool>(*node, ns + "enable_simultaneous_execution_as_approved_module");
       config_.enable_simultaneous_execution_as_candidate_module = getOrDeclareParameter<bool>(

From 6ea6823f410a02b0c98364467f7082ca10c3c907 Mon Sep 17 00:00:00 2001
From: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Date: Sat, 20 Jan 2024 15:07:26 +0900
Subject: [PATCH 2/2] 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 <kyoichi.sugahara@tier4.jp>

* fix enable_rtc configuration in scene_module_manager_interface.hpp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Refactor scene module managers to use getEnableRTC function

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 .../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 @@
       <param from="$(var behavior_velocity_smoother_type_param_path)"/>
       <param from="$(var behavior_velocity_planner_common_param_path)"/>
       <param name="launch_modules" value="$(var behavior_velocity_planner_launch_modules)"/>
+      <param name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
       <!-- <param from="$(var template_param_path)"/> -->
       <param from="$(var behavior_velocity_planner_blind_spot_module_param_path)"/>
       <param from="$(var behavior_velocity_planner_crosswalk_module_param_path)"/>
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 <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
-#include <tier4_autoware_utils/ros/parameter.hpp>
 
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
 
@@ -28,12 +27,10 @@
 
 namespace behavior_velocity_planner
 {
-using tier4_autoware_utils::getOrDeclareParameter;
 
 BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node)
 : SceneModuleManagerInterfaceWithRTC(
-    node, getModuleName(),
-    getOrDeclareParameter<bool>(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<bool>(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<bool>(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<double>(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<bool>(node, std::string(getModuleName()) + ".enable_rtc.intersection")),
+    getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection")),
   occlusion_rtc_interface_(
     &node, "intersection_occlusion",
-    getOrDeclareParameter<bool>(
-      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<bool>(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 <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>
+#include <tier4_autoware_utils/ros/parameter.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
 #include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
@@ -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<bool>(node, "enable_all_modules_auto_mode")
+                     ? false
+                     : getOrDeclareParameter<bool>(node, param_name);
+    } catch (const std::exception & e) {
+      enable_rtc = getOrDeclareParameter<bool>(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<bool>(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<double>(node, ns + ".stop_margin");