def group(self, ns=None, if_arg=None, unless_arg=None): ''' Group the next nodes / entities into - another namespace - a if / unless condition depending on some argument ''' # save current entity index prev_index = self.index self.entities.append([]) self.index = len(self.entities) - 1 if ns is not None: self.entities[-1].append(PushRosNamespace(ns)) try: yield self finally: new_entities = self.entities[self.index] # restore state self.index = prev_index condition = None # get condition if if_arg is not None: condition = IfCondition(self.arg(if_arg)) elif unless_arg is not None: condition = UnlessCondition(self.arg(unless_arg)) # add new entities as sub-group self.entity(GroupAction(new_entities, condition=condition))
def test_load_node_with_namespace_in_group(mock_component_container): """Test loading a node with namespace scoped to a group.""" context = _assert_launch_no_errors([ GroupAction( [ PushRosNamespace('foo'), _load_composable_node( package='foo_package', plugin='bar_plugin', name='test_node_name', namespace='test_node_namespace' ), ], scoped=True, ), ]) # Check that launch is aware of loaded component assert get_node_name_count(context, '/foo/test_node_namespace/test_node_name') == 1 # Check that container recieved correct request assert len(mock_component_container.requests) == 1 request = mock_component_container.requests[0] assert request.package_name == 'foo_package' assert request.plugin_name == 'bar_plugin' assert request.node_name == 'test_node_name' assert request.node_namespace == '/foo/test_node_namespace' assert len(request.remap_rules) == 0 assert len(request.parameters) == 0 assert len(request.extra_arguments) == 0
def __init__(self, namespace=''): ''' Initializes entities in the given workspace ''' self.entities = [[]] self.index = 0 self.composed = False if namespace: self.entity(PushRosNamespace(namespace))
def test_push_ros_namespace(config): lc = LaunchContext() if config.push_ns is not None: pns1 = PushRosNamespace(config.push_ns) pns1.execute(lc) if config.second_push_ns is not None: pns2 = PushRosNamespace(config.second_push_ns) pns2.execute(lc) node = Node( package='dont_care', executable='whatever', node_namespace=config.node_ns, ) node._perform_substitutions(lc) expected_ns = config.expected_ns if config.expected_ns is not None else '' assert expected_ns == node.expanded_node_namespace
def test_push_ros_namespace_with_composable_node(config): lc = MockContext() if config.push_ns is not None: pns1 = PushRosNamespace(config.push_ns) pns1.execute(lc) if config.second_push_ns is not None: pns2 = PushRosNamespace(config.second_push_ns) pns2.execute(lc) node_description = ComposableNode( package='asd', plugin='my_plugin', namespace=config.node_ns, name=config.node_name, ) request = get_composable_node_load_request(node_description, lc) expected_ns = config.expected_ns if config.expected_ns is not None else '' assert expected_ns == request.node_namespace
def test_push_ros_namespace(config): lc = LaunchContext() if config.push_ns is not None: pns1 = PushRosNamespace(config.push_ns) pns1.execute(lc) if config.second_push_ns is not None: pns2 = PushRosNamespace(config.second_push_ns) pns2.execute(lc) node = Node(package='dont_care', executable='whatever', namespace=config.node_ns, name=config.node_name) node._perform_substitutions(lc) expected_ns = (config.expected_ns if config.expected_ns is not None else Node.UNSPECIFIED_NODE_NAMESPACE) expected_name = (config.node_name if config.node_name is not None else Node.UNSPECIFIED_NODE_NAME) expected_fqn = expected_ns.rstrip('/') + '/' + expected_name assert expected_ns == node.expanded_node_namespace assert expected_fqn == node.node_name
def generate_launch_description(): marta_launch_dir = os.path.join( get_package_share_directory('marta_launch'), 'launch') stereo_ns = LaunchConfiguration('stereo_ns') declare_stereo_ns_cmd = DeclareLaunchArgument( name='stereo_ns', default_value='/loc_cam', description= 'Namespace of camera in which topics ~/left/image_raw and ~/right/image_raw are published.' ) namespace = LaunchConfiguration('namespace') declare_ns_cmd = DeclareLaunchArgument(name='namespace', default_value='/test', description='Test Namespace') ld = LaunchDescription([ declare_stereo_ns_cmd, declare_ns_cmd, # Pushing of Namespace does not push back the composable nodes launched in stereo_image_proc GroupAction([ PushRosNamespace(namespace), IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(marta_launch_dir, 'stereo_image_proc.launch.py')), launch_arguments={ # 'namespace': stereo_ns, # 'use_sim_time': False, # 'use_system_default_qos': True, # 'approximate_sync': True }.items()) ]) ]) return ld
def generate_launch_description(): runtime_share = get_package_share_directory('robot_runtime') teleop_params_file = os.path.join(runtime_share, 'config', 'teleop_xbox.config.yaml') use_base_driver = IfCondition(LaunchConfiguration('base_driver')) standard_params = {'use_sim_time': LaunchConfiguration('use_sim_time')} base_device = LaunchConfiguration('base_device') return LaunchDescription([ DeclareLaunchArgument('base_driver', default_value='true'), DeclareLaunchArgument('use_sim_time', default_value='false'), DeclareLaunchArgument('base_device', default_value='/dev/ttyUSB0'), # Base Node( package='kobuki_node', executable='kobuki_ros_node', name='kobuki_node', parameters=[standard_params, { 'device_port': base_device, }], condition=use_base_driver, output='screen', ), # Teleop Node( package='cmd_vel_mux', executable='cmd_vel_mux', name='cmd_vel_mux', parameters=[standard_params], remappings=[('/cmd_vel', '/commands/velocity')], output='screen', ), GroupAction([ PushRosNamespace('joy'), Node( package='robot_indicators', executable='robot_indicators', name='xpad_led', parameters=[standard_params], output='screen', ), Node( package='robot_indicators', executable='joy_commands', name='commands', parameters=[standard_params], output='screen', ), Node( package='joy', executable='joy_node', name='driver', parameters=[standard_params], output='screen', ), Node( package='teleop_twist_joy', executable='teleop_node', name='interpreter', parameters=[ teleop_params_file, standard_params, ], output='screen', ), ]), ])
def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') urdf = os.getenv('TEST_URDF') sdf = os.getenv('TEST_SDF') bt_xml_file = os.path.join( get_package_share_directory('nav2_bt_navigator'), 'behavior_trees', os.getenv('BT_NAVIGATOR_XML')) bringup_dir = get_package_share_directory('nav2_bringup') robot1_params_file = os.path.join( bringup_dir, # noqa: F841 'params/nav2_multirobot_params_1.yaml') robot2_params_file = os.path.join( bringup_dir, # noqa: F841 'params/nav2_multirobot_params_2.yaml') # 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 }] # Launch Gazebo server for simulation start_gazebo_cmd = ExecuteProcess(cmd=[ 'gzserver', '-s', 'libgazebo_ros_factory.so', '--minimal_comms', world ], output='screen') # Define commands for spawing the robots into Gazebo spawn_robots_cmds = [] for robot in robots: spawn_robots_cmds.append( Node(package='nav2_gazebo_spawner', executable='nav2_gazebo_spawner', output='screen', arguments=[ '--robot_name', TextSubstitution(text=robot['name']), '--robot_namespace', TextSubstitution(text=robot['name']), '--sdf', TextSubstitution(text=sdf), '-x', TextSubstitution(text=str(robot['x_pose'])), '-y', TextSubstitution(text=str(robot['y_pose'])), '-z', TextSubstitution(text=str(robot['z_pose'])) ])) # Define commands for launching the robot state publishers robot_state_pubs_cmds = [] for robot in robots: robot_state_pubs_cmds.append( Node(package='robot_state_publisher', executable='robot_state_publisher', namespace=TextSubstitution(text=robot['name']), output='screen', parameters=[{ 'use_sim_time': 'True' }], remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], arguments=[urdf])) # Define commands for launching the navigation instances nav_instances_cmds = [] for robot in robots: params_file = eval(f"{robot['name']}_params_file") group = GroupAction([ # Instances use the robot's name for namespace PushRosNamespace(robot['name']), IncludeLaunchDescription(PythonLaunchDescriptionSource( os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), launch_arguments={ 'namespace': robot['name'], 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': params_file, 'bt_xml_file': bt_xml_file, 'autostart': 'True', 'use_remappings': 'True' }.items()) ]) nav_instances_cmds.append(group) ld = LaunchDescription() ld.add_action( SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), ) ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), ) ld.add_action(start_gazebo_cmd) for spawn_robot in spawn_robots_cmds: ld.add_action(spawn_robot) for state_pub in robot_state_pubs_cmds: ld.add_action(state_pub) for nav_instance in nav_instances_cmds: ld.add_action(nav_instance) return ld
def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') launch_dir = os.path.join(bringup_dir, 'launch') # Create the launch configuration variables namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') slam = LaunchConfiguration('slam') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename') autostart = LaunchConfiguration('autostart') stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') declare_namespace_cmd = DeclareLaunchArgument( 'namespace', default_value='', description='Top-level namespace') declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', description='Whether to apply a namespace to the navigation stack') declare_slam_cmd = DeclareLaunchArgument('slam', default_value='False', description='Whether run a SLAM') declare_map_yaml_cmd = DeclareLaunchArgument( 'map', description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true') 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( 'default_bt_xml_filename', 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_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='true', description='Automatically startup the nav2 stack') # Specify the actions bringup_cmd_group = GroupAction([ PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace), IncludeLaunchDescription(PythonLaunchDescriptionSource( os.path.join(launch_dir, 'slam_launch.py')), condition=IfCondition(slam), launch_arguments={ 'namespace': namespace, 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file }.items()), IncludeLaunchDescription(PythonLaunchDescriptionSource( os.path.join(launch_dir, 'localization_launch.py')), condition=IfCondition( PythonExpression(['not ', slam])), launch_arguments={ 'namespace': namespace, 'map': map_yaml_file, 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file, 'use_lifecycle_mgr': 'false' }.items()), IncludeLaunchDescription(PythonLaunchDescriptionSource( os.path.join(launch_dir, 'navigation_launch.py')), launch_arguments={ 'namespace': namespace, 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file, 'default_bt_xml_filename': default_bt_xml_filename, 'use_lifecycle_mgr': 'false', 'map_subscribe_transient_local': 'true' }.items()), ]) # Create the launch description and populate ld = LaunchDescription() # Set environment variables ld.add_action(stdout_linebuf_envvar) # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_bt_xml_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) return ld
def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') launch_dir = os.path.join(bringup_dir, 'launch') # Create the launch configuration variables namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') bt_xml_file = LaunchConfiguration('bt_xml_file') autostart = LaunchConfiguration('autostart') use_remappings = LaunchConfiguration('use_remappings') stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1') declare_namespace_cmd = DeclareLaunchArgument( 'namespace', default_value='', description='Top-level namespace') declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', description='Whether to apply a namespace to the navigation stack') # Declare the launch arguments declare_map_yaml_cmd = DeclareLaunchArgument( 'map', description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true') 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_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='true', description='Automatically startup the nav2 stack') declare_use_remappings_cmd = DeclareLaunchArgument( 'use_remappings', default_value='false', description='Arguments to pass to all nodes launched by the file') # Specify the actions bringup_cmd_group = GroupAction([ PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace), IncludeLaunchDescription(PythonLaunchDescriptionSource( os.path.join(launch_dir, 'nav2_localization_launch.py')), launch_arguments={ 'namespace': namespace, 'map': map_yaml_file, 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file, 'use_lifecycle_mgr': 'false', 'use_remappings': use_remappings }.items()), IncludeLaunchDescription(PythonLaunchDescriptionSource( os.path.join(launch_dir, 'nav2_navigation_launch.py')), launch_arguments={ 'namespace': namespace, 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file, 'bt_xml_file': bt_xml_file, 'use_lifecycle_mgr': 'false', 'use_remappings': use_remappings, 'map_subscribe_transient_local': 'true' }.items()), Node(package='nav2_lifecycle_manager', node_executable='lifecycle_manager', node_name='lifecycle_manager', output='screen', parameters=[{ 'use_sim_time': use_sim_time }, { 'autostart': autostart }, { 'node_names': [ 'map_server', 'amcl', 'controller_server', 'planner_server', 'recoveries_server', 'bt_navigator', 'waypoint_follower' ] }]), ]) # Create the launch description and populate ld = LaunchDescription() # Set environment variables ld.add_action(stdout_linebuf_envvar) # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_bt_xml_cmd) ld.add_action(declare_use_remappings_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) return ld
def generate_launch_description(): composable_nodes = [ ComposableNode( package='stereo_image_proc', plugin='stereo_image_proc::DisparityNode', parameters=[{ 'approximate_sync': LaunchConfiguration('approximate_sync'), 'use_system_default_qos': LaunchConfiguration('use_system_default_qos'), 'stereo_algorithm': LaunchConfiguration('stereo_algorithm'), 'prefilter_size': LaunchConfiguration('prefilter_size'), 'prefilter_cap': LaunchConfiguration('prefilter_cap'), 'correlation_window_size': LaunchConfiguration('correlation_window_size'), 'min_disparity': LaunchConfiguration('min_disparity'), 'disparity_range': LaunchConfiguration('disparity_range'), 'texture_threshold': LaunchConfiguration('texture_threshold'), 'speckle_size': LaunchConfiguration('speckle_size'), 'speckle_range': LaunchConfiguration('speckle_range'), 'disp12_max_diff': LaunchConfiguration('disp12_max_diff'), 'uniqueness_ratio': LaunchConfiguration('uniqueness_ratio'), 'P1': LaunchConfiguration('P1'), 'P2': LaunchConfiguration('P2'), 'full_dp': LaunchConfiguration('full_dp'), }], remappings=[ ('left/image_rect', [LaunchConfiguration('left_namespace'), '/image_rect']), ('left/camera_info', [LaunchConfiguration('left_namespace'), '/camera_info']), ('right/image_rect', [LaunchConfiguration('right_namespace'), '/image_rect']), ('right/camera_info', [LaunchConfiguration('right_namespace'), '/camera_info']), ]), ComposableNode( package='stereo_image_proc', plugin='stereo_image_proc::PointCloudNode', parameters=[{ 'approximate_sync': LaunchConfiguration('approximate_sync'), 'avoid_point_cloud_padding': LaunchConfiguration('avoid_point_cloud_padding'), 'use_color': LaunchConfiguration('use_color'), 'use_system_default_qos': LaunchConfiguration('use_system_default_qos'), }], remappings=[ ('left/camera_info', [LaunchConfiguration('left_namespace'), '/camera_info']), ('right/camera_info', [LaunchConfiguration('right_namespace'), '/camera_info']), ('left/image_rect_color', [LaunchConfiguration('left_namespace'), '/image_rect_color']), ]), ] return LaunchDescription([ DeclareLaunchArgument( name='approximate_sync', default_value='False', description= 'Whether to use approximate synchronization of topics. Set to true if ' 'the left and right cameras do not produce exactly synced timestamps.' ), DeclareLaunchArgument( name='avoid_point_cloud_padding', default_value='False', description='Avoid alignment padding in the generated point cloud.' 'This reduces bandwidth requirements, as the point cloud size is halved.' 'Using point clouds without alignment padding might degrade performance ' 'for some algorithms.'), DeclareLaunchArgument( name='use_color', default_value='True', description='Generate point cloud with rgb data.'), DeclareLaunchArgument( name='use_system_default_qos', default_value='False', description= 'Use the RMW QoS settings for the image and camera info subscriptions.' ), DeclareLaunchArgument( name='launch_image_proc', default_value='True', description= 'Whether to launch debayer and rectify nodes from image_proc.'), DeclareLaunchArgument(name='left_namespace', default_value='left', description='Namespace for the left camera'), DeclareLaunchArgument(name='right_namespace', default_value='right', description='Namespace for the right camera'), DeclareLaunchArgument( name='container', default_value='', description= ('Name of an existing node container to load launched nodes into. ' 'If unset, a new container will be created.')), # Stereo algorithm parameters DeclareLaunchArgument( name='stereo_algorithm', default_value='0', description= 'Stereo algorithm: Block Matching (0) or Semi-Global Block Matching (1)' ), DeclareLaunchArgument( name='prefilter_size', default_value='9', description='Normalization window size in pixels (must be odd)'), DeclareLaunchArgument(name='prefilter_cap', default_value='31', description='Bound on normalized pixel values'), DeclareLaunchArgument( name='correlation_window_size', default_value='15', description='SAD correlation window width in pixels (must be odd)' ), DeclareLaunchArgument( name='min_disparity', default_value='0', description='Disparity to begin search at in pixels'), DeclareLaunchArgument( name='disparity_range', default_value='64', description= 'Number of disparities to search in pixels (must be a multiple of 16)' ), DeclareLaunchArgument( name='texture_threshold', default_value='10', description= 'Filter out if SAD window response does not exceed texture threshold' ), DeclareLaunchArgument( name='speckle_size', default_value='100', description='Reject regions smaller than this size in pixels'), DeclareLaunchArgument( name='speckle_range', default_value='4', description= 'Maximum allowed difference between detected disparities'), DeclareLaunchArgument( name='disp12_max_diff', default_value='0', description= 'Maximum allowed difference in the left-right disparity check in pixels ' '(Semi-Global Block Matching only)'), DeclareLaunchArgument( name='uniqueness_ratio', default_value='15.0', description= 'Filter out if best match does not sufficiently exceed the next-best match' ), DeclareLaunchArgument( name='P1', default_value='200.0', description= 'The first parameter ccontrolling the disparity smoothness ' '(Semi-Global Block Matching only)'), DeclareLaunchArgument( name='P2', default_value='400.0', description= 'The second parameter ccontrolling the disparity smoothness ' '(Semi-Global Block Matching only)'), DeclareLaunchArgument( name='full_dp', default_value='False', description= 'Run the full variant of the algorithm (Semi-Global Block Matching only)' ), ComposableNodeContainer( condition=LaunchConfigurationEquals('container', ''), package='rclcpp_components', executable='component_container', name='stereo_image_proc_container', namespace='', composable_node_descriptions=composable_nodes, ), LoadComposableNodes( condition=LaunchConfigurationNotEquals('container', ''), composable_node_descriptions=composable_nodes, target_container=LaunchConfiguration('container'), ), # If a container name is not provided, # set the name of the container launched above for image_proc nodes SetLaunchConfiguration(condition=LaunchConfigurationEquals( 'container', ''), name='container', value='stereo_image_proc_container'), GroupAction( [ PushRosNamespace(LaunchConfiguration('left_namespace')), IncludeLaunchDescription(PythonLaunchDescriptionSource([ FindPackageShare('image_proc'), '/launch/image_proc.launch.py' ]), launch_arguments={ 'container': LaunchConfiguration('container') }.items()), ], condition=IfCondition(LaunchConfiguration('launch_image_proc')), ), GroupAction( [ PushRosNamespace(LaunchConfiguration('right_namespace')), IncludeLaunchDescription(PythonLaunchDescriptionSource([ FindPackageShare('image_proc'), '/launch/image_proc.launch.py' ]), launch_arguments={ 'container': LaunchConfiguration('container') }.items()), ], condition=IfCondition(LaunchConfiguration('launch_image_proc')), ) ])
def generate_launch_description(): pkg_bringup = get_package_share_directory('nanosaur_bringup') pkg_description = get_package_share_directory('nanosaur_description') pkg_control = get_package_share_directory('nanosaur_control') # Load nanosaur configuration and check if are included extra parameters # This feature is used to avoid pass configuration from docker conf = load_config(os.path.join(pkg_bringup, 'param', 'robot.yml')) # Load namespace from robot.yml namespace_conf = os.getenv("HOSTNAME") if conf.get("multirobot", False) else "nanosaur" # Load cover_type if "cover_type" in conf: cover_type_conf = conf.get("cover_type", 'fisheye') print(f"Load cover_type from robot.xml: {cover_type_conf}") else: cover_type_conf = os.getenv("NANOSAUR_COVER_TYPE", 'fisheye') print(f"Load cover_type from ENV: {cover_type_conf}") config_common_path = LaunchConfiguration('config_common_path') namespace = LaunchConfiguration('namespace') cover_type = LaunchConfiguration('cover_type') declare_config_common_path_cmd = DeclareLaunchArgument( 'config_common_path', default_value=os.path.join(pkg_bringup, 'param', 'nanosaur.yml'), description='Path to the `nanosaur.yml` file.') declare_namespace_cmd = DeclareLaunchArgument( 'namespace', default_value=namespace_conf, description= 'Enable a namespace for multiple robot. This namespace group all nodes and topics.' ) declare_cover_type_cmd = DeclareLaunchArgument( name='cover_type', default_value=cover_type_conf, description= 'Cover type to use. Options: pi, fisheye, realsense, zedmini.') jtop_node = Node(package='jetson_stats_wrapper', namespace=namespace, executable='jtop', name='jtop', remappings=[('diagnostics', '/diagnostics')]) # System manager system_manager = Node(package='ros2_system_manager', namespace=namespace, executable='system_manager', name='system_manager') nanosaur_base_node = Node(package='nanosaur_base', namespace=namespace, executable='nanosaur_base', name='nanosaur_base', respawn=True, respawn_delay=5, parameters=[config_common_path], output='screen') # include another launch file in nanosaur namespace # https://docs.ros.org/en/foxy/How-To-Guides/Launch-file-different-formats.html description_launch = GroupAction(actions=[ # push-ros-namespace to set namespace of included nodes PushRosNamespace(namespace), # Nanosaur description IncludeLaunchDescription(PythonLaunchDescriptionSource( [pkg_description, '/launch/description.launch.py']), launch_arguments={'cover_type': cover_type }.items()) ]) # include another launch file in nanosaur namespace twist_control_launch = GroupAction(actions=[ # push-ros-namespace to set namespace of included nodes PushRosNamespace(namespace), # nanosaur twist launch IncludeLaunchDescription( PythonLaunchDescriptionSource( [pkg_control, '/launch/twist_control.launch.py'])) ]) joy2eyes_node = Node(package='nanosaur_base', namespace="teleop", executable='joy2eyes', name='joy2eyes', remappings=[('eyes', f'/{namespace_conf}/eyes')], output='screen') system_manager_node = Node(package='ros2_system_manager', namespace="teleop", executable='joy2sm', name='joy2sm', parameters=[config_common_path], remappings=[('shutdown', f'/{namespace_conf}/shutdown')], output='screen') teleop_extra_node_actions = [ # push-ros-namespace to set namespace of included nodes PushRosNamespace(namespace), # nanosaur twist launch system_manager_node ] # Extra Debug packages # - Eyes bridge if conf.get("debug", False): print("DEBUG variable exist - Load extra nodes") teleop_extra_node_actions += [joy2eyes_node] # include another launch file in nanosaur namespace teleop_extra_nodes_launch = GroupAction(actions=teleop_extra_node_actions) # https://answers.ros.org/question/306935/ros2-include-a-launch-file-from-a-launch-file/ # include another launch file in the chatter_ns namespace teleop_launch = GroupAction(actions=[ # push-ros-namespace to set namespace of included nodes PushRosNamespace(namespace), # teleoperation launcher IncludeLaunchDescription(PythonLaunchDescriptionSource( [pkg_control, '/launch/teleop.launch.py']), launch_arguments={ 'joy_vel': f"/{namespace_conf}/joy_vel", 'config_filepath': os.path.join(pkg_control, 'config', 'ps3.nanosaur.yml') }.items()) ]) print(f"----- cover_type: {cover_type} -----") # Define LaunchDescription variable and return it ld = LaunchDescription() # Namespace nanosaur ld.add_action(declare_namespace_cmd) # cover_type ld.add_action(declare_cover_type_cmd) # Nanosaur parameter yml file path ld.add_action(declare_config_common_path_cmd) # Nanosaur description launch ld.add_action(description_launch) # jtop node ld.add_action(jtop_node) # System manager ld.add_action(system_manager) # Nanusaur driver motors and display ld.add_action(nanosaur_base_node) # Twist control launcher if conf.get("no_twist_mux", False): print("Disable twist-mux") else: ld.add_action(twist_control_launch) # teleoperation joystick nanosaur # only if joystick is connected if os.path.exists("/dev/input/js0"): print("Enable Joystick") # Teleoperation control ld.add_action(teleop_launch) # Run Joystick to system_manager node ld.add_action(teleop_extra_nodes_launch) return ld
def generate_launch_description(): base_driver = LaunchConfiguration('base_driver') use_sim_time = LaunchConfiguration('use_sim_time') standard_params = {'use_sim_time': LaunchConfiguration('use_sim_time')} return LaunchDescription([ DeclareLaunchArgument('base_driver', default_value='true'), DeclareLaunchArgument('slam', default_value='false'), DeclareLaunchArgument('nav', default_value='false'), DeclareLaunchArgument('use_sim_time', default_value='false'), DeclareLaunchArgument('map_path'), # Base # -- Kobuki Base include_launch( 'hls_lfcd_lds_driver', 'hlds_laser.launch.py', cond='base_driver', launch_arguments={ 'port': '/dev/lds01', 'frame_id': 'laser_link', 'use_sim_time': use_sim_time, }.items()), include_launch( 'robot_runtime', 'description.launch.py', cond=None, launch_arguments={ **standard_params, 'joint_states': base_driver, }.items()), include_launch( 'robot_runtime', 'teleop.launch.py', launch_arguments={ 'base_driver': base_driver, 'use_sim_time': use_sim_time, }.items()), Node( package='prometheus_exporter', node_executable='prometheus_exporter', node_name='prometheus_exporter', parameters=[standard_params], output='screen', ), GroupAction([ PushRosNamespace('parking'), include_launch( 'parking', 'parking.launch.py', launch_arguments={ 'map': LaunchConfiguration('map_path'), 'use_sim_time': use_sim_time, }.items()), ]), include_launch( 'robot_runtime', 'cartographer.launch.py', cond='slam', launch_arguments={ 'use_sim_sime': use_sim_time, 'configuration_basename': 'kobuki_lds_2d.lua', }.items()), include_launch( 'robot_runtime', 'nav.launch.py', cond='nav', launch_arguments={ 'use_sim_time': use_sim_time, 'map': LaunchConfiguration('map_path'), }.items()), ])
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
def generate_launch_description(): # Get the launch directory sm_aws_warehouse_navigation_dir = get_package_share_directory( "sm_aws_warehouse_navigation") launch_dir = os.path.join(sm_aws_warehouse_navigation_dir, "launch") # Create the launch configuration variables namespace = LaunchConfiguration("namespace") use_namespace = LaunchConfiguration("use_namespace") # slam = LaunchConfiguration("slam") # map_yaml_file = LaunchConfiguration("map") use_sim_time = LaunchConfiguration("use_sim_time") # params_file = LaunchConfiguration("params_file") # default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") # autostart = LaunchConfiguration("autostart") stdout_linebuf_envvar = SetEnvironmentVariable( "RCUTILS_LOGGING_BUFFERED_STREAM", "1") declare_namespace_cmd = DeclareLaunchArgument( "namespace", default_value="", description="Top-level namespace") declare_use_namespace_cmd = DeclareLaunchArgument( "use_namespace", default_value="false", description="Whether to apply a namespace to the navigation stack", ) # declare_slam_cmd = DeclareLaunchArgument( # "slam", default_value="False", description="Whether run a SLAM" # ) # declare_map_yaml_cmd = DeclareLaunchArgument( # "map", description="Full path to map yaml file to load" # ) declare_use_sim_time_cmd = DeclareLaunchArgument( "use_sim_time", default_value="false", description="Use simulation (Gazebo) clock if true") # declare_params_file_cmd = DeclareLaunchArgument( # "params_file", # default_value=os.path.join( # sm_aws_warehouse_navigation_dir, "params", "nav2z_client", "nav2_params.yaml" # ), # description="Full path to the ROS2 parameters file to use for all launched nodes", # ) # declare_bt_xml_cmd = DeclareLaunchArgument( # "default_nav_to_pose_bt_xml", # default_value=os.path.join( # sm_aws_warehouse_navigation_dir, "params", "nav2z_client", "navigation_tree.xml" # ), # description="Full path to the behavior tree xml file to use", # ) declare_autostart_cmd = DeclareLaunchArgument( "autostart", default_value="true", description="Automatically startup the nav2 stack") # Specify the actions bringup_cmd_group = GroupAction([ PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace), # IncludeLaunchDescription( # PythonLaunchDescriptionSource(os.path.join(launch_dir, "slam_launch.py")), # condition=IfCondition(slam), # launch_arguments={ # "namespace": namespace, # "use_sim_time": use_sim_time, # "autostart": autostart, # "params_file": params_file, # }.items(), # ), # IncludeLaunchDescription( # PythonLaunchDescriptionSource(os.path.join(launch_dir, "localization_launch.py")), # condition=IfCondition(PythonExpression(["not ", slam])), # launch_arguments={ # "namespace": namespace, # "map": map_yaml_file, # "use_sim_time": use_sim_time, # "autostart": autostart, # "params_file": params_file, # "use_lifecycle_mgr": "false", # }.items(), # ), IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(launch_dir, "navigation_launch.py")), launch_arguments={ "namespace": namespace, "use_sim_time": use_sim_time, # "autostart": autostart, # "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml, "use_lifecycle_mgr": "false", "map_subscribe_transient_local": "false", }.items(), ), ]) # Create the launch description and populate ld = LaunchDescription() # Set environment variables ld.add_action(stdout_linebuf_envvar) # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) # ld.add_action(declare_slam_cmd) # ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) # ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) # ld.add_action(declare_bt_xml_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) return ld
def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') launch_dir = os.path.join(bringup_dir, 'launch') # Create the launch configuration variables namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') slam = LaunchConfiguration('slam') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') use_composition = LaunchConfiguration('use_composition') # Map fully qualified names 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 # https://github.com/ros/geometry2/issues/32 # https://github.com/ros/robot_state_publisher/pull/30 # TODO(orduno) Substitute with `PushNodeRemapping` # https://github.com/ros2/launch_ros/issues/56 remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Create our own temporary YAML files that include substitutions param_substitutions = { 'use_sim_time': use_sim_time, 'yaml_filename': map_yaml_file } configured_params = RewrittenYaml(source_file=params_file, root_key=namespace, param_rewrites=param_substitutions, convert_types=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') declare_namespace_cmd = DeclareLaunchArgument( 'namespace', default_value='', description='Top-level namespace') declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', description='Whether to apply a namespace to the navigation stack') declare_slam_cmd = DeclareLaunchArgument('slam', default_value='False', description='Whether run a SLAM') declare_map_yaml_cmd = DeclareLaunchArgument( 'map', description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true') 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_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='true', description='Automatically startup the nav2 stack') declare_use_composition_cmd = DeclareLaunchArgument( 'use_composition', default_value='True', description='Whether to use composed bringup') # Specify the actions bringup_cmd_group = GroupAction([ PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace), IncludeLaunchDescription(PythonLaunchDescriptionSource( os.path.join(launch_dir, 'slam_launch.py')), condition=IfCondition(slam), launch_arguments={ 'namespace': namespace, 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file }.items()), IncludeLaunchDescription(PythonLaunchDescriptionSource( os.path.join(launch_dir, 'localization_launch.py')), condition=IfCondition( PythonExpression(['not ', slam])), launch_arguments={ 'namespace': namespace, 'map': map_yaml_file, 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file }.items()), Node(condition=IfCondition(use_composition), package='nav2_bringup', executable='composed_bringup', output='screen', parameters=[configured_params, { 'autostart': autostart }], remappings=remappings), IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(launch_dir, 'navigation_launch.py')), condition=IfCondition(PythonExpression(['not ', use_composition])), launch_arguments={ 'namespace': namespace, 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file }.items()), ]) # Create the launch description and populate ld = LaunchDescription() # Set environment variables ld.add_action(stdout_linebuf_envvar) # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_use_composition_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) return ld
def generate_launch_description(): # Get the launch directory rover_config_dir = get_package_share_directory('rover_config') marta_launch_dir = os.path.join( get_package_share_directory('marta_launch'), 'launch') # Create the launch configuration variables namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename') autostart = LaunchConfiguration('autostart') stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1') declare_namespace_cmd = DeclareLaunchArgument( 'namespace', default_value='', description='Top-level namespace') declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', description='Whether to apply a namespace to the navigation stack') declare_map_yaml_cmd = DeclareLaunchArgument( 'map', description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true') declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(rover_config_dir, 'config', 'nav2_params.yaml'), description= 'Full path to the ROS2 parameters file to use for all launched nodes') declare_bt_xml_cmd = DeclareLaunchArgument( 'default_bt_xml_filename', default_value=os.path.join( get_package_share_directory('nav2_bt_navigator'), 'behavior_trees', 'navigate_w_replanning_distance.xml'), description='Full path to the behavior tree xml file to use') declare_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='true', description='Automatically startup the nav2 stack') # Specify the actions bringup_cmd_group = GroupAction([ PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace), IncludeLaunchDescription(PythonLaunchDescriptionSource( os.path.join(marta_launch_dir, 'nav2_localization_launch.py')), launch_arguments={ 'namespace': namespace, 'map': map_yaml_file, 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file, 'use_lifecycle_mgr': 'false' }.items()), IncludeLaunchDescription(PythonLaunchDescriptionSource( os.path.join(marta_launch_dir, 'nav2_navigation_launch.py')), launch_arguments={ 'namespace': namespace, 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file, 'default_bt_xml_filename': default_bt_xml_filename, 'use_lifecycle_mgr': 'false', 'map_subscribe_transient_local': 'true' }.items()), ]) # Create the launch description and populate return LaunchDescription([ # Set env var to print messages colored. The ANSI color codes will appear in a log. SetEnvironmentVariable('RCUTILS_COLORIZED_OUTPUT', '1'), # Declare the launch options declare_namespace_cmd, declare_use_namespace_cmd, declare_map_yaml_cmd, declare_use_sim_time_cmd, declare_params_file_cmd, declare_autostart_cmd, declare_bt_xml_cmd, # Add the actions to launch all of the navigation nodes bringup_cmd_group ])
def launch_setup(context, *args, **kwargs): # Perform substitutions debug = Lc('debug').perform(context) namespace = Lc('namespace').perform(context) x = Lc('x').perform(context) y = Lc('y').perform(context) z = Lc('z').perform(context) roll = Lc('roll').perform(context) pitch = Lc('pitch').perform(context) yaw = Lc('yaw').perform(context) # use_sim_time = Lc('use_sim_time').perform(context) use_world_ned = Lc('use_ned_frame').perform(context) # Request sim time value to the global node res = is_sim_time(return_param=False, use_subprocess=True) # Xacro #xacro_file = PathJoinSubstitution(get_package_share_directory('uuv_descriptions'),'robots','rexrov_') xacro_file = os.path.join( get_package_share_directory('uuv_descriptions'), 'robots', 'rexrov_' + (Lc('mode')).perform(context) + '.xacro') # Build the directories, check for existence path = os.path.join( get_package_share_directory('uuv_descriptions'), 'robots', 'generated', namespace, ) if not pathlib.Path(path).exists(): try: # Create directory if required and sub-directory os.makedirs(path) except OSError: print("Creation of the directory %s failed" % path) output = os.path.join(path, 'robot_description') if not pathlib.Path(xacro_file).exists(): exc = 'Launch file ' + xacro_file + ' does not exist' raise Exception(exc) mapping = {} if to_bool(use_world_ned): mappings = { 'debug': debug, 'namespace': namespace, 'inertial_reference_frame': 'world_ned' } else: mappings = { 'debug': debug, 'namespace': namespace, 'inertial_reference_frame': 'world' } doc = xacro.process(xacro_file, mappings=mappings) with open(output, 'w') as file_out: file_out.write(doc) # URDF spawner args = ('-gazebo_namespace /gazebo ' '-x %s -y %s -z %s -R %s -P %s -Y %s -entity %s -file %s' % (x, y, z, roll, pitch, yaw, namespace, output)).split() # Urdf spawner. NB: node namespace does not prefix the topic, # as using a leading / urdf_spawner = Node( node_name='urdf_spawner', package='gazebo_ros', node_executable='spawn_entity.py', output='screen', parameters=[{ 'use_sim_time': res }], # To replace in foxy with parameters=[{'robot_description', Command('ros2 run xacro...')}] arguments=args) # A joint state publisher plugin already is started with the model, no need to use the default joint state publisher args = (output).split() robot_state_publisher = Node( node_name='robot_state_publisher', package='robot_state_publisher', node_executable='robot_state_publisher', # To replace in foxy with parameters=[{'robot_description', Command('ros2 run xacro...')}] arguments=args, output='screen', parameters=[{ 'use_sim_time': res }] # Use subst here ) # Message to tf message_to_tf_launch = os.path.join( get_package_share_directory('uuv_assistants'), 'launch', 'message_to_tf.launch') if not pathlib.Path(message_to_tf_launch).exists(): exc = 'Launch file ' + message_to_tf_launch + ' does not exist' raise Exception(exc) launch_args = [('namespace', namespace), ('world_frame', 'world'), ('child_frame_id', '/' + namespace + '/base_link')] message_to_tf_launch = IncludeLaunchDescription( AnyLaunchDescriptionSource(message_to_tf_launch), launch_arguments=launch_args) group = GroupAction([ PushRosNamespace(namespace), urdf_spawner, robot_state_publisher, ]) return [group, message_to_tf_launch]
def generate_robot_launch_description(robot_namespace: str, simulation=False): """Generate robot launch description based on namespace.""" map = LaunchConfiguration("map") namespace = LaunchConfiguration("namespace") use_namespace = LaunchConfiguration("use_namespace") use_sim_time = LaunchConfiguration("use_sim_time") autostart = LaunchConfiguration("autostart") default_bt_xml_filename = LaunchConfiguration("default_bt_xml_filename") params = tempfile.NamedTemporaryFile(mode="w", delete=False) robot_params = os.path.join(get_package_share_directory(robot_namespace), "param", f"{robot_namespace}.yml") navigation_params = os.path.join(get_package_share_directory("robot"), "param", "robot.yml") merged_params = hiyapyco.load([navigation_params, robot_params], method=hiyapyco.METHOD_MERGE) params.file.write(hiyapyco.dump(merged_params)) urdf = os.path.join(get_package_share_directory(robot_namespace), "robot", f"{robot_namespace}.urdf") robot_launch_file_dir = os.path.dirname(__file__) nav2_bt_xml_file = os.path.join( get_package_share_directory("robot"), "behavior_trees", "navigate_w_replanning_time.xml", ) remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] return launch.LaunchDescription([ DeclareLaunchArgument( "namespace", default_value=robot_namespace, description="Robot global namespace", ), DeclareLaunchArgument( "use_namespace", default_value="true", description= "Whether to apply a namespace to the robot stack including navigation2", ), DeclareLaunchArgument("autostart", default_value="true", description="Autostart the nav stack"), DeclareLaunchArgument("use_sim_time", default_value="false", description="Use /clock if true"), DeclareLaunchArgument( "default_bt_xml_filename", default_value=nav2_bt_xml_file, description="Full path to the behavior tree xml file to use", ), GroupAction([ PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace), Node( package="lcd_driver", executable="lcd_driver", output="screen", parameters=[params.name], remappings=remappings, ), Node( package="robot_state_publisher", executable="robot_state_publisher", output="screen", parameters=[params.name], arguments=[urdf], remappings=remappings, ), Node( package="localisation", executable="localisation", output="screen", parameters=[params.name], remappings=remappings, ), Node( package="cetautomatix", executable="cetautomatix", output="screen", parameters=[params.name], remappings=remappings, ), Node( package="teb_obstacles", executable="teb_obstacles", output="screen", parameters=[], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( [robot_launch_file_dir, "/navigation_launch.py"]), launch_arguments={ "autostart": autostart, "namespace": namespace, "use_sim_time": use_sim_time, "default_bt_xml_filename": default_bt_xml_filename, "params_file": params.name, }.items(), ), ]), IncludeLaunchDescription( PythonLaunchDescriptionSource( [robot_launch_file_dir, "/interfaces.py"]), condition=IfCondition(str(not simulation)), launch_arguments={ "namespace": namespace, "use_namespace": use_namespace, "use_sim_time": use_sim_time, "params_file": params.name, }.items(), ), ])