From 43d7f7e309e782f1e277191ff414c98b9ac4f51c Mon Sep 17 00:00:00 2001 From: Gilbert Tanner Date: Tue, 7 Jan 2025 22:39:06 +0100 Subject: [PATCH] fix: Changed cloned_multi_tb3_simulation_launch.py file to conform with common ROS 2 launch syntax (#4811) * Changed cloned_multi_tb3_simulation_launch.py file to conform with common ROS 2 launch syntax and thereby allow for including it in other launch files as expected Signed-off-by: Tanner, Gilbert * chore: Changed formatting to conform to flake8 linting rules Signed-off-by: Tanner, Gilbert chore: Changed formatting to conform to flake8 linting rules II Signed-off-by: Tanner, Gilbert chore: Changed formatting to conform to flake8 linting rules III Signed-off-by: Tanner, Gilbert chore: Changed formatting to conform to flake8 linting rules IV Signed-off-by: Tanner, Gilbert * Changed ParseMultiRobotPose to be a launch substitution class Signed-off-by: Tanner, Gilbert * chore: Changed formatting to conform to flake8 linting rules II Signed-off-by: Tanner, Gilbert * Changed formatting based on PR feedback Signed-off-by: Tanner, Gilbert * Undo removal of position parsing options Signed-off-by: Tanner, Gilbert --------- Signed-off-by: Tanner, Gilbert --- .../cloned_multi_tb3_simulation_launch.py | 136 ++++++++++-------- .../launch/parse_multirobot_pose.py | 85 +++++------ 2 files changed, 111 insertions(+), 110 deletions(-) diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index 6bd6bdddca..ecffa7771c 100644 --- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -37,6 +37,69 @@ from nav2_common.launch import ParseMultiRobotPose +def generate_robot_actions(context, *args, **kwargs): + bringup_dir = get_package_share_directory('nav2_bringup') + launch_dir = os.path.join(bringup_dir, 'launch') + use_rviz = LaunchConfiguration('use_rviz') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + rviz_config_file = LaunchConfiguration('rviz_config') + 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( + [ + LogInfo( + msg=['Launching namespace=', robot_name, ' init_pose=', str(init_pose),] + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py') + ), + condition=IfCondition(use_rviz), + launch_arguments={ + 'namespace': TextSubstitution(text=robot_name), + 'rviz_config': rviz_config_file, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py') + ), + launch_arguments={ + 'namespace': robot_name, + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_rviz': 'False', + '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), + }.items(), + ), + ] + ) + + bringup_cmd_group.append(group) + bringup_cmd_group.append(LogInfo(msg=['number_of_robots=', str(len(robots_list))])) + return bringup_cmd_group + + def generate_launch_description(): """ Bring up the multi-robots with given launch arguments. @@ -49,7 +112,6 @@ def generate_launch_description(): """ # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') - launch_dir = os.path.join(bringup_dir, 'launch') sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') # Simulation settings @@ -61,7 +123,6 @@ def generate_launch_description(): autostart = LaunchConfiguration('autostart') rviz_config_file = LaunchConfiguration('rviz_config') use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') - use_rviz = LaunchConfiguration('use_rviz') log_settings = LaunchConfiguration('log_settings', default='true') # Declare the launch arguments @@ -71,6 +132,13 @@ def generate_launch_description(): description='Full path to world file to load', ) + 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}""", + description='Robots and their initialization poses in YAML format', + ) + declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'), @@ -93,7 +161,8 @@ def generate_launch_description(): declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', - default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), + default_value=os.path.join( + bringup_dir, 'rviz', 'nav2_default_view.rviz'), description='Full path to the RVIZ config file to use.', ) @@ -121,60 +190,6 @@ def generate_launch_description(): OpaqueFunction(function=lambda _: os.remove(world_sdf)) ])) - robots_list = ParseMultiRobotPose('robots').value() - - # Define commands for launching the navigation instances - bringup_cmd_group = [] - for robot_name in robots_list: - init_pose = robots_list[robot_name] - group = GroupAction( - [ - LogInfo( - msg=[ - 'Launching namespace=', - robot_name, - ' init_pose=', - str(init_pose), - ] - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py') - ), - condition=IfCondition(use_rviz), - launch_arguments={ - 'namespace': TextSubstitution(text=robot_name), - 'rviz_config': rviz_config_file, - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py') - ), - launch_arguments={ - 'namespace': robot_name, - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': params_file, - 'autostart': autostart, - 'use_rviz': 'False', - '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), - }.items(), - ), - ] - ) - - bringup_cmd_group.append(group) - set_env_vars_resources = AppendEnvironmentVariable( 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')) set_env_vars_resources2 = AppendEnvironmentVariable( @@ -188,6 +203,7 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_world_cmd) + ld.add_action(declare_robots_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_use_rviz_cmd) @@ -200,8 +216,6 @@ def generate_launch_description(): ld.add_action(start_gazebo_cmd) ld.add_action(remove_temp_sdf_file) - ld.add_action(LogInfo(msg=['number_of_robots=', str(len(robots_list))])) - ld.add_action( LogInfo(condition=IfCondition(log_settings), msg=['map yaml: ', map_yaml_file]) ) @@ -223,8 +237,6 @@ def generate_launch_description(): ld.add_action( LogInfo(condition=IfCondition(log_settings), msg=['autostart: ', autostart]) ) - - for cmd in bringup_cmd_group: - ld.add_action(cmd) + ld.add_action(OpaqueFunction(function=generate_robot_actions)) return ld diff --git a/nav2_common/nav2_common/launch/parse_multirobot_pose.py b/nav2_common/nav2_common/launch/parse_multirobot_pose.py index 9207cad5f7..ced8352ac4 100644 --- a/nav2_common/nav2_common/launch/parse_multirobot_pose.py +++ b/nav2_common/nav2_common/launch/parse_multirobot_pose.py @@ -12,66 +12,55 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys from typing import Dict, Text +import launch +from launch.launch_context import LaunchContext import yaml -class ParseMultiRobotPose: - """Parsing argument using sys module.""" +class ParseMultiRobotPose(launch.Substitution): + """ + A custom substitution to parse the robots argument for multi-robot poses. - def __init__(self, target_argument: Text): - """ - Parse arguments for multi-robot's pose. + 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}"` - for example, - `ros2 launch nav2_bringup bringup_multirobot_launch.py - 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. + """ - `target_argument` shall be 'robots'. - Then, this will parse a string value for `robots` argument. + def __init__(self, robots_argument: launch.SomeSubstitutionsType) -> None: + super().__init__() + self.__robots_argument = robots_argument - Each robot name which corresponds to namespace and pose of it will be separated by `;`. - The pose consists of x, y and yaw with YAML format. - - :param: target argument name to parse - """ - self.__args: Text = self.__parse_argument(target_argument) - - def __parse_argument(self, target_argument: Text) -> Text: - """Get value of target argument.""" - if len(sys.argv) > 4: - argv = sys.argv[4:] - for arg in argv: - if arg.startswith(target_argument + ':='): - return arg.replace(target_argument + ':=', '') + def describe(self) -> Text: + """Return a description of this substitution as a string.""" return '' - def value(self) -> Dict: - """Get value of target argument.""" - args = self.__args - parsed_args = [] if len(args) == 0 else args.split(';') + 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 arg in parsed_args: - key_val = arg.strip().split('=') + for robot_entry in robots_str.split(';'): + key_val = robot_entry.strip().split('=') if len(key_val) != 2: continue - key = key_val[0].strip() - val = key_val[1].strip() - robot_pose = yaml.safe_load(val) - if 'x' not in robot_pose: - robot_pose['x'] = 0.0 - if 'y' not in robot_pose: - robot_pose['y'] = 0.0 - if 'z' not in robot_pose: - robot_pose['z'] = 0.0 - if 'roll' not in robot_pose: - robot_pose['roll'] = 0.0 - if 'pitch' not in robot_pose: - robot_pose['pitch'] = 0.0 - if 'yaw' not in robot_pose: - robot_pose['yaw'] = 0.0 - multirobots[key] = robot_pose + + 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