diff --git a/sensing_launch/launch/aip_customized/pointcloud_preprocessor.launch.py b/sensing_launch/launch/aip_customized/pointcloud_preprocessor.launch.py index 1a42cb2db..930f83765 100644 --- a/sensing_launch/launch/aip_customized/pointcloud_preprocessor.launch.py +++ b/sensing_launch/launch/aip_customized/pointcloud_preprocessor.launch.py @@ -21,6 +21,7 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode +from launch.substitutions import EnvironmentVariable def get_vehicle_info(context): @@ -63,6 +64,7 @@ def launch_setup(context, *args, **kwargs): '/sensing/lidar/left/outlier_filtered/pointcloud', '/sensing/lidar/right/outlier_filtered/pointcloud'], 'output_frame': 'base_link', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) else: @@ -78,6 +80,7 @@ def launch_setup(context, *args, **kwargs): 'output_frame': 'base_link', 'min_z': vehicle_info['min_height_offset'], 'max_z': vehicle_info['max_height_offset'], + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -101,6 +104,7 @@ def launch_setup(context, *args, **kwargs): 'min_z': vehicle_info['min_height_offset'], 'max_z': vehicle_info['max_height_offset'], 'negative': False, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -116,6 +120,7 @@ def launch_setup(context, *args, **kwargs): "general_max_slope": 10.0, "local_max_slope": 10.0, "min_height_threshold": 0.2, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -127,6 +132,7 @@ def launch_setup(context, *args, **kwargs): "input_topic": "/sensing/lidar/top/rectified/pointcloud", "output_topic": "/sensing/lidar/pointcloud", "type": "sensor_msgs/msg/PointCloud2", + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }], ) @@ -143,6 +149,9 @@ def launch_setup(context, *args, **kwargs): relay_component, ], output='screen', + parameters=[{ + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), + }], ) return [container] diff --git a/sensing_launch/launch/aip_s1/pointcloud_preprocessor.launch.py b/sensing_launch/launch/aip_s1/pointcloud_preprocessor.launch.py index 2247972d1..e68a57ecb 100644 --- a/sensing_launch/launch/aip_s1/pointcloud_preprocessor.launch.py +++ b/sensing_launch/launch/aip_s1/pointcloud_preprocessor.launch.py @@ -21,6 +21,7 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode +from launch.substitutions import EnvironmentVariable def get_vehicle_info(context): path = LaunchConfiguration('vehicle_param_file').perform(context) @@ -62,6 +63,7 @@ def launch_setup(context, *args, **kwargs): '/sensing/lidar/left/outlier_filtered/pointcloud', '/sensing/lidar/right/outlier_filtered/pointcloud'], 'output_frame': 'base_link', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) else: @@ -78,6 +80,7 @@ def launch_setup(context, *args, **kwargs): 'output_frame': 'base_link', 'min_z': vehicle_info['min_height_offset'], 'max_z': vehicle_info['max_height_offset'], + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -100,6 +103,7 @@ def launch_setup(context, *args, **kwargs): 'min_z': vehicle_info['min_height_offset'], 'max_z': vehicle_info['max_height_offset'], 'negative': False, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -115,6 +119,7 @@ def launch_setup(context, *args, **kwargs): "general_max_slope": 10.0, "local_max_slope": 10.0, "min_height_threshold": 0.2, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -126,6 +131,7 @@ def launch_setup(context, *args, **kwargs): "input_topic": "/sensing/lidar/top/rectified/pointcloud", "output_topic": "/sensing/lidar/pointcloud", "type": "sensor_msgs/msg/PointCloud2", + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }], ) @@ -142,6 +148,9 @@ def launch_setup(context, *args, **kwargs): relay_component, ], output='screen', + parameters=[{ + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), + }], ) return [container] diff --git a/sensing_launch/launch/aip_x1/pointcloud_preprocessor.launch.py b/sensing_launch/launch/aip_x1/pointcloud_preprocessor.launch.py index 1408f7184..079dc8d7e 100644 --- a/sensing_launch/launch/aip_x1/pointcloud_preprocessor.launch.py +++ b/sensing_launch/launch/aip_x1/pointcloud_preprocessor.launch.py @@ -21,6 +21,7 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode +from launch.substitutions import EnvironmentVariable def get_vehicle_info(context): path = LaunchConfiguration('vehicle_param_file').perform(context) @@ -63,6 +64,7 @@ def launch_setup(context, *args, **kwargs): '/sensing/lidar/front_right/mirror_cropped/pointcloud', '/sensing/lidar/front_center/mirror_cropped/pointcloud'], 'output_frame': 'base_link', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) else: @@ -79,6 +81,7 @@ def launch_setup(context, *args, **kwargs): 'output_frame': 'base_link', 'min_z': vehicle_info['min_height_offset'], 'max_z': vehicle_info['max_height_offset'], + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -101,6 +104,7 @@ def launch_setup(context, *args, **kwargs): 'min_z': vehicle_info['min_height_offset'], 'max_z': vehicle_info['max_height_offset'], 'negative': False, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -122,6 +126,7 @@ def launch_setup(context, *args, **kwargs): "max_x": vehicle_info['max_longitudinal_offset'], "min_y": vehicle_info['min_lateral_offset'], "max_y": vehicle_info['max_lateral_offset'], + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -139,6 +144,7 @@ def launch_setup(context, *args, **kwargs): "voxel_size_x": 0.04, "voxel_size_y": 0.04, "voxel_size_z": 0.08, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -154,6 +160,7 @@ def launch_setup(context, *args, **kwargs): parameters=[{ "search_radius": 0.2, "min_neighbors": 5 + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) else: @@ -170,6 +177,7 @@ def launch_setup(context, *args, **kwargs): "voxel_size_y": 0.4, "voxel_size_z": 100, "voxel_points_threshold": 5, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -181,6 +189,7 @@ def launch_setup(context, *args, **kwargs): "input_topic": "/sensing/lidar/top/rectified/pointcloud", "output_topic": "/sensing/lidar/pointcloud", "type": "sensor_msgs/msg/PointCloud2", + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }], ) @@ -197,6 +206,9 @@ def launch_setup(context, *args, **kwargs): relay_component, ], output='screen', + parameters=[{ + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), + }], ) return [container] diff --git a/sensing_launch/launch/aip_x2/pointcloud_preprocessor.launch.py b/sensing_launch/launch/aip_x2/pointcloud_preprocessor.launch.py index 2247972d1..e68a57ecb 100644 --- a/sensing_launch/launch/aip_x2/pointcloud_preprocessor.launch.py +++ b/sensing_launch/launch/aip_x2/pointcloud_preprocessor.launch.py @@ -21,6 +21,7 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode +from launch.substitutions import EnvironmentVariable def get_vehicle_info(context): path = LaunchConfiguration('vehicle_param_file').perform(context) @@ -62,6 +63,7 @@ def launch_setup(context, *args, **kwargs): '/sensing/lidar/left/outlier_filtered/pointcloud', '/sensing/lidar/right/outlier_filtered/pointcloud'], 'output_frame': 'base_link', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) else: @@ -78,6 +80,7 @@ def launch_setup(context, *args, **kwargs): 'output_frame': 'base_link', 'min_z': vehicle_info['min_height_offset'], 'max_z': vehicle_info['max_height_offset'], + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -100,6 +103,7 @@ def launch_setup(context, *args, **kwargs): 'min_z': vehicle_info['min_height_offset'], 'max_z': vehicle_info['max_height_offset'], 'negative': False, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -115,6 +119,7 @@ def launch_setup(context, *args, **kwargs): "general_max_slope": 10.0, "local_max_slope": 10.0, "min_height_threshold": 0.2, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -126,6 +131,7 @@ def launch_setup(context, *args, **kwargs): "input_topic": "/sensing/lidar/top/rectified/pointcloud", "output_topic": "/sensing/lidar/pointcloud", "type": "sensor_msgs/msg/PointCloud2", + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }], ) @@ -142,6 +148,9 @@ def launch_setup(context, *args, **kwargs): relay_component, ], output='screen', + parameters=[{ + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), + }], ) return [container] diff --git a/sensing_launch/launch/aip_xx1/pointcloud_preprocessor.launch.py b/sensing_launch/launch/aip_xx1/pointcloud_preprocessor.launch.py index 4139e0507..43d68f352 100644 --- a/sensing_launch/launch/aip_xx1/pointcloud_preprocessor.launch.py +++ b/sensing_launch/launch/aip_xx1/pointcloud_preprocessor.launch.py @@ -21,6 +21,7 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode +from launch.substitutions import EnvironmentVariable def get_vehicle_info(context): path = LaunchConfiguration('vehicle_param_file').perform(context) @@ -61,6 +62,7 @@ def launch_setup(context, *args, **kwargs): '/sensing/lidar/left/outlier_filtered/pointcloud', '/sensing/lidar/right/outlier_filtered/pointcloud'], 'output_frame': 'base_link', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) else: @@ -76,6 +78,7 @@ def launch_setup(context, *args, **kwargs): 'output_frame': 'base_link', 'min_z': vehicle_info['min_height_offset'], 'max_z': vehicle_info['max_height_offset'], + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -98,6 +101,7 @@ def launch_setup(context, *args, **kwargs): 'min_z': vehicle_info['min_height_offset'], 'max_z': vehicle_info['max_height_offset'], 'negative': False, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -113,6 +117,7 @@ def launch_setup(context, *args, **kwargs): "general_max_slope": 10.0, "local_max_slope": 10.0, "min_height_threshold": 0.2, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -124,6 +129,7 @@ def launch_setup(context, *args, **kwargs): "input_topic": "/sensing/lidar/top/rectified/pointcloud", "output_topic": "/sensing/lidar/pointcloud", "type": "sensor_msgs/msg/PointCloud2", + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }], ) @@ -140,6 +146,9 @@ def launch_setup(context, *args, **kwargs): relay_component, ], output='screen', + parameters=[{ + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), + }], ) return [container] diff --git a/sensing_launch/launch/aip_xx2/pointcloud_preprocessor.launch.py b/sensing_launch/launch/aip_xx2/pointcloud_preprocessor.launch.py index 06d6b670b..05b05df15 100644 --- a/sensing_launch/launch/aip_xx2/pointcloud_preprocessor.launch.py +++ b/sensing_launch/launch/aip_xx2/pointcloud_preprocessor.launch.py @@ -21,6 +21,7 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode +from launch.substitutions import EnvironmentVariable def get_vehicle_info(context): @@ -62,6 +63,7 @@ def launch_setup(context, *args, **kwargs): '/sensing/lidar/left/outlier_filtered/pointcloud', '/sensing/lidar/right/outlier_filtered/pointcloud'], 'output_frame': 'base_link', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) else: @@ -78,6 +80,7 @@ def launch_setup(context, *args, **kwargs): 'output_frame': 'base_link', 'min_z': vehicle_info['min_height_offset'], 'max_z': vehicle_info['max_height_offset'], + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -100,6 +103,7 @@ def launch_setup(context, *args, **kwargs): 'min_z': vehicle_info['min_height_offset'], 'max_z': vehicle_info['max_height_offset'], 'negative': False, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -115,6 +119,7 @@ def launch_setup(context, *args, **kwargs): "general_max_slope": 10.0, "local_max_slope": 10.0, "min_height_threshold": 0.2, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -126,6 +131,7 @@ def launch_setup(context, *args, **kwargs): "input_topic": "/sensing/lidar/top/rectified/pointcloud", "output_topic": "/sensing/lidar/pointcloud", "type": "sensor_msgs/msg/PointCloud2", + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }], ) @@ -142,6 +148,9 @@ def launch_setup(context, *args, **kwargs): relay_component, ], output='screen', + parameters=[{ + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), + }], ) return [container] diff --git a/sensing_launch/launch/livox_horizon_pointcloud_preprocessor.launch.py b/sensing_launch/launch/livox_horizon_pointcloud_preprocessor.launch.py index 0de1e8f7c..20e512952 100644 --- a/sensing_launch/launch/livox_horizon_pointcloud_preprocessor.launch.py +++ b/sensing_launch/launch/livox_horizon_pointcloud_preprocessor.launch.py @@ -21,6 +21,7 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode from launch.launch_context import LaunchContext +from launch.substitutions import EnvironmentVariable def get_vehicle_info(context): @@ -70,6 +71,7 @@ def launch_setup(context, *args, **kwargs): 'min_z': vehicle_info['min_height_offset'], 'max_z': vehicle_info['max_height_offset'], 'negative': True, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -92,6 +94,7 @@ def launch_setup(context, *args, **kwargs): 'min_z': vehicle_mirror_info['min_height_offset'], 'max_z': vehicle_mirror_info['max_height_offset'], 'negative': True, + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), }] ) @@ -106,6 +109,9 @@ def launch_setup(context, *args, **kwargs): cropbox_mirror_component, ], output='screen', + parameters=[{ + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), + }], ) return [container] diff --git a/sensing_launch/launch/velodyne_node_container.launch.py b/sensing_launch/launch/velodyne_node_container.launch.py index 189ae333d..2bf67af44 100644 --- a/sensing_launch/launch/velodyne_node_container.launch.py +++ b/sensing_launch/launch/velodyne_node_container.launch.py @@ -18,6 +18,7 @@ from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes from launch_ros.descriptions import ComposableNode +from launch.substitutions import EnvironmentVariable import uuid @@ -72,8 +73,10 @@ def create_parameter_dict(*args): package='velodyne_pointcloud', plugin='velodyne_pointcloud::Convert', name='velodyne_convert_node', - parameters=[create_parameter_dict('calibration', 'min_range', 'max_range', - 'num_points_thresholds', 'invalid_intensity', 'sensor_frame')], + parameters=[{**create_parameter_dict('calibration', 'min_range', 'max_range', + 'num_points_thresholds', 'invalid_intensity', 'sensor_frame'), + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), + }], remappings=[('velodyne_points', 'pointcloud_raw'), ('velodyne_points_ex', 'pointcloud_raw_ex')], ) @@ -81,6 +84,7 @@ def create_parameter_dict(*args): cropbox_parameters = create_parameter_dict('input_frame', 'output_frame') cropbox_parameters['negative'] = True + cropbox_parameters['use_sim_time'] = EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False') cropbox_remappings = [ ('/min_x', '/vehicle_info/min_longitudinal_offset'), @@ -124,6 +128,9 @@ def create_parameter_dict(*args): ('velodyne_points_interpolate', 'rectified/pointcloud'), ('velodyne_points_interpolate_ex', 'rectified/pointcloud_ex'), ], + parameters=[{ + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), + }], ) ) @@ -135,6 +142,9 @@ def create_parameter_dict(*args): ('/input', 'rectified/pointcloud_ex'), ('/output', 'outlier_filtered/pointcloud') ], + parameters=[{ + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), + }], ) ) @@ -146,6 +156,9 @@ def create_parameter_dict(*args): package='rclcpp_components', executable='component_container', composable_node_descriptions=nodes, + parameters=[{ + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), + }], ) driver_component = ComposableNode( @@ -153,9 +166,11 @@ def create_parameter_dict(*args): plugin='velodyne_driver::VelodyneDriver', # node is created in a global context, need to avoid name clash name='velodyne_driver', - parameters=[create_parameter_dict('device_ip', 'gps_time', 'read_once', 'read_fast', + parameters=[{**create_parameter_dict('device_ip', 'gps_time', 'read_once', 'read_fast', 'repeat_delay', 'frame_id', 'model', 'rpm', 'port', - 'pcap')], + 'pcap'), + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'), + }], ) # one way to add a ComposableNode conditional on a launch argument to a