diff --git a/awsim_labs_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/awsim_labs_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py index 26eaa1b..e296121 100644 --- a/awsim_labs_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py +++ b/awsim_labs_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py @@ -39,13 +39,14 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "input_topics": [ - "/sensing/lidar/top/pointcloud", - "/sensing/lidar/left/pointcloud", - "/sensing/lidar/right/pointcloud", + "/sensing/lidar/top/pointcloud_before_sync", + "/sensing/lidar/left/pointcloud_before_sync", + "/sensing/lidar/right/pointcloud_before_sync", ], "output_frame": LaunchConfiguration("base_frame"), "timeout_sec": 0.01, "input_twist_topic_type": "twist", + "publish_synchronized_pointcloud": True, } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py b/common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py index 2f8b8eb..b323cb8 100644 --- a/common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py +++ b/common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py @@ -71,6 +71,13 @@ def create_parameter_dict(*args): nodes = [] + nodes.append( + ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + ) cropbox_parameters = create_parameter_dict("input_frame", "output_frame") cropbox_parameters["negative"] = True @@ -117,7 +124,23 @@ def create_parameter_dict(*args): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) + nodes.append( + ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent", + name="distortion_corrector_node", + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("~/input/imu", "/sensing/imu/imu_data"), + ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), + ("~/output/pointcloud", "rectified/pointcloud_ex"), + ], + parameters=[distortion_corrector_node_param], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + # Ring Outlier Filter is the last component in the pipeline, so control the output frame here if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true": ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")} else: @@ -129,7 +152,7 @@ def create_parameter_dict(*args): name="ring_outlier_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), + ("output", "pointcloud_before_sync"), ], parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -144,48 +167,36 @@ def create_parameter_dict(*args): package="rclcpp_components", executable=LaunchConfiguration("container_executable"), composable_node_descriptions=nodes, + output="both", ) - distortion_component = ComposableNode( - package="autoware_pointcloud_preprocessor", - plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent", - name="distortion_corrector_node", - remappings=[ - ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), - ("~/input/imu", "/sensing/imu/imu_data"), - ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), - ("~/output/pointcloud", "rectified/pointcloud_ex"), - ], - parameters=[distortion_corrector_node_param], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - distortion_relay_component = ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="pointcloud_distortion_relay", - namespace="", - parameters=[ - {"input_topic": "mirror_cropped/pointcloud_ex"}, - {"output_topic": "rectified/pointcloud_ex"} - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) + # distortion_relay_component = ComposableNode( + # package="topic_tools", + # plugin="topic_tools::RelayNode", + # name="pointcloud_distortion_relay", + # namespace="", + # parameters=[ + # {"input_topic": "mirror_cropped/pointcloud_ex"}, + # {"output_topic": "rectified/pointcloud_ex"} + # ], + # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + # ) # one way to add a ComposableNode conditional on a launch argument to a # container. The `ComposableNode` itself doesn't accept a condition - distortion_loader = LoadComposableNodes( - composable_node_descriptions=[distortion_component], - target_container=container, - condition=launch.conditions.IfCondition(LaunchConfiguration("use_distortion_corrector")), - ) - distortion_relay_loader = LoadComposableNodes( - composable_node_descriptions=[distortion_relay_component], - target_container=container, - condition=launch.conditions.UnlessCondition(LaunchConfiguration("use_distortion_corrector")), - ) + # distortion_loader = LoadComposableNodes( + # composable_node_descriptions=[distortion_component], + # target_container=container, + # condition=launch.conditions.IfCondition(LaunchConfiguration("use_distortion_corrector")), + # ) + # distortion_relay_loader = LoadComposableNodes( + # composable_node_descriptions=[distortion_relay_component], + # target_container=container, + # condition=launch.conditions.UnlessCondition(LaunchConfiguration("use_distortion_corrector")), + # ) + # return [container, distortion_loader, distortion_relay_loader] - return [container, distortion_loader, distortion_relay_loader] + return [container] def generate_launch_description():