diff --git a/aip_x1_1_launch/launch/imu.launch.xml b/aip_x1_1_launch/launch/imu.launch.xml index 2479629d..30207f8d 100644 --- a/aip_x1_1_launch/launch/imu.launch.xml +++ b/aip_x1_1_launch/launch/imu.launch.xml @@ -1,11 +1,12 @@ - - - - + + + + + @@ -19,10 +20,18 @@ + + - + - + + + + + + + diff --git a/aip_x1_1_launch/launch/pandar_node_container.launch.py b/aip_x1_1_launch/launch/pandar_node_container.launch.py index e84947fc..ffc2a70c 100644 --- a/aip_x1_1_launch/launch/pandar_node_container.launch.py +++ b/aip_x1_1_launch/launch/pandar_node_container.launch.py @@ -39,7 +39,6 @@ def get_pandar_monitor_info(): p = yaml.safe_load(f)["/**"]["ros__parameters"] return p - def launch_setup(context, *args, **kwargs): def create_parameter_dict(*args): result = {} diff --git a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py index b5c8601b..183f3681 100644 --- a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py @@ -30,7 +30,10 @@ def launch_setup(context, *args, **kwargs): package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_data", - remappings=[("output", "concatenated/pointcloud")], + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "concatenated/pointcloud") + ], parameters=[ { "input_topics": [ @@ -39,6 +42,7 @@ def launch_setup(context, *args, **kwargs): ], "output_frame": LaunchConfiguration("base_frame"), "timeout_sec": 1.0, + "input_twist_topic_type": "twist", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/aip_x1_1_launch/launch/velodyne_node_container.launch.py b/aip_x1_1_launch/launch/velodyne_node_container.launch.py index edcef126..3f644f33 100644 --- a/aip_x1_1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_1_launch/launch/velodyne_node_container.launch.py @@ -104,15 +104,30 @@ def create_parameter_dict(*args): ) ) + # nodes.append( + # ComposableNode( + # package="velodyne_pointcloud", + # plugin="velodyne_pointcloud::Interpolate", + # name="velodyne_interpolate_node", + # remappings=[ + # ("velodyne_points_ex", "self_cropped/pointcloud_ex"), + # ("velodyne_points_interpolate", "rectified/pointcloud"), + # ("velodyne_points_interpolate_ex", "rectified/pointcloud_ex"), + # ], + # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + # ) + # ) + nodes.append( ComposableNode( - package="velodyne_pointcloud", - plugin="velodyne_pointcloud::Interpolate", - name="velodyne_interpolate_node", + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::DistortionCorrectorComponent", + name="distortion_corrector_node", remappings=[ - ("velodyne_points_ex", "self_cropped/pointcloud_ex"), - ("velodyne_points_interpolate", "rectified/pointcloud"), - ("velodyne_points_interpolate_ex", "rectified/pointcloud_ex"), + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("~/input/imu", "/sensing/imu/imu_data"), + ("~/input/pointcloud", "self_cropped/pointcloud_ex"), + ("~/output/pointcloud", "rectified/pointcloud_ex"), ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], )