def test_launch_configuration_equals():
"""Test LaunchConfigurationEquals class."""
class MockLaunchContext:
def perform_substitution(self, substitution):
return substitution.perform(self)
@property
def launch_configurations(self):
return {
'foo': 'foo_value',
'bar': 'bar_value',
'empty': '',
}
lc = MockLaunchContext()
test_cases = [
('foo', 'foo_value', True),
('bar', 'bar_value', True),
('bar', 'foo_value', False),
('bar', None, False),
('empty', '', True),
('empty', 'foo_value', False),
('empty', None, False),
('baz', None, True),
('baz', 'foo_value', False),
]
for name, value, expected in test_cases:
assert LaunchConfigurationEquals(
name, [TextSubstitution(text=value)]
if value is not None else None).evaluate(lc) is expected
def generate_launch_description():
composable_nodes = [
ComposableNode(
package='image_proc',
plugin='image_proc::DebayerNode',
name='debayer_node',
),
ComposableNode(
package='image_proc',
plugin='image_proc::RectifyNode',
name='rectify_mono_node',
# Remap subscribers and publishers
remappings=[('image', 'image_mono'),
('camera_info', 'camera_info'),
('image_rect', 'image_rect')],
),
ComposableNode(
package='image_proc',
plugin='image_proc::RectifyNode',
name='rectify_color_node',
# Remap subscribers and publishers
remappings=[('image', 'image_color'),
('image_rect', 'image_rect_color')],
)
]
arg_container = 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.'))
# If an existing container is not provided, start a container and load nodes into it
image_processing_container = ComposableNodeContainer(
condition=LaunchConfigurationEquals('container', ''),
name='image_proc_container',
namespace='',
package='rclcpp_components',
executable='component_container',
composable_node_descriptions=composable_nodes,
output='screen')
# If an existing container name is provided, load composable nodes into it
# This will block until a container with the provided name is available and nodes are loaded
load_composable_nodes = LoadComposableNodes(
condition=LaunchConfigurationNotEquals('container', ''),
composable_node_descriptions=composable_nodes,
target_container=LaunchConfiguration('container'),
)
return LaunchDescription([
arg_container,
image_processing_container,
load_composable_nodes,
])
def generate_launch_description():
def add_launch_arg(name: str, default_value=None):
return DeclareLaunchArgument(name, default_value=default_value)
nodes = [
ComposableNode(
package="ground_segmentation",
plugin="ground_segmentation::ScanGroundFilterComponent",
name="scan_ground_filter",
remappings=[
("input", LaunchConfiguration("input/pointcloud")),
("output", LaunchConfiguration("output/pointcloud")),
],
parameters=[{
"global_slope_max_angle_deg": 10.0,
"local_slope_max_angle_deg": 30.0,
"split_points_distance_tolerance": 0.2,
"split_height_distance": 0.2,
}],
),
]
loader = LoadComposableNodes(
condition=LaunchConfigurationNotEquals("container", ""),
composable_node_descriptions=nodes,
target_container=LaunchConfiguration("container"),
)
container = ComposableNodeContainer(
name="scan_ground_filter_container",
namespace="",
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=nodes,
output="screen",
condition=LaunchConfigurationEquals("container", ""),
)
return launch.LaunchDescription([
add_launch_arg("container", ""),
add_launch_arg("input/pointcloud", "pointcloud"),
add_launch_arg("output/pointcloud", "no_ground/pointcloud"),
container,
loader,
])
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():
# Package directories
pkg_robot_navigation = get_package_share_directory('robot_navigation')
pkg_clock_publisher = get_package_share_directory('clock_publisher')
# Launch arguments
launch_arg_rosbag_file = DeclareLaunchArgument('rosbag_file')
launch_arg_rviz = DeclareLaunchArgument('rviz', default_value='true')
launch_arg_use_sim_time = DeclareLaunchArgument('use_sim_time',
default_value='true')
launch_arg_rate = DeclareLaunchArgument('rate', default_value='1')
# Currently no way to extract launch argument values with LaunchConfiguration
# launch_arg_urdf = DeclareLaunchArgument(
# 'urdf_xacro',
# description='The path to the robot .urdf.xacro file.',
# default_value=[os.path.join(get_package_share_directory('robot_description'), 'urdf', 'generic_roomba_bot.urdf.xacro')]
# )
urdf_xacro = os.path.join(get_package_share_directory('robot_description'),
'urdf', 'generic_roomba_bot.urdf.xacro')
robot_desc = urdf_xacro_reader(urdf_xacro)
# rosbag2 can only be run as an executable, not node action
process_rosbag_play_sim_time = ExecuteProcess(
cmd=[
'ros2', 'bag', 'play',
LaunchConfiguration('rosbag_file'), '--rate',
LaunchConfiguration('rate'), '--remap', '/clock:=/clock_real'
],
output='screen',
condition=LaunchConfigurationEquals('use_sim_time', 'true'))
process_rosbag_play_real_time = ExecuteProcess(
cmd=[
'ros2', 'bag', 'play',
LaunchConfiguration('rosbag_file'), '--rate',
LaunchConfiguration('rate')
],
output='screen',
condition=LaunchConfigurationEquals('use_sim_time', 'false'))
# Nodes
node_robot_state_pub = Node( # publish the urdf so that RViz gets it
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[{
'robot_description': robot_desc
}])
node_static_transform_pub = Node( # static transform publisher node to establish odom to map
package='tf2_ros',
executable='static_transform_publisher',
arguments=['0', '0', '0', '0', '0', '0', '1', 'map', 'odom'])
node_rviz = Node( # RViz node
package='rviz2',
executable='rviz2',
arguments=[
'-d',
os.path.join(pkg_robot_navigation, 'rviz',
'robot_rosbag_play.rviz')
],
condition=IfCondition(LaunchConfiguration('rviz')))
# Temporary workaround to using simulated time for rosbag playback in ros2
launch_clock_publisher = IncludeLaunchDescription(
FrontendLaunchDescriptionSource(
os.path.join(pkg_clock_publisher, 'launch',
'clock_publisher.launch.xml')),
condition=IfCondition(LaunchConfiguration('use_sim_time')))
return LaunchDescription([
launch_arg_rosbag_file, launch_arg_rviz, launch_arg_use_sim_time,
launch_arg_rate, process_rosbag_play_sim_time,
process_rosbag_play_real_time, node_static_transform_pub,
node_robot_state_pub, node_rviz, launch_clock_publisher
])
def generate_launch_description():
camera_name = 'forward_camera'
orca_bringup_dir = get_package_share_directory('orca_bringup')
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
orca_description_dir = get_package_share_directory('orca_description')
nav2_bringup_launch_dir = os.path.join(nav2_bringup_dir, 'launch')
urdf_file = os.path.join(orca_description_dir, 'urdf',
'hw7.urdf') # TODO choose urdf
teleop_params_file = os.path.join(orca_bringup_dir, 'params',
'xbox_holonomic_3d.yaml')
nav2_bt_file = os.path.join(orca_bringup_dir, 'behavior_trees',
'orca3_bt.xml')
use_sim_time = LaunchConfiguration('use_sim_time')
orca_params_file = LaunchConfiguration('orca_params_file')
nav2_params_file = LaunchConfiguration('nav2_params_file')
vlam_map_file = LaunchConfiguration('vlam_map')
nav2_map_file = LaunchConfiguration('nav2_map')
# ORB features vocabulary file
# This works well in simulation, but I'm sure how it will do in a marine environment
orb_slam_dir = get_package_share_directory('orb_slam2_ros')
orb_voc_file = os.path.join(orb_slam_dir, 'orb_slam2', 'Vocabulary',
'ORBvoc.txt')
# Read the params file and make some global substitutions
configured_orca_params = RewrittenYaml(source_file=orca_params_file,
param_rewrites={
'use_sim_time':
use_sim_time,
'marker_map_load_full_filename':
vlam_map_file,
},
convert_types=True)
configured_nav2_params = RewrittenYaml(source_file=nav2_params_file,
param_rewrites={
'use_sim_time': use_sim_time,
'yaml_filename': nav2_map_file,
},
convert_types=True)
return LaunchDescription([
SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
DeclareLaunchArgument(
'use_sim_time',
default_value='False', # TODO sim time broken
description='Use simulation (Gazebo) clock (BROKEN BROKEN BROKEN)?'
),
DeclareLaunchArgument('slam',
default_value='orb',
description='Choose SLAM strategy: ' +
', '.join(slams)),
DeclareLaunchArgument(
'vlam_map',
default_value=
'install/orca_gazebo/share/orca_gazebo/worlds/medium_ring_map.yaml',
description='Full path to Vlam map file'),
DeclareLaunchArgument(
'nav2_map',
default_value=
'install/orca_bringup/share/orca_bringup/worlds/empty_world.yaml',
description='Full path to Nav2 map file'),
DeclareLaunchArgument(
'orca_params_file',
default_value=os.path.join(orca_bringup_dir, 'params',
'orca_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for Orca nodes'),
DeclareLaunchArgument(
'nav2_params_file',
default_value=os.path.join(orca_bringup_dir, 'params',
'nav2_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for Nav2 nodes'),
# Publish static /tf
Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
name='robot_state_publisher',
arguments=[urdf_file],
parameters=[configured_orca_params]),
# Publish /joy
Node(package='joy',
executable='joy_node',
output='screen',
name='joy_node',
parameters=[configured_orca_params]),
# Subscribe to /joy and publish /cmd_vel
Node(package='teleop_twist_joy',
executable='teleop_node',
output='screen',
name='teleop_node',
parameters=[
teleop_params_file, {
'use_sim_time': LaunchConfiguration('use_sim_time'),
}
]),
# Subscribe to /cmd_vel and publish /thrusters, /odom and /tf odom->base_link
Node(package='orca_base',
executable='base_controller',
output='screen',
name='base_controller',
parameters=[configured_orca_params]),
# fiducial_vlam: publish a map of ArUco markers
Node(package='fiducial_vlam',
executable='vmap_main',
output='screen',
name='vmap_main',
parameters=[configured_orca_params],
condition=LaunchConfigurationEquals('slam', 'vlam')),
# fiducial_vlam: find ArUco markers and publish the camera pose
Node(package='fiducial_vlam',
executable='vloc_main',
output='screen',
name='vloc_main',
namespace=camera_name,
parameters=[configured_orca_params],
condition=LaunchConfigurationEquals('slam', 'vlam')),
# fiducial_vlam: subscribe to the camera pose and publish /tf map->odom
Node(package='orca_localize',
executable='fiducial_localizer',
output='screen',
name='fiducial_localizer',
parameters=[configured_orca_params],
remappings=[
('camera_pose', '/' + camera_name + '/camera_pose'),
],
condition=LaunchConfigurationEquals('slam', 'vlam')),
# orb_slam2: build a map of 3d points, localize against the map, and publish the camera pose
Node(package='orb_slam2_ros',
executable='orb_slam2_ros_stereo',
output='screen',
name='orb_slam2_stereo',
parameters=[configured_orca_params, {
'voc_file': orb_voc_file,
}],
remappings=[
('/image_left/image_color_rect', '/stereo/left/image_raw'),
('/image_right/image_color_rect', '/stereo/right/image_raw'),
('/camera/camera_info', '/stereo/left/camera_info'),
],
condition=LaunchConfigurationEquals('slam', 'orb')),
# orb_slam2: subscribe to the camera pose and publish /tf map->odom
Node(package='orca_localize',
executable='orb_slam2_localizer',
output='screen',
name='orb_slam2_localizer',
parameters=[configured_orca_params],
remappings=[
('/camera_pose', '/orb_slam2_stereo_node/pose'),
],
condition=LaunchConfigurationEquals('slam', 'orb')),
# Publish a [likely empty] nav2 map
Node(package='nav2_map_server',
executable='map_server',
name='map_server',
output='screen',
parameters=[configured_nav2_params]),
# Manage the lifecycle of map_server
Node(package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_map_server',
output='screen',
parameters=[{
'use_sim_time': use_sim_time,
'autostart': True,
'node_names': ['map_server'],
}]),
# Include the rest of Nav2
IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(nav2_bringup_launch_dir, 'navigation_launch.py')),
launch_arguments={
'use_sim_time': use_sim_time,
'autostart': 'True',
'params_file': nav2_params_file,
'map_subscribe_transient_local': 'true',
'default_bt_xml_filename': nav2_bt_file,
}.items()),
])
def generate_launch_description():
def add_launch_arg(name: str, default_value=None):
return DeclareLaunchArgument(name, default_value=default_value)
set_container_executable = SetLaunchConfiguration(
"container_executable",
"component_container",
condition=UnlessCondition(LaunchConfiguration("use_multithread")),
)
set_container_mt_executable = SetLaunchConfiguration(
"container_executable",
"component_container_mt",
condition=IfCondition(LaunchConfiguration("use_multithread")),
)
composable_nodes = [
ComposableNode(
package="pointcloud_to_laserscan",
plugin="pointcloud_to_laserscan::PointCloudToLaserScanNode",
name="pointcloud_to_laserscan_node",
remappings=[
("~/input/pointcloud", LaunchConfiguration("input/obstacle_pointcloud")),
("~/output/laserscan", LaunchConfiguration("output/laserscan")),
("~/output/pointcloud", LaunchConfiguration("output/pointcloud")),
("~/output/ray", LaunchConfiguration("output/ray")),
("~/output/stixel", LaunchConfiguration("output/stixel")),
],
parameters=[
{
"target_frame": "base_link", # Leave disabled to output scan in pointcloud frame
"transform_tolerance": 0.01,
"min_height": 0.0,
"max_height": 2.0,
"angle_min": -3.141592, # -M_PI
"angle_max": 3.141592, # M_PI
"angle_increment": 0.00436332222, # 0.25*M_PI/180.0
"scan_time": 0.0,
"range_min": 1.0,
"range_max": 200.0,
"use_inf": True,
"inf_epsilon": 1.0,
# Concurrency level, affects number of pointclouds queued for processing
# and number of threads used
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
"concurrency_level": 1,
}
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
),
ComposableNode(
package="laserscan_to_occupancy_grid_map",
plugin="occupancy_grid_map::OccupancyGridMapNode",
name="occupancy_grid_map_node",
remappings=[
("~/input/laserscan", LaunchConfiguration("output/laserscan")),
("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")),
("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")),
("~/output/occupancy_grid_map", LaunchConfiguration("output")),
],
parameters=[
{
"map_resolution": 0.5,
"use_height_filter": True,
"input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"),
"input_obstacle_and_raw_pointcloud": LaunchConfiguration(
"input_obstacle_and_raw_pointcloud"
),
}
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
),
]
occupancy_grid_map_container = ComposableNodeContainer(
condition=LaunchConfigurationEquals("container", ""),
name="occupancy_grid_map_container",
namespace="",
package="rclcpp_components",
executable=LaunchConfiguration("container_executable"),
composable_node_descriptions=composable_nodes,
output="screen",
)
load_composable_nodes = LoadComposableNodes(
condition=LaunchConfigurationNotEquals("container", ""),
composable_node_descriptions=composable_nodes,
target_container=LaunchConfiguration("container"),
)
return LaunchDescription(
[
add_launch_arg("container", ""),
add_launch_arg("use_multithread", "false"),
add_launch_arg("use_intra_process", "false"),
add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"),
add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"),
add_launch_arg("output", "occupancy_grid"),
add_launch_arg("output/laserscan", "virtual_scan/laserscan"),
add_launch_arg("output/pointcloud", "virtual_scan/pointcloud"),
add_launch_arg("output/ray", "virtual_scan/ray"),
add_launch_arg("output/stixel", "virtual_scan/stixel"),
add_launch_arg("input_obstacle_pointcloud", "false"),
add_launch_arg("input_obstacle_and_raw_pointcloud", "true"),
set_container_executable,
set_container_mt_executable,
occupancy_grid_map_container,
load_composable_nodes,
]
)
def generate_launch_description():
camera_name = 'forward_camera'
orca_bringup_dir = get_package_share_directory('orca_bringup')
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
orca_description_dir = get_package_share_directory('orca_description')
nav2_bringup_launch_dir = os.path.join(nav2_bringup_dir, 'launch')
urdf_file = os.path.join(orca_description_dir, 'urdf', 'hw7.urdf')
nav2_bt_file = os.path.join(orca_bringup_dir, 'behavior_trees',
'orca3_bt.xml')
use_sim_time = LaunchConfiguration('use_sim_time')
orca_params_file = LaunchConfiguration('orca_params_file')
nav2_params_file = LaunchConfiguration('nav2_params_file')
vlam_map_file = LaunchConfiguration('vlam_map')
nav2_map_file = LaunchConfiguration('nav2_map')
# get_package_share_directory('orb_slam2_ros') will fail if orb_slam2_ros isn't installed
orb_voc_file = os.path.join('install', 'orb_slam2_ros', 'share',
'orb_slam2_ros', 'orb_slam2', 'Vocabulary',
'ORBvoc.txt')
# Read the params file and make some global substitutions
configured_orca_params = RewrittenYaml(source_file=orca_params_file,
param_rewrites={
'use_sim_time':
use_sim_time,
'marker_map_load_full_filename':
vlam_map_file,
},
convert_types=True)
configured_nav2_params = RewrittenYaml(
source_file=nav2_params_file,
param_rewrites={
'use_sim_time': use_sim_time,
'yaml_filename': nav2_map_file,
'default_nav_through_poses_bt_xml': 'invalid_file',
'default_nav_to_pose_bt_xml': nav2_bt_file,
},
convert_types=True)
return LaunchDescription([
SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
DeclareLaunchArgument(
'use_sim_time',
default_value='False', # TODO sim time broken
description='Use simulation (Gazebo) clock (BROKEN BROKEN BROKEN)?'
),
DeclareLaunchArgument('slam',
default_value='orb',
description='Choose SLAM strategy: ' +
', '.join(slams)),
DeclareLaunchArgument('nav',
default_value='True',
description='Launch nav?'),
DeclareLaunchArgument(
'vlam_map',
default_value=
'install/orca_gazebo/share/orca_gazebo/worlds/medium_ring_map.yaml',
description='Full path to Vlam map file'),
DeclareLaunchArgument(
'nav2_map',
default_value=
'install/orca_bringup/share/orca_bringup/worlds/empty_world.yaml',
description='Full path to Nav2 map file'),
DeclareLaunchArgument(
'orca_params_file',
default_value=os.path.join(orca_bringup_dir, 'params',
'orca_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for Orca nodes'),
DeclareLaunchArgument(
'nav2_params_file',
default_value=os.path.join(orca_bringup_dir, 'params',
'nav2_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for Nav2 nodes'),
# Publish static /tf
Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
name='robot_state_publisher',
arguments=[urdf_file],
parameters=[configured_orca_params]),
# Barometer filter
Node(package='orca_base',
executable='baro_filter_node',
output='screen',
name='baro_filter_node',
parameters=[configured_orca_params]),
# Publish /joy
# joy::joy_node uses SDL; multiplatform, but burns a lot of CPU
# Use joy_linux::joy_linux_node instead
# https://discourse.libsdl.org/t/patch-for-sdl-waitevent-to-avoid-using-cpu-while-waiting-an-event/28555
Node(package='joy_linux',
executable='joy_linux_node',
output='screen',
name='joy_linux_node',
parameters=[configured_orca_params]),
# Subscribe to /joy and publish /cmd_vel
Node(package='orca_topside',
executable='teleop_node',
output='screen',
name='teleop_node',
parameters=[configured_orca_params],
remappings=[
('slam', 'orb_slam2_stereo_node/status'),
('debug_image', 'orb_slam2_stereo_node/debug_image'),
]),
# Subscribe to /cmd_vel and publish /thrust, /odom and /tf odom->base_link
Node(package='orca_base',
executable='base_controller',
output='screen',
name='base_controller',
parameters=[configured_orca_params],
remappings=[
('barometer', 'filtered_barometer'),
]),
# fiducial_vlam: publish a map of ArUco markers
Node(package='fiducial_vlam',
executable='vmap_main',
output='screen',
name='vmap_main',
parameters=[configured_orca_params],
condition=LaunchConfigurationEquals('slam', 'vlam')),
# fiducial_vlam: find ArUco markers and publish the camera pose
Node(package='fiducial_vlam',
executable='vloc_main',
output='screen',
name='vloc_main',
namespace=camera_name,
parameters=[configured_orca_params],
condition=LaunchConfigurationEquals('slam', 'vlam')),
# fiducial_vlam: subscribe to the camera pose and publish /tf map->odom
Node(package='orca_localize',
executable='fiducial_localizer',
output='screen',
name='fiducial_localizer',
parameters=[configured_orca_params],
remappings=[
('camera_pose', '/' + camera_name + '/camera_pose'),
],
condition=LaunchConfigurationEquals('slam', 'vlam')),
# orb_slam2: build a map of 3d points, localize against the map, and publish the camera pose
Node(package='orb_slam2_ros',
executable='orb_slam2_ros_stereo',
output='screen',
name='orb_slam2_stereo',
parameters=[configured_orca_params, {
'voc_file': orb_voc_file,
}],
remappings=[
('/image_left/image_color_rect', '/stereo/left/image_raw'),
('/image_right/image_color_rect', '/stereo/right/image_raw'),
('/camera/camera_info', '/stereo/left/camera_info'),
],
condition=LaunchConfigurationEquals('slam', 'orb')),
# orb_slam2, but inputs are h264 streams rather than rectified images
Node(
package='orb_slam2_ros',
executable='orb_slam2_ros_h264_stereo',
output='screen',
name=
'orb_slam2_stereo', # Same name so 'orb' and 'orb_h264' can share param files
parameters=[configured_orca_params, {
'voc_file': orb_voc_file,
}],
remappings=[
# No need to remap. Node expects:
# /stereo/left/image_raw/h264
# /stereo/left/camera_info
# /stereo/right/image_raw/h264
# /stereo/right/camera_info
],
condition=LaunchConfigurationEquals('slam', 'orb_h264')),
# orb_slam2: subscribe to the camera pose and publish /tf map->odom
Node(
package='orca_localize',
executable='orb_slam2_localizer',
output='screen',
name='orb_slam2_localizer',
parameters=[configured_orca_params],
remappings=[
# Topic is hard coded in orb_slam2_ros to /orb_slam2_stereo_node/pose
('/camera_pose', '/orb_slam2_stereo_node/pose'),
],
# Launch if slam:=orb or slam:=orb_h264
condition=IfCondition(
PythonExpression(
["'orb' in '",
LaunchConfiguration('slam'), "'"]))),
# Publish a [likely empty] nav2 map
Node(package='nav2_map_server',
executable='map_server',
name='map_server',
output='screen',
parameters=[configured_nav2_params],
condition=IfCondition(LaunchConfiguration('nav'))),
# Manage the lifecycle of map_server
Node(package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_map_server',
output='screen',
parameters=[{
'use_sim_time': use_sim_time,
'autostart': True,
'node_names': ['map_server'],
}],
condition=IfCondition(LaunchConfiguration('nav'))),
# Include the rest of Nav2
IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(nav2_bringup_launch_dir, 'navigation_launch.py')),
launch_arguments={
'use_sim_time': use_sim_time,
'autostart': 'True',
'params_file': nav2_params_file,
'map_subscribe_transient_local': 'true',
}.items(),
condition=IfCondition(
LaunchConfiguration('nav'))),
])