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(),
),
])
