From 692fac14335d27dab648db9d84cf5618042f99a1 Mon Sep 17 00:00:00 2001 From: Darin Date: Mon, 21 Nov 2022 19:14:48 -0600 Subject: [PATCH] add launch args (#11) Co-authored-by: Darin Keever --- launch/octomap_server_launch.py | 111 +++++++++++++++++++------------- 1 file changed, 66 insertions(+), 45 deletions(-) diff --git a/launch/octomap_server_launch.py b/launch/octomap_server_launch.py index 92b3a57..7a8f32e 100644 --- a/launch/octomap_server_launch.py +++ b/launch/octomap_server_launch.py @@ -1,51 +1,72 @@ #!/usr/bin/env python -import os.path as osp - -from launch import LaunchDescription, logging -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import \ - PythonLaunchDescriptionSource +from launch import LaunchDescription from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -from launch.substitutions import ThisLaunchFileDir +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument def generate_launch_description(): - - params = {'resolution': 0.15, - 'frame_id': 'map', - 'base_frame_id': 'base_footprint', - 'height_map': True, - 'colored_map': True, - 'color_factor': 0.8, - 'filter_ground': False, - 'filter_speckles': False, - 'ground_filter/distance': 0.04, - 'ground_filter/angle': 0.15, - 'ground_filter/plane_distance': 0.07, - 'compress_map': True, - 'incremental_2D_projection': False, - 'sensor_model/max_range': -1.0, - 'sensor_model/hit': 0.7, - 'sensor_model/miss': 0.4, - 'sensor_model/min': 0.12, - 'sensor_model/max': 0.97, - 'color/r': 0.0, - 'color/g': 0.0, - 'color/b': 1.0, - 'color/a': 1.0, - 'color_free/r': 0.0, - 'color_free/g': 0.0, - 'color_free/b': 1.0, - 'color_free/a': 1.0, - 'publish_free_space': False, - } - - remap = [('cloud_in', '/livox/lidar')] - node = Node(package='octomap_server2', - executable='octomap_server', - output='screen', - remappings=remap, - parameters=[params]) - return LaunchDescription([node]) + return LaunchDescription([ + DeclareLaunchArgument('input_cloud_topic', default_value='/livox/lidar'), + DeclareLaunchArgument('resolution', default_value='0.15'), + DeclareLaunchArgument('frame_id', default_value='map'), + DeclareLaunchArgument('base_frame_id', default_value='base_footprint'), + DeclareLaunchArgument('height_map', default_value='True'), + DeclareLaunchArgument('colored_map', default_value='True'), + DeclareLaunchArgument('color_factor', default_value='0.8'), + DeclareLaunchArgument('filter_ground', default_value='False'), + DeclareLaunchArgument('filter_speckles', default_value='False'), + DeclareLaunchArgument('ground_filter/distance', default_value='0.04'), + DeclareLaunchArgument('ground_filter/angle', default_value='0.15'), + DeclareLaunchArgument('ground_filter/plane_distance', default_value='0.07'), + DeclareLaunchArgument('compress_map', default_value='True'), + DeclareLaunchArgument('incremental_2D_projection', default_value='False'), + DeclareLaunchArgument('sensor_model/max_range', default_value='-1.0'), + DeclareLaunchArgument('sensor_model/hit', default_value='0.7'), + DeclareLaunchArgument('sensor_model/miss', default_value='0.4'), + DeclareLaunchArgument('sensor_model/min', default_value='0.12'), + DeclareLaunchArgument('sensor_model/max', default_value='0.97'), + DeclareLaunchArgument('color/r', default_value='0.0'), + DeclareLaunchArgument('color/g', default_value='0.0'), + DeclareLaunchArgument('color/b', default_value='1.0'), + DeclareLaunchArgument('color/a', default_value='1.0'), + DeclareLaunchArgument('color_free/r', default_value='0.0'), + DeclareLaunchArgument('color_free/g', default_value='0.0'), + DeclareLaunchArgument('color_free/b', default_value='1.0'), + DeclareLaunchArgument('color_free/a', default_value='1.0'), + DeclareLaunchArgument('publish_free_space', default_value='False'), + Node( + package='octomap_server2', + executable='octomap_server', + output='screen', + remappings=[('cloud_in', LaunchConfiguration('input_cloud_topic'))], + parameters=[{'resolution': LaunchConfiguration('resolution'), + 'frame_id': LaunchConfiguration('frame_id'), + 'base_frame_id': LaunchConfiguration('base_frame_id'), + 'height_map': LaunchConfiguration('height_map'), + 'colored_map': LaunchConfiguration('colored_map'), + 'color_factor': LaunchConfiguration('color_factor'), + 'filter_ground': LaunchConfiguration('filter_ground'), + 'filter_speckles': LaunchConfiguration('filter_speckles'), + 'ground_filter/distance': LaunchConfiguration('ground_filter/distance'), + 'ground_filter/angle': LaunchConfiguration('ground_filter/angle'), + 'ground_filter/plane_distance': LaunchConfiguration('ground_filter/plane_distance'), + 'compress_map': LaunchConfiguration('compress_map'), + 'incremental_2D_projection': LaunchConfiguration('incremental_2D_projection'), + 'sensor_model/max_range': LaunchConfiguration('sensor_model/max_range'), + 'sensor_model/hit': LaunchConfiguration('sensor_model/hit'), + 'sensor_model/miss': LaunchConfiguration('sensor_model/miss'), + 'sensor_model/min': LaunchConfiguration('sensor_model/min'), + 'sensor_model/max': LaunchConfiguration('sensor_model/max'), + 'color/r': LaunchConfiguration('color/r'), + 'color/g': LaunchConfiguration('color/g'), + 'color/b': LaunchConfiguration('color/b'), + 'color/a': LaunchConfiguration('color/a'), + 'color_free/r': LaunchConfiguration('color_free/r'), + 'color_free/g': LaunchConfiguration('color_free/g'), + 'color_free/b': LaunchConfiguration('color_free/b'), + 'color_free/a': LaunchConfiguration('color_free/a'), + 'publish_free_space': LaunchConfiguration('publish_free_space')}] + ) + ])