def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('amazon_robot_bringup') # Create the launch configuration variables namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') rviz_config_file = LaunchConfiguration('rviz_config') # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( 'namespace', default_value='navigation', description=('Top-level namespace. The value will be used to replace the ' '<robot_namespace> keyword on the rviz config file.')) declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', description='Whether to apply a namespace to the navigation stack') declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'amazon_robot_default_view.rviz'), description='Full path to the RVIZ config file to use') # Launch rviz start_rviz_cmd = Node( condition=UnlessCondition(use_namespace), package='rviz2', executable='rviz2', name='rviz2', arguments=['-d', rviz_config_file], output='screen') namespaced_rviz_config_file = ReplaceString( source_file=rviz_config_file, replacements={'<robot_namespace>': ('/', namespace)}) start_namespaced_rviz_cmd = Node( condition=IfCondition(use_namespace), package='rviz2', executable='rviz2', name='rviz2', namespace=namespace, arguments=['-d', namespaced_rviz_config_file], output='screen', remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static'), ('/goal_pose', 'goal_pose'), ('/clicked_point', 'clicked_point'), ('/initialpose', 'initialpose')]) exit_event_handler = RegisterEventHandler( condition=UnlessCondition(use_namespace), event_handler=OnProcessExit( target_action=start_rviz_cmd, on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) exit_event_handler_namespaced = RegisterEventHandler( condition=IfCondition(use_namespace), event_handler=OnProcessExit( target_action=start_namespaced_rviz_cmd, on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) # Create the launch description and populate ld = LaunchDescription() # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_rviz_config_file_cmd) # Add any conditioned actions ld.add_action(start_rviz_cmd) ld.add_action(start_namespaced_rviz_cmd) # Add other nodes and processes we need ld.add_action(exit_event_handler) ld.add_action(exit_event_handler_namespaced) return ld
def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') # Names and poses of the robots robots = [{ 'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01 }, { 'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01 }] # Simulation settings world = LaunchConfiguration('world') simulator = LaunchConfiguration('simulator') # On this example all robots are launched with the same settings map_yaml_file = LaunchConfiguration('map') params_file = LaunchConfiguration('params_file') bt_xml_file = LaunchConfiguration('bt_xml_file') rviz_config_file = LaunchConfiguration('rviz_config') log_settings = LaunchConfiguration('log_settings', default='true') # Declare the launch arguments declare_world_cmd = DeclareLaunchArgument( 'world', default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), description='Full path to world file to load') declare_simulator_cmd = DeclareLaunchArgument( 'simulator', default_value='gazebo', description='The simulator to use (gazebo or gzserver)') declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), description='Full path to map file to load') declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), description= 'Full path to the ROS2 parameters file to use for all launched nodes') declare_bt_xml_cmd = DeclareLaunchArgument( 'bt_xml_file', default_value=os.path.join( get_package_share_directory('nav2_bt_navigator'), 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), description='Full path to the behavior tree xml file to use') declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), description='Full path to the RVIZ config file to use') # Start Gazebo with plugin providing the robot spawing service start_gazebo_cmd = ExecuteProcess( cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', world], output='screen') # Define commands for spawing the robots into Gazebo spawn_robots_cmds = [] for robot in robots: spawn_robots_cmds.append( IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(bringup_dir, 'launch', 'spawn_robot_launch.py')), launch_arguments={ 'x_pose': TextSubstitution(text=str(robot['x_pose'])), 'y_pose': TextSubstitution(text=str(robot['y_pose'])), 'z_pose': TextSubstitution(text=str(robot['z_pose'])), 'robot_name': robot['name'], 'turtlebot_type': TextSubstitution(text='waffle') }.items())) # Define commands for launching the navigation instances nav_instances_cmds = [] for robot in robots: namespaced_rviz_config_file = ReplaceString( source_file=rviz_config_file, replacements={'<robot_namespace>': ('/' + robot['name'])}) group = GroupAction([ # TODO(orduno) # Each `action.Node` within the `localization` and `navigation` launch # files has two versions, one with the required remaps and another without. # The `use_remappings` flag specifies which runs. # A better mechanism would be to have a PushNodeRemapping() action: # https://github.com/ros2/launch_ros/issues/56 # For more on why we're remapping topics, see the note below # PushNodeRemapping(remappings) # Instances use the robot's name for namespace PushRosNamespace(robot['name']), IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(bringup_dir, 'launch', 'nav2_tb3_simulation_launch.py')), launch_arguments={ # TODO(orduno) might not be necessary to pass the robot name 'namespace': robot['name'], 'map_yaml_file': map_yaml_file, 'use_sim_time': 'True', 'params_file': params_file, 'bt_xml_file': bt_xml_file, 'autostart': 'False', 'use_remappings': 'True', 'rviz_config_file': namespaced_rviz_config_file, 'use_simulator': 'False' }.items()), LogInfo(condition=IfCondition(log_settings), msg=['Launching ', robot['name']]), LogInfo(condition=IfCondition(log_settings), msg=[robot['name'], ' map yaml: ', map_yaml_file]), LogInfo(condition=IfCondition(log_settings), msg=[robot['name'], ' params yaml: ', params_file]), LogInfo(condition=IfCondition(log_settings), msg=[robot['name'], ' behavior tree xml: ', bt_xml_file]), LogInfo(condition=IfCondition(log_settings), msg=[ robot['name'], ' rviz config file: ', namespaced_rviz_config_file ]) ]) nav_instances_cmds.append(group) # A note on the `remappings` variable defined above and the fact it's passed as a node arg. # A few topics have fully qualified names (have a leading '/'), these need to be remapped # to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative # for multi-robot transforms: # https://github.com/ros/geometry2/issues/32 # https://github.com/ros/robot_state_publisher/pull/30 # Create the launch description and populate ld = LaunchDescription() # Declare the launch options ld.add_action(declare_simulator_cmd) ld.add_action(declare_world_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_bt_xml_cmd) ld.add_action(declare_rviz_config_file_cmd) # Add the actions to start gazebo, robots and simulations ld.add_action(start_gazebo_cmd) for spawn_robot_cmd in spawn_robots_cmds: ld.add_action(spawn_robot_cmd) for simulation_instance_cmd in nav_instances_cmds: ld.add_action(simulation_instance_cmd) return ld