From 7e11d6be53bbff6c44dab962151ef9d18753ff96 Mon Sep 17 00:00:00 2001 From: t4-x2 Date: Thu, 16 May 2024 15:16:34 +0900 Subject: [PATCH] chore: update sensor launch --- aip_x2_launch/launch/lidar.launch.xml | 96 ++------ .../launch/pointcloud_preprocessor.launch.py | 39 ++- aip_x2_launch/launch/radar.launch.xml | 193 ++++++++++++--- aip_x2_launch/launch/sensing.launch.xml | 14 +- .../launch/hesai_OT128.launch.xml | 3 + .../launch/nebula_node_container.launch.py | 223 ++++++++++++------ 6 files changed, 372 insertions(+), 196 deletions(-) diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index 07c4cb0e..58e1b6ed 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -1,6 +1,7 @@ + @@ -9,74 +10,14 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + @@ -91,10 +32,11 @@ + + - @@ -109,6 +51,10 @@ + + + + @@ -126,6 +72,8 @@ + + @@ -143,19 +91,15 @@ + + + + - - - - - - - - diff --git a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py index 6d435dee..291a4fd1 100644 --- a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py @@ -20,7 +20,7 @@ from launch.conditions import IfCondition from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration -from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import LoadComposableNodes, ComposableNodeContainer from launch_ros.descriptions import ComposableNode @@ -37,16 +37,18 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "input_topics": [ - "/sensing/lidar/front_upper/pointcloud", - "/sensing/lidar/front_lower/pointcloud", - "/sensing/lidar/left_upper/pointcloud", - "/sensing/lidar/left_lower/pointcloud", + # "/sensing/lidar/front_upper/pointcloud", + # "/sensing/lidar/front_lower/pointcloud", + # "/sensing/lidar/left_upper/pointcloud", + # "/sensing/lidar/left_lower/pointcloud", "/sensing/lidar/right_upper/pointcloud", "/sensing/lidar/right_lower/pointcloud", "/sensing/lidar/rear_upper/pointcloud", "/sensing/lidar/rear_lower/pointcloud", ], - "input_offset": [0.025, 0.025, 0.01, 0.0, 0.05, 0.05, 0.05, 0.05], + "input_offset": [ + # 0.005, 0.025, 0.050, 0.005, + 0.050, 0.005, 0.005, 0.025], "timeout_sec": 0.075, "output_frame": LaunchConfiguration("base_frame"), "input_twist_topic_type": "twist", @@ -55,11 +57,30 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + measure_component = ComposableNode( + package="topic_delay", + plugin="TopicDelay", + name="measure_concat", + parameters=[ + { + "cloud_topic": "concatenated/pointcloud", + } + ] + ) + # load concat or passthrough filter - concat_loader = LoadComposableNodes( - composable_node_descriptions=[concat_component], + # concat_loader = ComposableNodeContainer( + # composable_node_descriptions=[concat_component, measure_component], + # namespace="", + # package='rclcpp_components', + # executable='component_container_mt', + # name=LaunchConfiguration("pointcloud_container_name"), + # condition=IfCondition(LaunchConfiguration("use_concat_filter")), + # ) + + concat_loader = loader = LoadComposableNodes( + composable_node_descriptions=[concat_component, measure_component], target_container=LaunchConfiguration("pointcloud_container_name"), - condition=IfCondition(LaunchConfiguration("use_concat_filter")), ) return [concat_loader] diff --git a/aip_x2_launch/launch/radar.launch.xml b/aip_x2_launch/launch/radar.launch.xml index 042e9d81..3c47b248 100644 --- a/aip_x2_launch/launch/radar.launch.xml +++ b/aip_x2_launch/launch/radar.launch.xml @@ -1,68 +1,185 @@ + - - + + + - + + + + + - - - - - + + + + + + + + - - - - - + + + + + + + + - - - - - + + + + + + + + + - - - - - + + + + + + + + + + + + + + + + + + - - - - - + + + + + + + + + + + + + + + + + + + - - - - - + + + + + + + + + + + + + + + + + + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + \ No newline at end of file diff --git a/aip_x2_launch/launch/sensing.launch.xml b/aip_x2_launch/launch/sensing.launch.xml index 274f6334..52f9bb34 100644 --- a/aip_x2_launch/launch/sensing.launch.xml +++ b/aip_x2_launch/launch/sensing.launch.xml @@ -2,7 +2,7 @@ - + @@ -15,19 +15,19 @@ - + - + - + diff --git a/common_sensor_launch/launch/hesai_OT128.launch.xml b/common_sensor_launch/launch/hesai_OT128.launch.xml index 6e9db6be..16d6d1e7 100644 --- a/common_sensor_launch/launch/hesai_OT128.launch.xml +++ b/common_sensor_launch/launch/hesai_OT128.launch.xml @@ -17,6 +17,8 @@ + + @@ -38,5 +40,6 @@ + diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index 97e5079c..a53304c2 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -86,19 +86,19 @@ def create_parameter_dict(*args): nodes = [] - nodes.append( - ComposableNode( - package="glog_component", - plugin="GlogComponent", - name="glog_component", - ) - ) + # nodes.append( + # ComposableNode( + # package="glog_component", + # plugin="GlogComponent", + # name="glog_component", + # ) + # ) nodes.append( ComposableNode( package="nebula_ros", - plugin=sensor_make + "DriverRosWrapper", - name=sensor_make.lower() + "_driver_ros_wrapper_node", + plugin=sensor_make + "RosWrapper", + name=sensor_make.lower() + "_ros_wrapper_node", parameters=[ { "calibration_file": sensor_calib_fp, @@ -115,7 +115,15 @@ def create_parameter_dict(*args): "cloud_min_angle", "cloud_max_angle", "dual_return_distance_threshold", + "gnss_port", + "packet_mtu_size", + "setup_sensor", + "ptp_profile", + "ptp_transport_type", + "ptp_switch_type", + "ptp_domain", ), + "launch_hw": True }, ], remappings=[ @@ -126,6 +134,20 @@ def create_parameter_dict(*args): ) ) + # nodes.append( + # ComposableNode( + # package="topic_delay", + # plugin="TopicDelay", + # name="measure_nebula", + # parameters=[ + # { + # "cloud_topic": "pointcloud_raw_ex", + # "packets_topic": "pandar_packets" + # } + # ] + # ) + # ) + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") cropbox_parameters["negative"] = True @@ -144,35 +166,70 @@ def create_parameter_dict(*args): name="crop_box_filter_self", remappings=[ ("input", "pointcloud_raw_ex"), - ("output", "self_cropped/pointcloud_ex"), + ("output", "self_cropped_temp/pointcloud_ex"), ], parameters=[cropbox_parameters], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) - mirror_info = get_vehicle_mirror_info(context) - cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"] - cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"] - cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"] - cropbox_parameters["max_y"] = mirror_info["max_lateral_offset"] - cropbox_parameters["min_z"] = mirror_info["min_height_offset"] - cropbox_parameters["max_z"] = mirror_info["max_height_offset"] + # nodes.append( + # ComposableNode( + # package="topic_delay", + # plugin="TopicDelay", + # name="measure_cropbox", + # parameters=[ + # { + # "cloud_topic": "self_cropped_temp/pointcloud_ex", + # } + # ] + # ) + # ) + + # mirror_info = get_vehicle_mirror_info(context) + # cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"] + # cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"] + # cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"] + # cropbox_parameters["max_y"] = mirror_info["max_lateral_offset"] + # cropbox_parameters["min_z"] = mirror_info["min_height_offset"] + # cropbox_parameters["max_z"] = mirror_info["max_height_offset"] + + # todo: miura 24/03/03 temporarily added + cropbox_parameters["negative"] = True + cropbox_parameters["min_x"] = -50.0 + cropbox_parameters["max_x"] = 50.0 + cropbox_parameters["min_y"] = -50.0 + cropbox_parameters["max_y"] = 50.0 + cropbox_parameters["min_z"] = 4.5 + cropbox_parameters["max_z"] = 7.0 nodes.append( ComposableNode( package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_mirror", + name="crop_box_filter_ceiling", remappings=[ - ("input", "self_cropped/pointcloud_ex"), - ("output", "mirror_cropped/pointcloud_ex"), + ("input", "self_cropped_temp/pointcloud_ex"), + ("output", "self_cropped/pointcloud_ex"), ], parameters=[cropbox_parameters], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) + # nodes.append( + # ComposableNode( + # package="topic_delay", + # plugin="TopicDelay", + # name="measure_cropbox_ceiling", + # parameters=[ + # { + # "cloud_topic": "self_cropped/pointcloud_ex", + # } + # ] + # ) + # ) + nodes.append( ComposableNode( package="pointcloud_preprocessor", @@ -188,68 +245,102 @@ def create_parameter_dict(*args): ) ) + # nodes.append( + # ComposableNode( + # package="topic_delay", + # plugin="TopicDelay", + # name="measure_distortion_corrector", + # parameters=[ + # { + # "cloud_topic": "rectified/pointcloud_ex", + # } + # ] + # ) + # ) + nodes.append( ComposableNode( package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::RingOutlierFilterComponent", name="ring_outlier_filter", remappings=[ - ("input", "rectified/pointcloud_ex"), + ("input", "self_cropped/pointcloud_ex"), #todo: miura 24/03/03 temporarily input self_cropped/point ("output", "pointcloud"), ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) + # nodes.append( + # ComposableNode( + # package="topic_delay", + # plugin="TopicDelay", + # name="measure_ring_outlier", + # parameters=[ + # { + # "cloud_topic": "pointcloud", + # } + # ] + # ) + # ) + # set container to run all required components in the same process - container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), - namespace="pointcloud_preprocessor", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), + # container = ComposableNodeContainer( + # name=LaunchConfiguration("container_name"), + # namespace="", + # package="rclcpp_components", + # executable="component_container_mt", + # composable_node_descriptions=nodes, + # output="both", + # ) + + + + # driver_component = ComposableNode( + # package="nebula_ros", + # plugin=sensor_make + "HwInterfaceRosWrapper", + # # node is created in a global context, need to avoid name clash + # name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", + # parameters=[ + # { + # "sensor_model": sensor_model, + # "calibration_file": sensor_calib_fp, + # **create_parameter_dict( + # "sensor_ip", + # "host_ip", + # "scan_phase", + # "return_mode", + # "frame_id", + # "rotation_speed", + # "data_port", + # "cloud_min_angle", + # "cloud_max_angle", + # "dual_return_distance_threshold", + # "gnss_port", + # "packet_mtu_size", + # "setup_sensor", + # "ptp_profile", + # "ptp_transport_type", + # "ptp_switch_type", + # "ptp_domain", + # ), + # } + # ], + # ) + + # driver_component_loader = LoadComposableNodes( + # composable_node_descriptions=[driver_component], + # target_container=container, + # condition=IfCondition(LaunchConfiguration("launch_driver")), + # ) + + loader = LoadComposableNodes( composable_node_descriptions=nodes, - output="both", - ) - - driver_component = ComposableNode( - package="nebula_ros", - plugin=sensor_make + "HwInterfaceRosWrapper", - # node is created in a global context, need to avoid name clash - name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", - parameters=[ - { - "sensor_model": sensor_model, - "calibration_file": sensor_calib_fp, - **create_parameter_dict( - "sensor_ip", - "host_ip", - "scan_phase", - "return_mode", - "frame_id", - "rotation_speed", - "data_port", - "gnss_port", - "cloud_min_angle", - "cloud_max_angle", - "packet_mtu_size", - "dual_return_distance_threshold", - "setup_sensor", - "ptp_profile", - "ptp_tranport_type", - "ptp_switch_type", - "ptp_domain", - ), - } - ], - ) - - driver_component_loader = LoadComposableNodes( - composable_node_descriptions=[driver_component], - target_container=container, - condition=IfCondition(LaunchConfiguration("launch_driver")), + target_container=LaunchConfiguration("container_name"), ) - return [container, driver_component_loader] + # return [container] + return [loader] def generate_launch_description():