From 8da1ce44723d6e239ed16decd1f0a58c02a6ef8e Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Fri, 21 Mar 2025 14:50:55 +0000 Subject: [PATCH 1/3] Removed ParseMultiRobotPose and introduced ForEach instead. Signed-off-by: Leander Stephen D'Souza --- .../cloned_multi_tb3_simulation_launch.py | 80 ++++++++++++------- 1 file changed, 51 insertions(+), 29 deletions(-) diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index ecffa7771c8..823f56ba2a7 100644 --- 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,17 +70,11 @@ 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( @@ -65,7 +82,7 @@ def generate_robot_actions(context, *args, **kwargs): ), 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 From 40a9353215443720169e41425d04e2643960891f Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Fri, 21 Mar 2025 14:52:57 +0000 Subject: [PATCH 2/3] Deprecated ParseMultiRobotPose throughout the codebase. Signed-off-by: Leander Stephen D'Souza --- nav2_common/nav2_common/launch/__init__.py | 2 - .../launch/parse_multirobot_pose.py | 66 ------------------- 2 files changed, 68 deletions(-) delete mode 100644 nav2_common/nav2_common/launch/parse_multirobot_pose.py diff --git a/nav2_common/nav2_common/launch/__init__.py b/nav2_common/nav2_common/launch/__init__.py index 61ea85dd793..b8eaaba2a9f 100644 --- a/nav2_common/nav2_common/launch/__init__.py +++ b/nav2_common/nav2_common/launch/__init__.py @@ -13,7 +13,6 @@ # 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 @@ -21,5 +20,4 @@ '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 deleted file mode 100644 index ced8352ac42..00000000000 --- a/nav2_common/nav2_common/launch/parse_multirobot_pose.py +++ /dev/null @@ -1,66 +0,0 @@ -# Copyright (c) 2023 LG Electronics. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from typing import Dict, Text - -import launch -from launch.launch_context import LaunchContext -import yaml - - -class ParseMultiRobotPose(launch.Substitution): - """ - A custom substitution to parse the robots argument for multi-robot poses. - - Expects input in the format: - robots:="robot1={x: 1.0, y: 1.0, yaw: 0.0}; - robot2={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}"` - - The individual robots are separated by a `;` and each robot consists of a name and pose object. - The name corresponds to the namespace of the robot and name of the Gazebo object. - The pose consists of X, Y, Z, Roll, Pitch, Yaw each of which can be omitted in which case it is - inferred as 0. - """ - - def __init__(self, robots_argument: launch.SomeSubstitutionsType) -> None: - super().__init__() - self.__robots_argument = robots_argument - - def describe(self) -> Text: - """Return a description of this substitution as a string.""" - return '' - - def perform(self, context: LaunchContext) -> Dict: - """Resolve and parse the robots argument string into a dictionary.""" - robots_str = self.__robots_argument.perform(context) - if not robots_str: - return {} - - multirobots = {} - for robot_entry in robots_str.split(';'): - key_val = robot_entry.strip().split('=') - if len(key_val) != 2: - continue - - robot_name, pose_str = key_val[0].strip(), key_val[1].strip() - robot_pose = yaml.safe_load(pose_str) - # Set default values if not provided - robot_pose.setdefault('x', 0.0) - robot_pose.setdefault('y', 0.0) - robot_pose.setdefault('z', 0.0) - robot_pose.setdefault('roll', 0.0) - robot_pose.setdefault('pitch', 0.0) - robot_pose.setdefault('yaw', 0.0) - multirobots[robot_name] = robot_pose - return multirobots From 8ad7329b68d22f19b7cc686d2832256e0d5f7e6b Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Fri, 21 Mar 2025 20:55:37 +0000 Subject: [PATCH 3/3] Bump version to break cache. Signed-off-by: Leander Stephen D'Souza --- .circleci/config.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index c52e5a26e57..1fa5c7b80ef 100644 --- 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\ -\ @@ -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 }}\