From 174a6ef545dd632583cd0c983f223fa8c12e1602 Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Wed, 13 Dec 2023 16:38:49 +0900
Subject: [PATCH] fix(behavior, launch): fix launch error (#5847)

* fix(launch): set null to avoid launch error

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(behavior): check null

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* chore(behavior): add comment

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(launch): set  at the end of list

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(launch): fill empty string at the end of module list

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../behavior_planning/behavior_planning.launch.xml          | 6 ++++--
 .../src/behavior_path_planner_node.cpp                      | 4 ++++
 planning/behavior_velocity_planner/src/node.cpp             | 4 ++++
 3 files changed, 12 insertions(+), 2 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 3a1f417106f54..4539e15a55954 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
@@ -43,6 +43,8 @@
   <arg name="launch_out_of_lane_module" default="true"/>
   <arg name="launch_no_drivable_lane_module" default="true"/>
 
+  <arg name="launch_module_list_end" default="&quot;&quot;]"/>
+
   <!-- assemble launch config for behavior path planner -->
   <arg name="behavior_path_planner_launch_modules" default="["/>
   <let
@@ -95,7 +97,7 @@
     value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::AvoidanceByLaneChangeModuleManager, '&quot;)"
     if="$(var launch_avoidance_by_lane_change_module)"
   />
-  <let name="behavior_path_planner_launch_modules" value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + ']'&quot;)"/>
+  <let name="behavior_path_planner_launch_modules" value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + '$(var launch_module_list_end)'&quot;)"/>
 
   <!-- assemble launch config for behavior velocity planner -->
   <arg name="behavior_velocity_planner_launch_modules" default="["/>
@@ -174,7 +176,7 @@
     value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::NoDrivableLaneModulePlugin, '&quot;)"
     if="$(var launch_no_drivable_lane_module)"
   />
-  <let name="behavior_velocity_planner_launch_modules" value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + ']'&quot;)"/>
+  <let name="behavior_velocity_planner_launch_modules" value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + '$(var launch_module_list_end)'&quot;)"/>
 
   <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="" output="screen">
     <composable_node pkg="behavior_path_planner" plugin="behavior_path_planner::BehaviorPathPlannerNode" name="behavior_path_planner" namespace="">
diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
index 46057031afd8a..4828a0d62e7f6 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -135,6 +135,10 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     planner_manager_ = std::make_shared<PlannerManager>(*this, p.max_iteration_num, p.verbose);
 
     for (const auto & name : declare_parameter<std::vector<std::string>>("launch_modules")) {
+      // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter.
+      if (name == "") {
+        break;
+      }
       planner_manager_->launchScenePlugin(*this, name);
     }
 
diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp
index 7c0036b7812fa..5da8df9e70c35 100644
--- a/planning/behavior_velocity_planner/src/node.cpp
+++ b/planning/behavior_velocity_planner/src/node.cpp
@@ -147,6 +147,10 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
 
   // Initialize PlannerManager
   for (const auto & name : declare_parameter<std::vector<std::string>>("launch_modules")) {
+    // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter.
+    if (name == "") {
+      break;
+    }
     planner_manager_.launchScenePlugin(*this, name);
   }