From 5aff1f1b26c9623cabb266565f541fee7e65a2a7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?A=2E=20Sena=20Y=C4=B1lmaz?= Date: Tue, 1 Oct 2024 15:43:21 +0300 Subject: [PATCH 1/3] feat : ring outlier filter load from param file MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: A. Sena Yılmaz --- .../distortion_corrector_node.param.yaml | 5 +++ .../ring_outlier_filter_node.param.yaml | 14 ++++++ .../launch/velodyne_node_container.launch.py | 43 ++++++++++++++++++- 3 files changed, 60 insertions(+), 2 deletions(-) create mode 100644 common_awsim_labs_sensor_launch/config/distortion_corrector_node.param.yaml create mode 100644 common_awsim_labs_sensor_launch/config/ring_outlier_filter_node.param.yaml 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..2f8b8eb 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,6 +59,16 @@ 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 = [] cropbox_parameters = create_parameter_dict("input_frame", "output_frame") @@ -106,6 +118,10 @@ def create_parameter_dict(*args): ) ) + 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", @@ -115,6 +131,7 @@ def create_parameter_dict(*args): ("input", "rectified/pointcloud_ex"), ("output", "pointcloud"), ], + parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -139,6 +156,7 @@ def create_parameter_dict(*args): ("~/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")}], ) @@ -178,17 +196,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", From 5bce945f7ce318f04e7da3813c197135717b66ac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?A=2E=20Sena=20Y=C4=B1lmaz?= Date: Tue, 1 Oct 2024 17:45:15 +0300 Subject: [PATCH 2/3] fix topic names MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: A. Sena Yılmaz --- .../launch/pointcloud_preprocessor.launch.py | 7 +- .../launch/velodyne_node_container.launch.py | 85 +++++++++++-------- 2 files changed, 52 insertions(+), 40 deletions(-) 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(): From 993f3a5533f2b69d6959155613cea39a541ef882 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?A=2E=20Sena=20Y=C4=B1lmaz?= Date: Wed, 2 Oct 2024 11:55:21 +0300 Subject: [PATCH 3/3] fix ring outlier filter node MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: A. Sena Yılmaz --- .../launch/velodyne_node_container.launch.py | 36 +++---------------- 1 file changed, 5 insertions(+), 31 deletions(-) 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 b323cb8..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 @@ -141,10 +141,10 @@ def create_parameter_dict(*args): ) # 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 + # 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", @@ -154,7 +154,7 @@ def create_parameter_dict(*args): ("input", "rectified/pointcloud_ex"), ("output", "pointcloud_before_sync"), ], - parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame], + parameters=[ring_outlier_filter_node_param], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -170,32 +170,6 @@ def create_parameter_dict(*args): output="both", ) - # 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]