Deprecate ParseMultiRobotPose with launch.actions.ForEach (#5000)

leander-dsouza · web-flow · commit 2cd166f53ed0 · 2025-03-21T15:26:16.000-07:00
* Removed ParseMultiRobotPose and introduced ForEach instead.

Signed-off-by: Leander Stephen D'Souza &lt;leanderdsouza1234@gmail.com&gt;

* Deprecated ParseMultiRobotPose throughout the codebase.

Signed-off-by: Leander Stephen D'Souza &lt;leanderdsouza1234@gmail.com&gt;

* Bump version to break cache.

Signed-off-by: Leander Stephen D'Souza &lt;leanderdsouza1234@gmail.com&gt;

---------

Signed-off-by: Leander Stephen D'Souza &lt;leanderdsouza1234@gmail.com&gt;
diff --git a/.circleci/config.yml b/.circleci/config.yml
@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v31\
+              - "<< parameters.key >>-v32\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v31\
+              - "<< parameters.key >>-v32\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v31\
+            key: "<< parameters.key >>-v32\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\
diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py
@@ -24,20 +24,43 @@
     AppendEnvironmentVariable,
     DeclareLaunchArgument,
     ExecuteProcess,
+    ForEach,
     GroupAction,
     IncludeLaunchDescription,
     LogInfo,
     OpaqueFunction,
     RegisterEventHandler,
 )
+
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
+from launch.launch_context import LaunchContext
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, TextSubstitution
-from nav2_common.launch import ParseMultiRobotPose
 
+import yaml
+
+
+def count_robots(context: LaunchContext) -> list[LogInfo]:
+    """Count the number of robots from the 'robots' launch argument."""
+    robots_str = LaunchConfiguration('robots').perform(context).strip()
+    log_msg = ''
+
+    if not robots_str:
+        log_msg = 'No robots provided in the launch argument.'
+
+    try:
+        robots_list = [yaml.safe_load(robot.strip()) for robot in
+                       robots_str.split(';') if robot.strip()]
+        log_msg = f'number_of_robots={len(robots_list)}'
+    except yaml.YAMLError as e:
+        log_msg = f'Error parsing the robots launch argument: {e}'
+
+    return [LogInfo(msg=[log_msg])]
 
-def generate_robot_actions(context, *args, **kwargs):
+
+def generate_robot_actions(name: str = '', pose: dict = {}) -> list[GroupAction]:
+    """Generate the actions to launch a robot with the given name and pose."""
     bringup_dir = get_package_share_directory('nav2_bringup')
     launch_dir = os.path.join(bringup_dir, 'launch')
     use_rviz = LaunchConfiguration('use_rviz')
@@ -47,25 +70,19 @@ def generate_robot_actions(context, *args, **kwargs):
     map_yaml_file = LaunchConfiguration('map')
     use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
 
-    robots_substitution = ParseMultiRobotPose(LaunchConfiguration('robots'))
-    robots_list = robots_substitution.perform(context)
-
     # Define commands for launching the navigation instances
-    bringup_cmd_group = []
-    for robot_name in robots_list:
-        init_pose = robots_list[robot_name]
-        group = GroupAction(
+    group = GroupAction(
             [
                 LogInfo(
-                    msg=['Launching namespace=', robot_name, ' init_pose=', str(init_pose),]
+                    msg=['Launching namespace=', name, ' init_pose=', str(pose),]
                 ),
                 IncludeLaunchDescription(
                     PythonLaunchDescriptionSource(
                         os.path.join(launch_dir, 'rviz_launch.py')
                     ),
                     condition=IfCondition(use_rviz),
                     launch_arguments={
-                        'namespace': TextSubstitution(text=robot_name),
+                        'namespace': TextSubstitution(text=name),
                         'rviz_config': rviz_config_file,
                     }.items(),
                 ),
@@ -74,7 +91,7 @@ def generate_robot_actions(context, *args, **kwargs):
                         os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py')
                     ),
                     launch_arguments={
-                        'namespace': robot_name,
+                        'namespace': name,
                         'map': map_yaml_file,
                         'use_sim_time': 'True',
                         'params_file': params_file,
@@ -83,21 +100,18 @@ def generate_robot_actions(context, *args, **kwargs):
                         'use_simulator': 'False',
                         'headless': 'False',
                         'use_robot_state_pub': use_robot_state_pub,
-                        'x_pose': TextSubstitution(text=str(init_pose['x'])),
-                        'y_pose': TextSubstitution(text=str(init_pose['y'])),
-                        'z_pose': TextSubstitution(text=str(init_pose['z'])),
-                        'roll': TextSubstitution(text=str(init_pose['roll'])),
-                        'pitch': TextSubstitution(text=str(init_pose['pitch'])),
-                        'yaw': TextSubstitution(text=str(init_pose['yaw'])),
-                        'robot_name': TextSubstitution(text=robot_name),
+                        'x_pose': TextSubstitution(text=str(pose.get('x', 0.0))),
+                        'y_pose': TextSubstitution(text=str(pose.get('y', 0.0))),
+                        'z_pose': TextSubstitution(text=str(pose.get('z', 0.0))),
+                        'roll': TextSubstitution(text=str(pose.get('roll', 0.0))),
+                        'pitch': TextSubstitution(text=str(pose.get('pitch', 0.0))),
+                        'yaw': TextSubstitution(text=str(pose.get('yaw', 0.0))),
+                        'robot_name': TextSubstitution(text=name),
                     }.items(),
                 ),
             ]
         )
