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 _load_composable_node(*,
package,
plugin,
name,
namespace='',
parameters=None,
remappings=None,
target_container=f'/{TEST_CONTAINER_NAME}'):
return LoadComposableNodes(target_container=target_container,
composable_node_descriptions=[
ComposableNode(
package=package,
plugin=plugin,
name=name,
namespace=namespace,
parameters=parameters,
remappings=remappings,
)
])
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
namespace = LaunchConfiguration('namespace')
use_sim_time = LaunchConfiguration('use_sim_time')
autostart = LaunchConfiguration('autostart')
params_file = LaunchConfiguration('params_file')
use_composition = LaunchConfiguration('use_composition')
container_name = LaunchConfiguration('container_name')
lifecycle_nodes = [
'controller_server', 'smoother_server', 'planner_server',
'behavior_server', 'bt_navigator', 'waypoint_follower'
]
# 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,
'autostart': autostart
}
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_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='False',
description='Use composed bringup if True')
declare_container_name_cmd = DeclareLaunchArgument(
'container_name',
default_value='nav2_container',
description=
'the name of conatiner that nodes will load in if use composition')
load_nodes = GroupAction(condition=IfCondition(
PythonExpression(['not ', use_composition])),
actions=[
Node(package='nav2_controller',
executable='controller_server',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(package='nav2_smoother',
executable='smoother_server',
name='smoother_server',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(package='nav2_planner',
executable='planner_server',
name='planner_server',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(package='nav2_behaviors',
executable='behavior_server',
name='behavior_server',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(package='nav2_bt_navigator',
executable='bt_navigator',
name='bt_navigator',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(package='nav2_waypoint_follower',
executable='waypoint_follower',
name='waypoint_follower',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_navigation',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}, {
'autostart': autostart
}, {
'node_names': lifecycle_nodes
}]),
])
load_composable_nodes = LoadComposableNodes(
condition=IfCondition(use_composition),
target_container=container_name,
composable_node_descriptions=[
ComposableNode(package='nav2_controller',
plugin='nav2_controller::ControllerServer',
name='controller_server',
parameters=[configured_params],
remappings=remappings),
ComposableNode(package='nav2_smoother',
plugin='nav2_smoother::SmootherServer',
name='smoother_server',
parameters=[configured_params],
remappings=remappings),
ComposableNode(package='nav2_planner',
plugin='nav2_planner::PlannerServer',
name='planner_server',
parameters=[configured_params],
remappings=remappings),
ComposableNode(package='nav2_behaviors',
plugin='behavior_server::BehaviorServer',
name='behavior_server',
parameters=[configured_params],
remappings=remappings),
ComposableNode(package='nav2_bt_navigator',
plugin='nav2_bt_navigator::BtNavigator',
name='bt_navigator',
parameters=[configured_params],
remappings=remappings),
ComposableNode(package='nav2_waypoint_follower',
plugin='nav2_waypoint_follower::WaypointFollower',
name='waypoint_follower',
parameters=[configured_params],
remappings=remappings),
ComposableNode(package='nav2_lifecycle_manager',
plugin='nav2_lifecycle_manager::LifecycleManager',
name='lifecycle_manager_navigation',
parameters=[{
'use_sim_time': use_sim_time,
'autostart': autostart,
'node_names': lifecycle_nodes
}]),
],
)
# 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_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_use_composition_cmd)
ld.add_action(declare_container_name_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(load_nodes)
ld.add_action(load_composable_nodes)
return ld
def generate_launch_description():
arguments = [
# component
DeclareLaunchArgument("use_intra_process"),
DeclareLaunchArgument("target_container"),
# settings
DeclareLaunchArgument(
"initial_selector_mode", default_value="local", choices=["local", "remote"]
),
# service
DeclareLaunchArgument(
"service/select_external_command", default_value="~/select_external_command"
),
# local input
DeclareLaunchArgument(
"input/local/control_cmd", default_value="/api/external/set/command/local/control"
),
DeclareLaunchArgument(
"input/local/shift_cmd", default_value="/api/external/set/command/local/shift"
),
DeclareLaunchArgument(
"input/local/turn_signal_cmd",
default_value="/api/external/set/command/local/turn_signal",
),
DeclareLaunchArgument(
"input/local/heartbeat", default_value="/api/external/set/command/local/heartbeat"
),
# remote input
DeclareLaunchArgument(
"input/remote/control_cmd", default_value="/api/external/set/command/remote/control"
),
DeclareLaunchArgument(
"input/remote/shift_cmd", default_value="/api/external/set/command/remote/shift"
),
DeclareLaunchArgument(
"input/remote/turn_signal_cmd",
default_value="/api/external/set/command/remote/turn_signal",
),
DeclareLaunchArgument(
"input/remote/heartbeat", default_value="/api/external/set/command/remote/heartbeat"
),
# output
DeclareLaunchArgument(
"output/control_cmd", default_value="/external/selected/external_control_cmd"
),
DeclareLaunchArgument("output/gear_cmd", default_value="/external/selected/gear_cmd"),
DeclareLaunchArgument(
"output/turn_indicators_cmd", default_value="/external/selected/turn_indicators_cmd"
),
DeclareLaunchArgument(
"output/hazard_lights_cmd", default_value="/external/selected/hazard_lights_cmd"
),
DeclareLaunchArgument("output/heartbeat", default_value="/external/selected/heartbeat"),
DeclareLaunchArgument(
"output/current_selector_mode", default_value="~/current_selector_mode"
),
]
component = ComposableNode(
package="external_cmd_selector",
plugin="ExternalCmdSelector",
name="external_cmd_selector",
remappings=[
_create_mapping_tuple("service/select_external_command"),
_create_mapping_tuple("input/local/control_cmd"),
_create_mapping_tuple("input/local/shift_cmd"),
_create_mapping_tuple("input/local/turn_signal_cmd"),
_create_mapping_tuple("input/local/heartbeat"),
_create_mapping_tuple("input/remote/control_cmd"),
_create_mapping_tuple("input/remote/shift_cmd"),
_create_mapping_tuple("input/remote/turn_signal_cmd"),
_create_mapping_tuple("input/remote/heartbeat"),
_create_mapping_tuple("output/control_cmd"),
_create_mapping_tuple("output/gear_cmd"),
_create_mapping_tuple("output/turn_indicators_cmd"),
_create_mapping_tuple("output/hazard_lights_cmd"),
_create_mapping_tuple("output/heartbeat"),
_create_mapping_tuple("output/current_selector_mode"),
],
parameters=[
{
"initial_selector_mode": LaunchConfiguration("initial_selector_mode"),
}
],
extra_arguments=[
{
"use_intra_process_comms": LaunchConfiguration("use_intra_process"),
}
],
)
loader = LoadComposableNodes(
composable_node_descriptions=[component],
target_container=LaunchConfiguration("target_container"),
)
return LaunchDescription(arguments + [loader])
def generate_launch_description():
arguments = [
# component
DeclareLaunchArgument("use_intra_process"),
DeclareLaunchArgument("target_container"),
# map file
DeclareLaunchArgument(
"csv_path_accel_map",
default_value=[
FindPackageShare("raw_vehicle_cmd_converter"),
"/data/default/accel_map.csv",
], # noqa: E501
description="csv file path for accel map",
),
DeclareLaunchArgument(
"csv_path_brake_map",
default_value=[
FindPackageShare("raw_vehicle_cmd_converter"),
"/data/default/brake_map.csv",
], # noqa: E501
description="csv file path for brake map",
),
# settings
DeclareLaunchArgument(
"ref_vel_gain",
default_value="3.0",
description="gain for external command accel",
),
DeclareLaunchArgument(
"wait_for_first_topic",
default_value="true",
description=
"disable topic disruption detection until subscribing first topics",
),
DeclareLaunchArgument(
"control_command_timeout",
default_value="1.0",
description="external control command timeout",
),
DeclareLaunchArgument(
"emergency_stop_timeout",
default_value="3.0",
description="emergency stop timeout for external heartbeat",
),
# input
DeclareLaunchArgument(
"in/external_control_cmd",
default_value="/external/selected/external_control_cmd",
),
DeclareLaunchArgument(
"in/shift_cmd",
default_value="/external/selected/gear_cmd",
),
DeclareLaunchArgument(
"in/emergency_stop",
default_value="/external/selected/heartbeat",
),
DeclareLaunchArgument(
"in/current_gate_mode",
default_value="/control/current_gate_mode",
),
DeclareLaunchArgument(
"in/odometry",
default_value="/localization/kinematic_state",
),
# output
DeclareLaunchArgument(
"out/control_cmd",
default_value="/external/selected/control_cmd",
),
DeclareLaunchArgument(
"out/latest_external_control_cmd",
default_value="/api/external/get/command/selected/control",
),
]
component = ComposableNode(
package="external_cmd_converter",
plugin="external_cmd_converter::ExternalCmdConverterNode",
name="external_cmd_converter",
remappings=[
_create_mapping_tuple("in/external_control_cmd"),
_create_mapping_tuple("in/shift_cmd"),
_create_mapping_tuple("in/emergency_stop"),
_create_mapping_tuple("in/current_gate_mode"),
_create_mapping_tuple("in/odometry"),
_create_mapping_tuple("out/control_cmd"),
_create_mapping_tuple("out/latest_external_control_cmd"),
],
parameters=[
dict( # noqa: C406 for using helper function
[
_create_mapping_tuple("csv_path_accel_map"),
_create_mapping_tuple("csv_path_brake_map"),
_create_mapping_tuple("ref_vel_gain"),
_create_mapping_tuple("wait_for_first_topic"),
_create_mapping_tuple("control_command_timeout"),
_create_mapping_tuple("emergency_stop_timeout"),
])
],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process"),
}],
)
loader = LoadComposableNodes(
composable_node_descriptions=[component],
target_container=LaunchConfiguration("target_container"),
)
return LaunchDescription(arguments + [loader])
def launch_setup(context, *args, **kwargs):
# https://github.com/ros2/launch_ros/issues/156
def load_composable_node_param(param_path):
with open(LaunchConfiguration(param_path).perform(context), "r") as f:
return yaml.safe_load(f)["/**"]["ros__parameters"]
ns = "euclidean_cluster"
pkg = "euclidean_cluster"
# set voxel grid filter as a component
voxel_grid_filter_component = ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
name=AnonName("voxel_grid_filter"),
remappings=[
("input", LaunchConfiguration("input_pointcloud")),
("output", "voxel_grid_filtered/pointcloud"),
],
parameters=[load_composable_node_param("voxel_grid_param_path")],
)
# set compare map filter as a component
compare_map_filter_component = ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::VoxelBasedCompareMapFilterComponent",
name=AnonName("compare_map_filter"),
remappings=[
("input", "voxel_grid_filtered/pointcloud"),
("map", LaunchConfiguration("input_map")),
("output", "compare_map_filtered/pointcloud"),
],
)
use_map_euclidean_cluster_component = ComposableNode(
package=pkg,
plugin="euclidean_cluster::EuclideanClusterNode",
name=AnonName("euclidean_cluster"),
remappings=[
("input", "compare_map_filtered/pointcloud"),
("output", LaunchConfiguration("output_clusters")),
],
parameters=[load_composable_node_param("euclidean_param_path")],
)
disuse_map_euclidean_cluster_component = ComposableNode(
package=pkg,
plugin="euclidean_cluster::EuclideanClusterNode",
name=AnonName("euclidean_cluster"),
remappings=[
("input", "voxel_grid_filtered/pointcloud"),
("output", LaunchConfiguration("output_clusters")),
],
parameters=[load_composable_node_param("euclidean_param_path")],
)
container = ComposableNodeContainer(
name="euclidean_cluster_container",
namespace=ns,
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=[voxel_grid_filter_component],
output="screen",
)
use_map_loader = LoadComposableNodes(
composable_node_descriptions=[
compare_map_filter_component,
use_map_euclidean_cluster_component,
],
target_container=container,
condition=IfCondition(LaunchConfiguration("use_pointcloud_map")),
)
disuse_map_loader = LoadComposableNodes(
composable_node_descriptions=[disuse_map_euclidean_cluster_component],
target_container=container,
condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")),
)
return [container, use_map_loader, disuse_map_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():
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,
]
)