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/config/distortion_corrector_node.param.yaml b/common_awsim_labs_sensor_launch/config/distortion_corrector_node.param.yaml new file mode 100644 index 0000000..3afa481 --- /dev/null +++ b/common_awsim_labs_sensor_launch/config/distortion_corrector_node.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + base_frame: base_link + use_imu: true + use_3d_distortion_correction: false diff --git a/common_awsim_labs_sensor_launch/config/ring_outlier_filter_node.param.yaml b/common_awsim_labs_sensor_launch/config/ring_outlier_filter_node.param.yaml new file mode 100644 index 0000000..76bf689 --- /dev/null +++ b/common_awsim_labs_sensor_launch/config/ring_outlier_filter_node.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + distance_ratio: 1.03 + object_length_threshold: 0.1 + num_points_threshold: 4 + max_rings_num: 128 + max_points_num_per_ring: 4000 + publish_outlier_pointcloud: false + min_azimuth_deg: 0.0 + max_azimuth_deg: 360.0 + max_distance: 12.0 + vertical_bins: 128 + horizontal_bins: 36 + noise_threshold: 2 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 87565eb..0f5993c 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 @@ -11,7 +11,8 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. - +import os +from ament_index_python.packages import get_package_share_directory import launch from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction @@ -22,6 +23,7 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +from launch_ros.parameter_descriptions import ParameterFile import yaml @@ -57,8 +59,25 @@ def create_parameter_dict(*args): result[x] = LaunchConfiguration(x) return result + # Pointcloud preprocessor parameters + distortion_corrector_node_param = ParameterFile( + param_file=LaunchConfiguration("distortion_correction_node_param_path").perform(context), + allow_substs=True, + ) + ring_outlier_filter_node_param = ParameterFile( + param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(context), + allow_substs=True, + ) + 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 @@ -105,7 +124,27 @@ 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: + # ring_outlier_output_frame = {"output_frame": ""} # keep the output frame as the input frame nodes.append( ComposableNode( package="autoware_pointcloud_preprocessor", @@ -113,8 +152,9 @@ 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], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -127,47 +167,10 @@ 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"), - ], - 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")), - ) - - return [container, distortion_loader, distortion_relay_loader] + return [container] def generate_launch_description(): @@ -178,17 +181,38 @@ def add_launch_arg(name: str, default_value=None, description=None): launch_arguments.append( DeclareLaunchArgument(name, default_value=default_value, description=description) ) + + common_sensor_share_dir = get_package_share_directory("common_sensor_launch") add_launch_arg("base_frame", "base_link", "base frame id") add_launch_arg("container_name", "velodyne_composable_node_container", "container name") add_launch_arg("input_frame", LaunchConfiguration("base_frame"), "use for cropbox") add_launch_arg("output_frame", LaunchConfiguration("base_frame"), "use for cropbox") + add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") add_launch_arg( "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" ) + add_launch_arg("frame_id", "lidar", "frame id") add_launch_arg("use_multithread", "False", "use multithread") add_launch_arg("use_intra_process", "False", "use ROS2 component container communication") - + add_launch_arg( + "distortion_correction_node_param_path", + os.path.join( + common_sensor_share_dir, + "config", + "distortion_corrector_node.param.yaml", + ), + description="path to parameter file of distortion correction node", + ) + add_launch_arg( + "ring_outlier_filter_node_param_path", + os.path.join( + common_sensor_share_dir, + "config", + "ring_outlier_filter_node.param.yaml", + ), + description="path to parameter file of ring outlier filter node", + ) set_container_executable = SetLaunchConfiguration( "container_executable", "component_container",