-
-        bringup_cmd_group.append(group)
-    bringup_cmd_group.append(LogInfo(msg=['number_of_robots=', str(len(robots_list))]))
-    return bringup_cmd_group
+    return [group]
 
 
 def generate_launch_description():
@@ -106,9 +120,13 @@ def generate_launch_description():
 
     Launch arguments consist of robot name(which is namespace) and pose for initialization.
     Keep general yaml format for pose information.
-    ex) robots:='robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}'
-    ex) robots:='robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707};
-                 robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}'
+
+    ex) robots:='{name: 'robot1', pose: {x: 1.0, y: 1.0, yaw: 1.5707}};
+                 {name: 'robot2', pose: {x: 1.0, y: 1.0, yaw: 1.5707}}'
+    ex) robots:='{name: 'robot3', pose: {x: 1.0, y: 1.0, z: 1.0,
+                                    roll: 0.0, pitch: 1.5707, yaw: 1.5707}};
+                 {name: 'robot4', pose: {x: 1.0, y: 1.0, z: 1.0,
+                                    roll: 0.0, pitch: 1.5707, yaw: 1.5707}}'
     """
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
@@ -134,8 +152,10 @@ def generate_launch_description():
 
     declare_robots_cmd = DeclareLaunchArgument(
         'robots',
-        default_value="""robot1={x: 0.5, y: 0.5, yaw: 0};
-                   robot2={x: -0.5, y: -0.5, z: 0, roll: 0, pitch: 0, yaw: 1.5707}""",
+        default_value=(
+            "{name: 'robot1', pose: {x: 0.5, y: 0.5, yaw: 0}};"
+            "{name: 'robot2', pose: {x: -0.5, y: -0.5, z: 0, roll: 0, pitch: 0, yaw: 1.5707}}"
+        ),
         description='Robots and their initialization poses in YAML format',
     )
 
@@ -237,6 +257,8 @@ def generate_launch_description():
     ld.add_action(
         LogInfo(condition=IfCondition(log_settings), msg=['autostart: ', autostart])
     )
-    ld.add_action(OpaqueFunction(function=generate_robot_actions))
+
+    ld.add_action(OpaqueFunction(function=count_robots))
+    ld.add_action(ForEach(LaunchConfiguration('robots'), function=generate_robot_actions))
 
     return ld
diff --git a/nav2_common/nav2_common/launch/__init__.py b/nav2_common/nav2_common/launch/__init__.py
@@ -13,13 +13,11 @@
 # limitations under the License.
 
 from .has_node_params import HasNodeParams
-from .parse_multirobot_pose import ParseMultiRobotPose
 from .replace_string import ReplaceString
 from .rewritten_yaml import RewrittenYaml
 
 __all__ = [
     'HasNodeParams',
     'RewrittenYaml',
     'ReplaceString',
-    'ParseMultiRobotPose',
 ]
diff --git a/nav2_common/nav2_common/launch/parse_multirobot_pose.py b/nav2_common/nav2_common/launch/parse_multirobot_pose.py
