From 46702405b8777b8481cc518cc0818cc8b7d9145d Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Mon, 15 May 2023 12:26:23 +0900 Subject: [PATCH 01/50] feat: add XT32 sensor model Signed-off-by: 1222-takeshi --- aip_x1_1_description/CMakeLists.txt | 11 + .../config/sensor_kit_calibration.yaml | 22 + .../config/sensors_calibration.yaml | 8 + aip_x1_1_description/package.xml | 18 + aip_x1_1_description/urdf/sensor_kit.xacro | 66 +++ aip_x1_1_description/urdf/sensors.xacro | 21 + aip_x1_1_launch/CMakeLists.txt | 15 + .../sensor_kit.param.yaml | 58 ++ .../elevation_map_parameters.yaml | 30 + .../ground_segmentation.param.yaml | 51 ++ .../ground_segmentation.launch.py | 533 ++++++++++++++++++ aip_x1_1_launch/launch/imu.launch.xml | 29 + aip_x1_1_launch/launch/lidar.launch.xml | 53 ++ .../launch/pandar_node_container.launch.py | 184 ++++++ .../launch/pointcloud_preprocessor.launch.py | 101 ++++ aip_x1_1_launch/launch/sensing.launch.xml | 25 + .../launch/topic_state_monitor.launch.py | 155 +++++ .../launch/velodyne_VLP16.launch.xml | 41 ++ .../launch/velodyne_node_container.launch.py | 233 ++++++++ aip_x1_1_launch/package.xml | 36 ++ 20 files changed, 1690 insertions(+) create mode 100644 aip_x1_1_description/CMakeLists.txt create mode 100644 aip_x1_1_description/config/sensor_kit_calibration.yaml create mode 100644 aip_x1_1_description/config/sensors_calibration.yaml create mode 100644 aip_x1_1_description/package.xml create mode 100644 aip_x1_1_description/urdf/sensor_kit.xacro create mode 100644 aip_x1_1_description/urdf/sensors.xacro create mode 100644 aip_x1_1_launch/CMakeLists.txt create mode 100644 aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml create mode 100644 aip_x1_1_launch/config/ground_segmentation/elevation_map_parameters.yaml create mode 100644 aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml create mode 100644 aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py create mode 100644 aip_x1_1_launch/launch/imu.launch.xml create mode 100644 aip_x1_1_launch/launch/lidar.launch.xml create mode 100644 aip_x1_1_launch/launch/pandar_node_container.launch.py create mode 100644 aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py create mode 100644 aip_x1_1_launch/launch/sensing.launch.xml create mode 100644 aip_x1_1_launch/launch/topic_state_monitor.launch.py create mode 100644 aip_x1_1_launch/launch/velodyne_VLP16.launch.xml create mode 100644 aip_x1_1_launch/launch/velodyne_node_container.launch.py create mode 100644 aip_x1_1_launch/package.xml diff --git a/aip_x1_1_description/CMakeLists.txt b/aip_x1_1_description/CMakeLists.txt new file mode 100644 index 00000000..9e5669bf --- /dev/null +++ b/aip_x1_1_description/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_x1_1_description) + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_package(INSTALL_TO_SHARE + urdf + config +) diff --git a/aip_x1_1_description/config/sensor_kit_calibration.yaml b/aip_x1_1_description/config/sensor_kit_calibration.yaml new file mode 100644 index 00000000..e8955379 --- /dev/null +++ b/aip_x1_1_description/config/sensor_kit_calibration.yaml @@ -0,0 +1,22 @@ +sensor_kit_base_link: + velodyne_top_base_link: + x: 0.000 + y: 0.000 + z: 0.000 + roll: 0.000 + pitch: 0.000 + yaw: 0.000 + pandar_xt32_front_center_base_link: + x: 1.130 + y: 0.038 + z: -1.400 + roll: -0.000 + pitch: -0.00 + yaw: 1.6225 + tamagawa/imu_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 3.14159 + pitch: 0.0 + yaw: 0.0 diff --git a/aip_x1_1_description/config/sensors_calibration.yaml b/aip_x1_1_description/config/sensors_calibration.yaml new file mode 100644 index 00000000..796672f1 --- /dev/null +++ b/aip_x1_1_description/config/sensors_calibration.yaml @@ -0,0 +1,8 @@ +base_link: + sensor_kit_base_link: + x: 0.555 + y: 0.000 + z: 1.810 + roll: 0.000 + pitch: 0.000 + yaw: 0.000 diff --git a/aip_x1_1_description/package.xml b/aip_x1_1_description/package.xml new file mode 100644 index 00000000..382b29e2 --- /dev/null +++ b/aip_x1_1_description/package.xml @@ -0,0 +1,18 @@ + + + aip_x1_1_description + 0.1.0 + The aip_x1_1_description package + + Yohei Mishina + Apache 2 + + ament_cmake_auto + + pandar_description + velodyne_description + + + ament_cmake + + diff --git a/aip_x1_1_description/urdf/sensor_kit.xacro b/aip_x1_1_description/urdf/sensor_kit.xacro new file mode 100644 index 00000000..66508144 --- /dev/null +++ b/aip_x1_1_description/urdf/sensor_kit.xacro @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x1_1_description/urdf/sensors.xacro b/aip_x1_1_description/urdf/sensors.xacro new file mode 100644 index 00000000..6690fb8f --- /dev/null +++ b/aip_x1_1_description/urdf/sensors.xacro @@ -0,0 +1,21 @@ + + + + + + + + + + + + + diff --git a/aip_x1_1_launch/CMakeLists.txt b/aip_x1_1_launch/CMakeLists.txt new file mode 100644 index 00000000..707bd8ce --- /dev/null +++ b/aip_x1_1_launch/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_x1_1_launch) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml new file mode 100644 index 00000000..7c6f52f5 --- /dev/null +++ b/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -0,0 +1,58 @@ +/**: + ros__parameters: + sensing: + type: diagnostic_aggregator/AnalyzerGroup + path: sensing + analyzers: + lidar: + type: diagnostic_aggregator/AnalyzerGroup + path: lidar + analyzers: + velodyne: + type: diagnostic_aggregator/AnalyzerGroup + path: velodyne + analyzers: + health_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: health_monitoring + analyzers: + connection: + type: diagnostic_aggregator/GenericAnalyzer + path: connection + contains: [": velodyne_connection"] + timeout: 3.0 + + temperature: + type: diagnostic_aggregator/GenericAnalyzer + path: temperature + contains: [": velodyne_temperature"] + timeout: 3.0 + + rpm: + type: diagnostic_aggregator/GenericAnalyzer + path: rpm + contains: [": velodyne_rpm"] + timeout: 3.0 + pandar: + type: diagnostic_aggregator/AnalyzerGroup + path: pandar + analyzers: + health_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: health_monitoring + analyzers: + connection: + type: diagnostic_aggregator/GenericAnalyzer + path: connection + contains: [": pandar_connection"] + timeout: 1.0 + temperature: + type: diagnostic_aggregator/GenericAnalyzer + path: temperature + contains: [": pandar_temperature"] + timeout: 1.0 + ptp: + type: diagnostic_aggregator/GenericAnalyzer + path: ptp + contains: [": pandar_ptp"] + timeout: 1.0 diff --git a/aip_x1_1_launch/config/ground_segmentation/elevation_map_parameters.yaml b/aip_x1_1_launch/config/ground_segmentation/elevation_map_parameters.yaml new file mode 100644 index 00000000..781ccb80 --- /dev/null +++ b/aip_x1_1_launch/config/ground_segmentation/elevation_map_parameters.yaml @@ -0,0 +1,30 @@ +pcl_grid_map_extraction: + num_processing_threads: 12 + cloud_transform: + translation: + x: 0.0 + y: 0.0 + z: 0.0 + rotation: # intrinsic rotation X-Y-Z (r-p-y)sequence + r: 0.0 + p: 0.0 + y: 0.0 + cluster_extraction: + cluster_tolerance: 0.2 + min_num_points: 3 + max_num_points: 1000000 + outlier_removal: + is_remove_outliers: false + mean_K: 10 + stddev_threshold: 1.0 + downsampling: + is_downsample_cloud: false + voxel_size: + x: 0.02 + y: 0.02 + z: 0.02 + grid_map: + min_num_points_per_cell: 3 + resolution: 0.3 + height_type: 1 + height_thresh: 1.0 diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml new file mode 100644 index 00000000..3bd7b101 --- /dev/null +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -0,0 +1,51 @@ +/**: + ros__parameters: + additional_lidars: [] + ransac_input_topics: [ + "/sensing/lidar/front_center/outlier_filtered/pointcloud" + ] + use_single_frame_filter: True + use_time_series_filter: False + + common_crop_box_filter: + parameters: + min_x: -50.0 + max_x: 100.0 + min_y: -50.0 + max_y: 50.0 + min_z: -0.5 + negative: False + + common_ground_filter: + plugin: "ground_segmentation::RayGroundFilterComponent" + parameters: + initial_max_slope: 10.0 + general_max_slope: 10.0 + local_max_slope: 10.0 + min_height_threshold: 0.3 + radial_divider_angle: 1.0 + concentric_divider_distance: 0.0 + use_vehicle_footprint: True + + short_height_obstacle_detection_area_filter: + parameters: + min_x: 0.0 + max_x: 19.6 # max_x: 18.0m + base_link2livox_front_center distance 1.6m + min_y: -4.0 + max_y: 4.0 + min_z: -0.5 + max_z: 0.5 + negative: False + + ransac_ground_filter: + parameters: + outlier_threshold: 0.1 + min_points: 400 + min_inliers: 200 + max_iterations: 50 + height_threshold: 0.15 + plane_slope_threshold: 10.0 + voxel_size_x: 0.2 + voxel_size_y: 0.2 + voxel_size_z: 0.2 + debug: False diff --git a/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py b/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py new file mode 100644 index 00000000..a3687bec --- /dev/null +++ b/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py @@ -0,0 +1,533 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# 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 +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +class GroundSegmentationPipeline: + def __init__(self, context): + self.context = context + self.vehicle_info = self.get_vehicle_info() + ground_segmentation_param_path = os.path.join( + get_package_share_directory("aip_x1_1_launch"), + "config/ground_segmentation/ground_segmentation.param.yaml", + ) + with open(ground_segmentation_param_path, "r") as f: + self.ground_segmentation_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + self.single_frame_obstacle_seg_output = ( + "/perception/obstacle_segmentation/single_frame/pointcloud_raw" + ) + self.output_topic = "/perception/obstacle_segmentation/pointcloud" + self.use_single_frame_filter = self.ground_segmentation_param["use_single_frame_filter"] + self.use_time_series_filter = self.ground_segmentation_param["use_time_series_filter"] + + def get_vehicle_info(self): + # TODO: need to rename key from "ros_params" to "global_params" after Humble + gp = self.context.launch_configurations.get("ros_params", {}) + p = {} + p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] + p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] + p["min_longitudinal_offset"] = -gp["rear_overhang"] + p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] + p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) + p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] + p["min_height_offset"] = 0.0 + p["max_height_offset"] = gp["vehicle_height"] + return p + + def get_vehicle_mirror_info(self): + path = LaunchConfiguration("vehicle_mirror_param_file").perform(self.context) + with open(path, "r") as f: + p = yaml.safe_load(f) + return p + + def create_additional_pipeline(self, lidar_name): + components = [] + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name=f"{lidar_name}_crop_box_filter", + remappings=[ + ("input", f"/sensing/lidar/{lidar_name}/outlier_filtered/pointcloud"), + ("output", f"{lidar_name}/range_cropped/pointcloud"), + ], + parameters=[ + { + "input_frame": LaunchConfiguration("base_frame"), + "output_frame": LaunchConfiguration("base_frame"), + "min_z": self.vehicle_info["min_height_offset"], + "max_z": self.vehicle_info["max_height_offset"], + }, + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"], + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + components.append( + ComposableNode( + package="ground_segmentation", + plugin=self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["plugin"], + name=f"{lidar_name}_ground_filter", + remappings=[ + ("input", f"{lidar_name}/range_cropped/pointcloud"), + ("output", f"{lidar_name}/pointcloud"), + ], + parameters=[ + self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["parameters"] + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + return components + + def create_ransac_pipeline(self): + components = [] + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="short_height_obstacle_detection_area_filter", + namespace="plane_fitting", + remappings=[ + ("input", self.ground_segmentation_param["ransac_input_topics"]), + ("output", "detection_area/pointcloud"), + ], + parameters=[ + { + "input_frame": LaunchConfiguration("base_frame"), + "output_frame": LaunchConfiguration("base_frame"), + }, + self.ground_segmentation_param["short_height_obstacle_detection_area_filter"][ + "parameters" + ], + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::Lanelet2MapFilterComponent", + name="vector_map_filter", + namespace="plane_fitting", + remappings=[ + ("input/pointcloud", "detection_area/pointcloud"), + ("input/vector_map", "/map/vector_map"), + ("output", "vector_map_filtered/pointcloud"), + ], + parameters=[ + { + "voxel_size_x": 0.25, + "voxel_size_y": 0.25, + } + ], + # cannot use intra process because vector map filter uses transient local. + extra_arguments=[{"use_intra_process_comms": False}], + ) + ) + + components.append( + ComposableNode( + package="ground_segmentation", + plugin="ground_segmentation::RANSACGroundFilterComponent", + name="ransac_ground_filter", + namespace="plane_fitting", + remappings=[ + ("input", "vector_map_filtered/pointcloud"), + ("output", "pointcloud"), + ], + parameters=[self.ground_segmentation_param["ransac_ground_filter"]["parameters"]], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + return components + + def create_common_pipeline(self, input_topic, output_topic): + components = [] + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter", + remappings=[ + ("input", input_topic), + ("output", "range_cropped/pointcloud"), + ], + parameters=[ + { + "input_frame": LaunchConfiguration("base_frame"), + "output_frame": LaunchConfiguration("base_frame"), + "min_z": self.vehicle_info["min_height_offset"], + "max_z": self.vehicle_info["max_height_offset"], + }, + self.ground_segmentation_param["common_crop_box_filter"]["parameters"], + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + components.append( + ComposableNode( + package="ground_segmentation", + plugin=self.ground_segmentation_param["common_ground_filter"]["plugin"], + name="common_ground_filter", + remappings=[ + ("input", "range_cropped/pointcloud"), + ("output", output_topic), + ], + parameters=[self.ground_segmentation_param["common_ground_filter"]["parameters"]], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", + name="voxel_grid_outlier_filter_single_frame", + remappings=[ + ("input", output_topic), + ("output", output_topic + "_outlier_filtered"), + ], + parameters=[ + { + "voxel_size_x": 0.4, + "voxel_size_y": 0.4, + "voxel_size_z": 100.0, + "voxel_points_threshold": 5, + } + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + return components + + def create_single_frame_obstacle_segmentation_components(self, input_topic, output_topic): + additional_lidars = self.ground_segmentation_param["additional_lidars"] + use_ransac = bool(self.ground_segmentation_param["ransac_input_topics"]) + use_additional = bool(additional_lidars) + relay_topic = "all_lidars/pointcloud" + common_pipeline_output = ( + "single_frame/pointcloud" if use_additional or use_ransac else output_topic + ) + + components = self.create_common_pipeline( + input_topic=input_topic, + output_topic=common_pipeline_output, + ) + + if use_additional: + for lidar_name in additional_lidars: + components.extend(self.create_additional_pipeline(lidar_name)) + components.append( + self.get_additional_lidars_concatenated_component( + input_topics=[common_pipeline_output] + + [f"{x}/pointcloud" for x in additional_lidars], + output_topic=relay_topic if use_ransac else output_topic, + ) + ) + + if use_ransac: + components.extend(self.create_ransac_pipeline()) + components.append( + self.get_single_frame_obstacle_segmentation_concatenated_component( + input_topics=[ + "plane_fitting/pointcloud", + relay_topic if use_additional else common_pipeline_output, + ], + output_topic=output_topic, + ) + ) + + return components + + @staticmethod + def create_time_series_outlier_filter_components(input_topic, output_topic): + components = [] + components.append( + ComposableNode( + package="occupancy_grid_map_outlier_filter", + plugin="occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent", + name="occupancy_grid_map_outlier_filter", + remappings=[ + ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"), + ("~/input/pointcloud", input_topic), + ("~/output/pointcloud", output_topic), + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + return components + + @staticmethod + def create_single_frame_outlier_filter_components(input_topic, output_topic): + components = [] + components.append( + ComposableNode( + package="elevation_map_loader", + plugin="ElevationMapLoaderNode", + name="elevation_map_loader", + namespace="elevation_map", + remappings=[ + ("output/elevation_map", "map"), + ("input/pointcloud_map", "/map/pointcloud_map"), + ("input/vector_map", "/map/vector_map"), + ], + parameters=[ + { + "use_lane_filter": True, + "lane_margin": 2.0, + "use_inpaint": True, + "inpaint_radius": 1.0, + "param_file_path": PathJoinSubstitution( + [ + FindPackageShare("aip_x1_1_launch"), + "config", + "ground_segmentation", + "elevation_map_parameters.yaml", + ] + ), + "elevation_map_directory": PathJoinSubstitution( + [FindPackageShare("elevation_map_loader"), "data", "elevation_maps"] + ), + "use_elevation_map_cloud_publisher": False, + } + ], + extra_arguments=[{"use_intra_process_comms": False}], + ) + ) + + components.append( + ComposableNode( + package="compare_map_segmentation", + plugin="compare_map_segmentation::CompareElevationMapFilterComponent", + name="compare_elevation_map_filter", + namespace="elevation_map", + remappings=[ + ("input", input_topic), + ("output", "map_filtered/pointcloud"), + ("input/elevation_map", "map"), + ], + parameters=[ + { + "map_frame": "map", + "map_layer_name": "elevation", + "height_diff_thresh": 0.15, + "input_frame": "map", + "output_frame": "base_link", + } + ], + extra_arguments=[ + {"use_intra_process_comms": False} + ], # can't use this with transient_local + ) + ) + + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + name="voxel_grid_filter", + namespace="elevation_map", + remappings=[ + ("input", "map_filtered/pointcloud"), + ("output", "voxel_grid_filtered/pointcloud"), + ], + parameters=[ + { + "input_frame": LaunchConfiguration("base_frame"), + "output_frame": LaunchConfiguration("base_frame"), + "voxel_size_x": 0.04, + "voxel_size_y": 0.04, + "voxel_size_z": 0.08, + } + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", + name="voxel_grid_outlier_filter", + namespace="elevation_map", + remappings=[ + ("input", "voxel_grid_filtered/pointcloud"), + ("output", output_topic), + ], + parameters=[ + { + "voxel_size_x": 0.4, + "voxel_size_y": 0.4, + "voxel_size_z": 100.0, + "voxel_points_threshold": 5, + } + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + return components + + @staticmethod + def get_additional_lidars_concatenated_component(input_topics, output_topic): + return ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="concatenate_data", + remappings=[("output", output_topic)], + parameters=[ + { + "input_topics": input_topics, + "output_frame": LaunchConfiguration("base_frame"), + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + @staticmethod + def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, output_topic): + return ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="concatenate_no_ground_data", + remappings=[("output", output_topic)], + parameters=[ + { + "input_topics": input_topics, + "output_frame": LaunchConfiguration("base_frame"), + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + +def launch_setup(context, *args, **kwargs): + pipeline = GroundSegmentationPipeline(context) + + components = [] + components.extend( + pipeline.create_single_frame_obstacle_segmentation_components( + input_topic="/sensing/lidar/concatenated/pointcloud", + output_topic=pipeline.single_frame_obstacle_seg_output, + ) + ) + + relay_topic = "single_frame/filtered/pointcloud" + if pipeline.use_single_frame_filter: + components.extend( + pipeline.create_single_frame_outlier_filter_components( + input_topic=pipeline.single_frame_obstacle_seg_output, + output_topic=relay_topic + if pipeline.use_time_series_filter + else pipeline.output_topic, + ) + ) + if pipeline.use_time_series_filter: + components.extend( + pipeline.create_time_series_outlier_filter_components( + input_topic=relay_topic + if pipeline.use_single_frame_filter + else pipeline.single_frame_obstacle_seg_output, + output_topic=pipeline.output_topic, + ) + ) + individual_container = ComposableNodeContainer( + name=LaunchConfiguration("container_name"), + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=components, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + output="screen", + ) + pointcloud_container_loader = LoadComposableNodes( + composable_node_descriptions=components, + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + return [individual_container, pointcloud_container_loader] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None): + launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) + + add_launch_arg("base_frame", "base_link") + add_launch_arg("use_multithread", "False") + add_launch_arg("use_intra_process", "True") + add_launch_arg("use_pointcloud_container", "False") + add_launch_arg("container_name", "perception_pipeline_container") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/aip_x1_1_launch/launch/imu.launch.xml b/aip_x1_1_launch/launch/imu.launch.xml new file mode 100644 index 00000000..2479629d --- /dev/null +++ b/aip_x1_1_launch/launch/imu.launch.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x1_1_launch/launch/lidar.launch.xml b/aip_x1_1_launch/launch/lidar.launch.xml new file mode 100644 index 00000000..115de1f9 --- /dev/null +++ b/aip_x1_1_launch/launch/lidar.launch.xml @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x1_1_launch/launch/pandar_node_container.launch.py b/aip_x1_1_launch/launch/pandar_node_container.launch.py new file mode 100644 index 00000000..cf635604 --- /dev/null +++ b/aip_x1_1_launch/launch/pandar_node_container.launch.py @@ -0,0 +1,184 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# 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 +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode +import yaml + + +def get_pandar_monitor_info(): + path = os.path.join( + get_package_share_directory("pandar_monitor"), + "config", + "pandar_monitor.param.yaml", + ) + with open(path, "r") as f: + p = yaml.safe_load(f)["/**"]["ros__parameters"] + return p + + +def launch_setup(context, *args, **kwargs): + def create_parameter_dict(*args): + result = {} + for x in args: + result[x] = LaunchConfiguration(x) + return result + + monitor_node = Node( + executable="pandar_monitor_node", + package="pandar_monitor", + name="pandar_monitor", + parameters=[ + { + "ip_address": LaunchConfiguration("device_ip"), + } + ] + + [get_pandar_monitor_info()], + condition=IfCondition(LaunchConfiguration("launch_driver")), + output="screen", + ) + + driver_component = ComposableNode( + package="pandar_driver", + plugin="pandar_driver::PandarDriver", + name="pandar_driver", + parameters=[ + { + **create_parameter_dict( + "pcap", "device_ip", "lidar_port", "gps_port", "scan_phase", "model", "frame_id" + ) + } + ], + ) + + pointcloud_component = ComposableNode( + package="pandar_pointcloud", + plugin="pandar_pointcloud::PandarCloud", + name="pandar_cloud", + parameters=[ + { + **create_parameter_dict( + "model", + "scan_phase", + "angle_range", + "distance_range", + "device_ip", + "calibration", + "return_mode", + ) + } + ], + remappings=[("pandar_points", "pointcloud_raw"), ("pandar_points_ex", "pointcloud_raw_ex")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + undistort_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::DistortionCorrectorComponent", + name="distortion_corrector_node", + remappings=[ + ("~/input/velocity_report", "/vehicle/status/velocity_status"), + ("~/input/pointcloud", "pointcloud_raw_ex"), + ("~/output/pointcloud", "rectified/pointcloud_ex"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + ring_outlier_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::RingOutlierFilterComponent", + name="ring_outlier_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "outlier_filtered/pointcloud"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + container = ComposableNodeContainer( + name="pandar_node_container", + namespace="pointcloud_preprocessor", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + pointcloud_component, + undistort_component, + ring_outlier_filter_component, + ], + ) + + driver_loader = LoadComposableNodes( + composable_node_descriptions=[driver_component], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("launch_driver")), + ) + + return [ + container, + driver_loader, + monitor_node, + ] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None): + launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) + + add_launch_arg("model") + add_launch_arg("launch_driver", "true") + add_launch_arg("calibration", "") + add_launch_arg("device_ip", "192.168.1.201") + add_launch_arg("scan_phase", "0.0") + add_launch_arg("angle_range", "[0.0, 360.0]") + add_launch_arg("distance_range", "[0.05, 200.0]") + add_launch_arg("return_mode", "Dual") + add_launch_arg("container_name", "pandar_composable_node_container") + add_launch_arg("pcap", "") + add_launch_arg("lidar_port", "2321") + add_launch_arg("gps_port", "10121") + add_launch_arg("frame_id", "pandar") + add_launch_arg("use_multithread", "False") + add_launch_arg("use_intra_process", "False") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py new file mode 100644 index 00000000..b5c8601b --- /dev/null +++ b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py @@ -0,0 +1,101 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# 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 launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def launch_setup(context, *args, **kwargs): + # set concat filter as a component + concat_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="concatenate_data", + remappings=[("output", "concatenated/pointcloud")], + parameters=[ + { + "input_topics": [ + "/sensing/lidar/top/outlier_filtered/pointcloud", + "/sensing/lidar/front_center/pointcloud_raw", + ], + "output_frame": LaunchConfiguration("base_frame"), + "timeout_sec": 1.0, + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # set container to run all required components in the same process + container = ComposableNodeContainer( + name=LaunchConfiguration("container_name"), + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[], + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + output="screen", + ) + + target_container = ( + container + if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) + else LaunchConfiguration("container_name") + ) + + # load concat or passthrough filter + concat_loader = LoadComposableNodes( + composable_node_descriptions=[concat_component], + target_container=target_container, + ) + + return [container, concat_loader] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None): + launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) + + add_launch_arg("base_frame", "base_link") + add_launch_arg("use_multithread", "False") + add_launch_arg("use_intra_process", "False") + add_launch_arg("use_pointcloud_container", "False") + add_launch_arg("container_name", "pointcloud_preprocessor_container") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/aip_x1_1_launch/launch/sensing.launch.xml b/aip_x1_1_launch/launch/sensing.launch.xml new file mode 100644 index 00000000..3dafb3d1 --- /dev/null +++ b/aip_x1_1_launch/launch/sensing.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x1_1_launch/launch/topic_state_monitor.launch.py b/aip_x1_1_launch/launch/topic_state_monitor.launch.py new file mode 100644 index 00000000..29bc9f69 --- /dev/null +++ b/aip_x1_1_launch/launch/topic_state_monitor.launch.py @@ -0,0 +1,155 @@ +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# 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. + +from launch.launch_description import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + # Topic Monitor For Front Lidar PointCloud + topic_state_monitor_pandar_front_center = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_pandar_front_center", + parameters=[ + { + "topic": "/sensing/lidar/front_center/pointcloud_raw", + "topic_type": "sensor_msgs/msg/PointCloud2", + "best_effort": True, + "diag_name": "sensing_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 1.0, + "window_size": 10, + }, + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # Topic Monitor For Concat PointCloud + topic_state_monitor_top_outlier_filtered = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_top_outlier_filtered", + parameters=[ + { + "topic": "/sensing/lidar/top/outlier_filtered/pointcloud", + "topic_type": "sensor_msgs/msg/PointCloud2", + "best_effort": True, + "diag_name": "sensing_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 1.0, + "window_size": 10, + }, + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + topic_state_monitor_pandar_front_center_outlier_filtered = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_pandar_front_center_outlier_filtered", + parameters=[ + { + "topic": "/sensing/lidar/front_center/outlier_filtered/pointcloud", + "topic_type": "sensor_msgs/msg/PointCloud2", + "best_effort": True, + "diag_name": "sensing_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 1.0, + "window_size": 10, + }, + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # Topic Monitor for NoGroundFilter + topic_state_monitor_rough_no_ground = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_rough_no_ground", + parameters=[ + { + "topic": "/perception/obstacle_segmentation/single_frame/pointcloud_raw", + "topic_type": "sensor_msgs/msg/PointCloud2", + "best_effort": True, + "diag_name": "sensing_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 1.0, + "window_size": 10, + }, + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + topic_state_monitor_short_height_no_ground = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_short_height_no_ground", + parameters=[ + { + "topic": "/perception/obstacle_segmentation/plane_fitting/pointcloud", + "topic_type": "sensor_msgs/msg/PointCloud2", + "best_effort": True, + "diag_name": "sensing_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 1.0, + "window_size": 10, + }, + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # topic monitor for tamagawa IMU + topic_state_monitor_imu_data = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_imu_data", + parameters=[ + { + "topic": "/sensing/imu/imu_data", + "topic_type": "sensor_msgs/msg/Imu", + "best_effort": False, + "diag_name": "sensing_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 1.0, + "window_size": 10, + }, + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # set container to run all required components in the same process + container = ComposableNodeContainer( + name="topic_state_monitor_container", + namespace="topic_state_monitor", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + topic_state_monitor_pandar_front_center, + topic_state_monitor_top_outlier_filtered, + topic_state_monitor_pandar_front_center_outlier_filtered, + topic_state_monitor_rough_no_ground, + topic_state_monitor_short_height_no_ground, + topic_state_monitor_imu_data, + ], + output="screen", + ) + + return LaunchDescription([container]) diff --git a/aip_x1_1_launch/launch/velodyne_VLP16.launch.xml b/aip_x1_1_launch/launch/velodyne_VLP16.launch.xml new file mode 100644 index 00000000..8ad01878 --- /dev/null +++ b/aip_x1_1_launch/launch/velodyne_VLP16.launch.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x1_1_launch/launch/velodyne_node_container.launch.py b/aip_x1_1_launch/launch/velodyne_node_container.launch.py new file mode 100644 index 00000000..edffc716 --- /dev/null +++ b/aip_x1_1_launch/launch/velodyne_node_container.launch.py @@ -0,0 +1,233 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# 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 launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def get_vehicle_info(context): + gp = context.launch_configurations.get("ros_params", {}) + p = {} + p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] + p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] + p["min_longitudinal_offset"] = -gp["rear_overhang"] + p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] + p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) + p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] + p["min_height_offset"] = 0.0 + p["max_height_offset"] = gp["vehicle_height"] + return p + + +def launch_setup(context, *args, **kwargs): + def create_parameter_dict(*args): + result = {} + for x in args: + result[x] = LaunchConfiguration(x) + return result + + nodes = [] + + # turn packets into pointcloud as in + # https://github.com/ros-drivers/velodyne/blob/ros2/velodyne_pointcloud/launch/velodyne_convert_node-VLP16-composed-launch.py + nodes.append( + ComposableNode( + package="velodyne_pointcloud", + plugin="velodyne_pointcloud::Convert", + name="velodyne_convert_node", + parameters=[ + { + **create_parameter_dict( + "calibration", + "min_range", + "max_range", + "num_points_thresholds", + "invalid_intensity", + "frame_id", + "scan_phase", + "view_direction", + "view_width", + ), + } + ], + remappings=[ + ("velodyne_points", "pointcloud_raw"), + ("velodyne_points_ex", "pointcloud_raw_ex"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") + cropbox_parameters["negative"] = True + + vehicle_info = get_vehicle_info(context) + cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] + cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] + cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] + cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] + cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] + cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] + + nodes.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_self", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "self_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + 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="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::RingOutlierFilterComponent", + name="ring_outlier_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "outlier_filtered/pointcloud"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + # set container to run all required components in the same process + container = ComposableNodeContainer( + # need unique name, otherwise all processes in same container and the node names then clash + name="velodyne_node_container", + namespace="pointcloud_preprocessor", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=nodes, + ) + + driver_component = ComposableNode( + package="velodyne_driver", + plugin="velodyne_driver::VelodyneDriver", + # node is created in a global context, need to avoid name clash + name="velodyne_driver", + parameters=[ + { + **create_parameter_dict( + "device_ip", + "gps_time", + "read_once", + "read_fast", + "repeat_delay", + "frame_id", + "model", + "rpm", + "port", + "pcap", + "scan_phase", + ), + } + ], + ) + + # one way to add a ComposableNode conditional on a launch argument to a + # container. The `ComposableNode` itself doesn't accept a condition + loader = LoadComposableNodes( + composable_node_descriptions=[driver_component], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("launch_driver")), + ) + + return [container, loader] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + # a default_value of None is equivalent to not passing that kwarg at all + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + add_launch_arg("model", description="velodyne model name") + add_launch_arg("launch_driver", "True", "do launch driver") + add_launch_arg("calibration", description="path to calibration file") + add_launch_arg("device_ip", "192.168.1.201", "device ip address") + add_launch_arg("scan_phase", "0.0") + add_launch_arg("base_frame", "base_link", "base frame id") + add_launch_arg("container_name", "velodyne_composable_node_container", "container name") + add_launch_arg("min_range", description="minimum view range") + add_launch_arg("max_range", description="maximum view range") + add_launch_arg("pcap", "") + add_launch_arg("port", "2368", description="device port number") + add_launch_arg("read_fast", "False") + add_launch_arg("read_once", "False") + add_launch_arg("repeat_delay", "0.0") + add_launch_arg("rpm", "600.0", "rotational frequency") + add_launch_arg("laserscan_ring", "-1") + add_launch_arg("laserscan_resolution", "0.007") + add_launch_arg("num_points_thresholds", "300") + add_launch_arg("invalid_intensity") + add_launch_arg("frame_id", "velodyne", "velodyne frame id") + add_launch_arg("gps_time", "False") + add_launch_arg("view_direction", description="the center of lidar angle") + add_launch_arg("view_width", description="lidar angle: 0~6.28 [rad]") + 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( + "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" + ) + add_launch_arg("use_multithread", "False", "use multithread") + add_launch_arg("use_intra_process", "False", "use ROS2 component container communication") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/aip_x1_1_launch/package.xml b/aip_x1_1_launch/package.xml new file mode 100644 index 00000000..8e7f2600 --- /dev/null +++ b/aip_x1_1_launch/package.xml @@ -0,0 +1,36 @@ + + + + aip_x1_1_launch + 0.1.0 + The aip_x1_1_launch package + + Yohei Mishina + Apache License 2.0 + + ament_cmake_auto + + compare_map_segmentation + elevation_map_loader + ground_segmentation + imu_corrector + individual_params + occupancy_grid_map_outlier_filter + pandar_driver + pandar_pointcloud + pointcloud_preprocessor + rclcpp_components + ros2_socketcan + tamagawa_imu_driver + topic_state_monitor + velodyne_driver + velodyne_monitor + velodyne_pointcloud + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + From 4dafb348e7386d0c7dba776ca26218585648997a Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Wed, 17 May 2023 18:10:36 +0900 Subject: [PATCH 02/50] fix(velodyne_node_container): fix param load failure Signed-off-by: 1222-takeshi --- aip_x1_1_launch/launch/velodyne_node_container.launch.py | 2 ++ 1 file changed, 2 insertions(+) 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 edffc716..edcef126 100644 --- a/aip_x1_1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_1_launch/launch/velodyne_node_container.launch.py @@ -26,6 +26,8 @@ def get_vehicle_info(context): gp = context.launch_configurations.get("ros_params", {}) + if not gp: + gp = dict(context.launch_configurations.get("global_params", {})) p = {} p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] From 1a9c6b367fb304fb382142b09855ff98c3e368d3 Mon Sep 17 00:00:00 2001 From: Yohei Mishina <66298900+YoheiMishina@users.noreply.github.com> Date: Thu, 18 May 2023 14:19:14 +0900 Subject: [PATCH 03/50] fix(diagnostic_aggregator): update sensor kit param (#22) (#23) * fix(diagnostic_aggregator): Modified aggregator confinguration to V1' * fix path on diagnostic aggregator --- .../config/diagnostic_aggregator/sensor_kit.param.yaml | 4 ++-- .../config/diagnostic_aggregator/sensor_kit.param.yaml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml index 7c6f52f5..09039b2d 100644 --- a/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml +++ b/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -33,9 +33,9 @@ path: rpm contains: [": velodyne_rpm"] timeout: 3.0 - pandar: + front: type: diagnostic_aggregator/AnalyzerGroup - path: pandar + path: front analyzers: health_monitoring: type: diagnostic_aggregator/AnalyzerGroup diff --git a/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml index dafe8bbc..9ca9ed49 100644 --- a/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml +++ b/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -34,9 +34,9 @@ contains: [": velodyne_rpm"] timeout: 3.0 - livox: + front: type: diagnostic_aggregator/AnalyzerGroup - path: livox + path: front analyzers: health_monitoring: type: diagnostic_aggregator/AnalyzerGroup From 63794986a9dcfb14b73d4a2ae82a2dc2d812d78e Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Mon, 5 Jun 2023 16:46:28 +0900 Subject: [PATCH 04/50] fix: add vehicle_velocity_converter.launch (#146) fix: add vehicle_velocity_converter.launch to aip_x1_1_launch Signed-off-by: 1222-takeshi --- aip_x1_1_launch/launch/pandar_node_container.launch.py | 2 ++ aip_x1_1_launch/launch/sensing.launch.xml | 7 +++++++ 2 files changed, 9 insertions(+) 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 cf635604..e84947fc 100644 --- a/aip_x1_1_launch/launch/pandar_node_container.launch.py +++ b/aip_x1_1_launch/launch/pandar_node_container.launch.py @@ -100,6 +100,8 @@ def create_parameter_dict(*args): plugin="pointcloud_preprocessor::DistortionCorrectorComponent", name="distortion_corrector_node", remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("~/input/imu", "/sensing/imu/imu_data"), ("~/input/velocity_report", "/vehicle/status/velocity_status"), ("~/input/pointcloud", "pointcloud_raw_ex"), ("~/output/pointcloud", "rectified/pointcloud_ex"), diff --git a/aip_x1_1_launch/launch/sensing.launch.xml b/aip_x1_1_launch/launch/sensing.launch.xml index 3dafb3d1..9b064ab6 100644 --- a/aip_x1_1_launch/launch/sensing.launch.xml +++ b/aip_x1_1_launch/launch/sensing.launch.xml @@ -20,6 +20,13 @@ + + + + + + + From 6141067877da8c81a0ce9a782b99f07e8b5907bc Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Wed, 13 Dec 2023 15:38:33 +0900 Subject: [PATCH 05/50] feat: add use_awsim option and add gnss imu Signed-off-by: 1222-takeshi --- aip_x1_description/urdf/sensor_kit.xacro | 27 ++++++++++++++++++++++++ aip_x1_launch/launch/lidar.launch.xml | 7 +++--- aip_x1_launch/launch/sensing.launch.xml | 5 +++++ 3 files changed, 36 insertions(+), 3 deletions(-) diff --git a/aip_x1_description/urdf/sensor_kit.xacro b/aip_x1_description/urdf/sensor_kit.xacro index 5ab2f15c..649ba65f 100644 --- a/aip_x1_description/urdf/sensor_kit.xacro +++ b/aip_x1_description/urdf/sensor_kit.xacro @@ -2,6 +2,7 @@ + @@ -67,5 +68,31 @@ pitch="${calibration['sensor_kit_base_link']['livox_front_right_base_link']['pitch']}" yaw="${calibration['sensor_kit_base_link']['livox_front_right_base_link']['yaw']}" /> + + + + diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml index 56066df5..ee0d93d0 100644 --- a/aip_x1_launch/launch/lidar.launch.xml +++ b/aip_x1_launch/launch/lidar.launch.xml @@ -5,6 +5,7 @@ + @@ -25,7 +26,7 @@ - + @@ -52,13 +53,13 @@ - + - + diff --git a/aip_x1_launch/launch/sensing.launch.xml b/aip_x1_launch/launch/sensing.launch.xml index 1c294fdf..19b4f5df 100644 --- a/aip_x1_launch/launch/sensing.launch.xml +++ b/aip_x1_launch/launch/sensing.launch.xml @@ -4,6 +4,9 @@ + + + @@ -12,6 +15,7 @@ + @@ -22,6 +26,7 @@ + From add6e310b5fbc700aae15c80885ace89d03bfbee Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Wed, 13 Dec 2023 20:43:57 +0900 Subject: [PATCH 06/50] feat: add GNSS Signed-off-by: 1222-takeshi --- aip_x1_1_description/urdf/sensor_kit.xacro | 17 +++++++++++++++-- aip_x1_1_launch/launch/sensing.launch.xml | 14 +++++++++++--- aip_x1_launch/launch/sensing.launch.xml | 2 +- .../launch/velodyne_node_container.launch.py | 2 -- 4 files changed, 27 insertions(+), 8 deletions(-) diff --git a/aip_x1_1_description/urdf/sensor_kit.xacro b/aip_x1_1_description/urdf/sensor_kit.xacro index 66508144..7629d304 100644 --- a/aip_x1_1_description/urdf/sensor_kit.xacro +++ b/aip_x1_1_description/urdf/sensor_kit.xacro @@ -2,6 +2,7 @@ + @@ -48,8 +49,20 @@ pitch="${calibration['sensor_kit_base_link']['pandar_xt32_front_center_base_link']['pitch']}" yaw="${calibration['sensor_kit_base_link']['pandar_xt32_front_center_base_link']['yaw']}" /> - - + + + + + + + - + @@ -20,13 +24,17 @@ + + + + + - + - diff --git a/aip_x1_launch/launch/sensing.launch.xml b/aip_x1_launch/launch/sensing.launch.xml index 19b4f5df..b8b04a0d 100644 --- a/aip_x1_launch/launch/sensing.launch.xml +++ b/aip_x1_launch/launch/sensing.launch.xml @@ -30,7 +30,7 @@ - + diff --git a/aip_x1_launch/launch/velodyne_node_container.launch.py b/aip_x1_launch/launch/velodyne_node_container.launch.py index 96e72a17..8dcf5787 100644 --- a/aip_x1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_launch/launch/velodyne_node_container.launch.py @@ -25,8 +25,6 @@ def get_vehicle_info(context): - # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support - # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py gp = context.launch_configurations.get("ros_params", {}) if not gp: gp = dict(context.launch_configurations.get("global_params", {})) From ce1b446808b63bc8a45b1b6162a791656982d606 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Wed, 13 Dec 2023 20:45:14 +0900 Subject: [PATCH 07/50] feat: update for upstream Signed-off-by: 1222-takeshi --- aip_x1_1_launch/launch/imu.launch.xml | 21 ++++++++++----- .../launch/pandar_node_container.launch.py | 1 - .../launch/pointcloud_preprocessor.launch.py | 6 ++++- .../launch/velodyne_node_container.launch.py | 27 ++++++++++++++----- 4 files changed, 41 insertions(+), 14 deletions(-) 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")}], ) From 71a808a18d8be791d50a4a41203f10d3253432c0 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Wed, 13 Dec 2023 20:45:29 +0900 Subject: [PATCH 08/50] feat: add use_awsim option Signed-off-by: 1222-takeshi --- aip_x1_1_launch/launch/lidar.launch.xml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/aip_x1_1_launch/launch/lidar.launch.xml b/aip_x1_1_launch/launch/lidar.launch.xml index 115de1f9..0101dd89 100644 --- a/aip_x1_1_launch/launch/lidar.launch.xml +++ b/aip_x1_1_launch/launch/lidar.launch.xml @@ -7,6 +7,7 @@ + @@ -23,7 +24,7 @@ - + @@ -40,7 +41,7 @@ - + @@ -48,6 +49,6 @@ - + From e1c74f33a08cf5b9ea95a41c12085e2cace75930 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Wed, 13 Dec 2023 22:00:50 +0900 Subject: [PATCH 09/50] feat: change topic name Signed-off-by: 1222-takeshi --- aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py | 2 +- aip_x1_1_launch/launch/sensing.launch.xml | 2 -- aip_x1_1_launch/launch/topic_state_monitor.launch.py | 2 +- aip_x1_1_launch/launch/velodyne_node_container.launch.py | 2 +- 4 files changed, 3 insertions(+), 5 deletions(-) diff --git a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py index 183f3681..b91fe124 100644 --- a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py @@ -37,7 +37,7 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "input_topics": [ - "/sensing/lidar/top/outlier_filtered/pointcloud", + "/sensing/lidar/top/pointcloud", "/sensing/lidar/front_center/pointcloud_raw", ], "output_frame": LaunchConfiguration("base_frame"), diff --git a/aip_x1_1_launch/launch/sensing.launch.xml b/aip_x1_1_launch/launch/sensing.launch.xml index e5ac18b5..5699d526 100644 --- a/aip_x1_1_launch/launch/sensing.launch.xml +++ b/aip_x1_1_launch/launch/sensing.launch.xml @@ -6,8 +6,6 @@ - - diff --git a/aip_x1_1_launch/launch/topic_state_monitor.launch.py b/aip_x1_1_launch/launch/topic_state_monitor.launch.py index 29bc9f69..03cdc3e0 100644 --- a/aip_x1_1_launch/launch/topic_state_monitor.launch.py +++ b/aip_x1_1_launch/launch/topic_state_monitor.launch.py @@ -45,7 +45,7 @@ def generate_launch_description(): name="topic_state_monitor_top_outlier_filtered", parameters=[ { - "topic": "/sensing/lidar/top/outlier_filtered/pointcloud", + "topic": "/sensing/lidar/top/pointcloud", "topic_type": "sensor_msgs/msg/PointCloud2", "best_effort": True, "diag_name": "sensing_topic_status", 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 3f644f33..aefe95b7 100644 --- a/aip_x1_1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_1_launch/launch/velodyne_node_container.launch.py @@ -140,7 +140,7 @@ def create_parameter_dict(*args): name="ring_outlier_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "outlier_filtered/pointcloud"), + ("output", "pointcloud"), ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) From 29d1ccff3d428366b4b69c863daf54e2edff6291 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Tue, 30 Jan 2024 11:30:41 +0900 Subject: [PATCH 10/50] feat: add node alive monitoring Signed-off-by: 1222-takeshi --- .../config/diagnostic_aggregator/sensor_kit.param.yaml | 9 +++++++++ .../config/diagnostic_aggregator/sensor_kit.param.yaml | 9 +++++++++ 2 files changed, 18 insertions(+) diff --git a/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml index 09039b2d..2d3f4014 100644 --- a/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml +++ b/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -4,6 +4,15 @@ type: diagnostic_aggregator/AnalyzerGroup path: sensing analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": sensing_topic_status"] + timeout: 1.0 lidar: type: diagnostic_aggregator/AnalyzerGroup path: lidar diff --git a/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml index 9ca9ed49..0f3ded87 100644 --- a/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml +++ b/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -4,6 +4,15 @@ type: diagnostic_aggregator/AnalyzerGroup path: sensing analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": sensing_topic_status"] + timeout: 1.0 lidar: type: diagnostic_aggregator/AnalyzerGroup path: lidar From e104c727d39a1d2623d0928217415e8450cd5bfb Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Tue, 30 Jan 2024 13:48:47 +0900 Subject: [PATCH 11/50] feat: add gnss Signed-off-by: 1222-takeshi --- aip_x1_1_description/config/sensor_kit_calibration.yaml | 7 +++++++ aip_x1_description/config/sensor_kit_calibration.yaml | 7 +++++++ 2 files changed, 14 insertions(+) diff --git a/aip_x1_1_description/config/sensor_kit_calibration.yaml b/aip_x1_1_description/config/sensor_kit_calibration.yaml index e8955379..70133656 100644 --- a/aip_x1_1_description/config/sensor_kit_calibration.yaml +++ b/aip_x1_1_description/config/sensor_kit_calibration.yaml @@ -20,3 +20,10 @@ sensor_kit_base_link: roll: 3.14159 pitch: 0.0 yaw: 0.0 + gnss_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 diff --git a/aip_x1_description/config/sensor_kit_calibration.yaml b/aip_x1_description/config/sensor_kit_calibration.yaml index 252bd1e9..93dbfbfe 100644 --- a/aip_x1_description/config/sensor_kit_calibration.yaml +++ b/aip_x1_description/config/sensor_kit_calibration.yaml @@ -27,3 +27,10 @@ sensor_kit_base_link: roll: 0.000 pitch: 0.000 yaw: -1.047 + gnss_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 From fa62cf7754847bd2e0f94c0b8692d0e5f398c651 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Tue, 30 Jan 2024 13:51:30 +0900 Subject: [PATCH 12/50] feat: update ground_segmentation param Signed-off-by: 1222-takeshi --- .../config/ground_segmentation/ground_segmentation.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index 3bd7b101..5030bb95 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -2,7 +2,7 @@ ros__parameters: additional_lidars: [] ransac_input_topics: [ - "/sensing/lidar/front_center/outlier_filtered/pointcloud" + "/sensing/lidar/front_center/pointcloud" ] use_single_frame_filter: True use_time_series_filter: False From 0a9981aed7d77bdbf502f3ff818b1e07eb2b6ace Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Tue, 30 Jan 2024 13:55:13 +0900 Subject: [PATCH 13/50] feat: remove use_awsim option Signed-off-by: 1222-takeshi --- aip_x1_1_launch/launch/lidar.launch.xml | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/aip_x1_1_launch/launch/lidar.launch.xml b/aip_x1_1_launch/launch/lidar.launch.xml index 0101dd89..5c1d680a 100644 --- a/aip_x1_1_launch/launch/lidar.launch.xml +++ b/aip_x1_1_launch/launch/lidar.launch.xml @@ -7,7 +7,6 @@ - @@ -21,10 +20,12 @@ + + - + @@ -41,7 +42,7 @@ - + @@ -49,6 +50,6 @@ - + From 1237a679f800462af5a36816dbf3dc7ab2651f47 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Tue, 30 Jan 2024 13:56:11 +0900 Subject: [PATCH 14/50] feat: add cropbox filter Signed-off-by: 1222-takeshi --- .../launch/pandar_node_container.launch.py | 48 ++++++++++++++++++- 1 file changed, 46 insertions(+), 2 deletions(-) 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 ffc2a70c..905607f5 100644 --- a/aip_x1_1_launch/launch/pandar_node_container.launch.py +++ b/aip_x1_1_launch/launch/pandar_node_container.launch.py @@ -39,6 +39,23 @@ def get_pandar_monitor_info(): p = yaml.safe_load(f)["/**"]["ros__parameters"] return p +def get_vehicle_info(context): + # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support + # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py + gp = context.launch_configurations.get("ros_params", {}) + if not gp: + gp = dict(context.launch_configurations.get("global_params", {})) + p = {} + p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] + p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] + p["min_longitudinal_offset"] = -gp["rear_overhang"] + p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] + p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) + p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] + p["min_height_offset"] = -0.3 # margin to crop pointcloud under vehicle + p["max_height_offset"] = gp["vehicle_height"] + return p + def launch_setup(context, *args, **kwargs): def create_parameter_dict(*args): result = {} @@ -94,6 +111,29 @@ def create_parameter_dict(*args): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") + cropbox_parameters["negative"] = True + + vehicle_info = get_vehicle_info(context) + cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] + cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] + cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] + cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] + cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] + cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] + + self_crop_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_self", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "self_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + undistort_component = ComposableNode( package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::DistortionCorrectorComponent", @@ -102,7 +142,7 @@ def create_parameter_dict(*args): ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), ("~/input/imu", "/sensing/imu/imu_data"), ("~/input/velocity_report", "/vehicle/status/velocity_status"), - ("~/input/pointcloud", "pointcloud_raw_ex"), + ("~/input/pointcloud", "self_cropped/pointcloud_ex"), ("~/output/pointcloud", "rectified/pointcloud_ex"), ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -114,7 +154,7 @@ def create_parameter_dict(*args): name="ring_outlier_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "outlier_filtered/pointcloud"), + ("output", "pointcloud"), ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -126,6 +166,7 @@ def create_parameter_dict(*args): executable=LaunchConfiguration("container_executable"), composable_node_descriptions=[ pointcloud_component, + self_crop_component, undistort_component, ring_outlier_filter_component, ], @@ -158,11 +199,14 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("angle_range", "[0.0, 360.0]") add_launch_arg("distance_range", "[0.05, 200.0]") add_launch_arg("return_mode", "Dual") + add_launch_arg("base_frame", "base_link") add_launch_arg("container_name", "pandar_composable_node_container") add_launch_arg("pcap", "") add_launch_arg("lidar_port", "2321") add_launch_arg("gps_port", "10121") add_launch_arg("frame_id", "pandar") + add_launch_arg("input_frame", LaunchConfiguration("base_frame")) + add_launch_arg("output_frame", LaunchConfiguration("base_frame")) add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False") From 726bb0c98c7762b6558883089186477d9b47ee5b Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Wed, 31 Jan 2024 12:09:50 +0900 Subject: [PATCH 15/50] feat: add diag of gyro_bias_validator Signed-off-by: 1222-takeshi --- .../diagnostic_aggregator/sensor_kit.param.yaml | 13 +++++++++++++ .../diagnostic_aggregator/sensor_kit.param.yaml | 1 - 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml index 2d3f4014..e32c0fb8 100644 --- a/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml +++ b/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -65,3 +65,16 @@ path: ptp contains: [": pandar_ptp"] timeout: 1.0 + imu: + type: diagnostic_aggregator/AnalyzerGroup + path: imu + analyzers: + bias_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: bias_monitoring + analyzers: + gyro_bias_validator: + type: diagnostic_aggregator/GenericAnalyzer + path: gyro_bias_validator + contains: [": gyro_bias_validator"] + timeout: 1.0 diff --git a/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml index 0f3ded87..24b3a6e6 100644 --- a/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml +++ b/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -110,7 +110,6 @@ # path: time_sync # contains: [": livox_time_sync"] # timeout: 3.0 - imu: type: diagnostic_aggregator/AnalyzerGroup path: imu From 48c48fe267ae6462063f4da4c17e162ed59ccea1 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Wed, 31 Jan 2024 12:10:14 +0900 Subject: [PATCH 16/50] chore: update for upstream Signed-off-by: 1222-takeshi --- aip_x1_1_launch/launch/imu.launch.xml | 4 ++-- aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/aip_x1_1_launch/launch/imu.launch.xml b/aip_x1_1_launch/launch/imu.launch.xml index 30207f8d..2008eda8 100644 --- a/aip_x1_1_launch/launch/imu.launch.xml +++ b/aip_x1_1_launch/launch/imu.launch.xml @@ -7,13 +7,13 @@ - + - + diff --git a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py index b91fe124..ed382e62 100644 --- a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py @@ -38,7 +38,7 @@ def launch_setup(context, *args, **kwargs): { "input_topics": [ "/sensing/lidar/top/pointcloud", - "/sensing/lidar/front_center/pointcloud_raw", + "/sensing/lidar/front_center/pointcloud", ], "output_frame": LaunchConfiguration("base_frame"), "timeout_sec": 1.0, @@ -69,6 +69,7 @@ def launch_setup(context, *args, **kwargs): concat_loader = LoadComposableNodes( composable_node_descriptions=[concat_component], target_container=target_container, + condition=IfCondition(LaunchConfiguration("use_concat_filter")), ) return [container, concat_loader] From f3eb8ebf712f2be8f5637a8905497a41cbdf284c Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Wed, 31 Jan 2024 12:11:18 +0900 Subject: [PATCH 17/50] chore: comment out for avoid no diag stale Signed-off-by: 1222-takeshi --- .../launch/topic_state_monitor.launch.py | 42 +++++++++---------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/aip_x1_1_launch/launch/topic_state_monitor.launch.py b/aip_x1_1_launch/launch/topic_state_monitor.launch.py index 03cdc3e0..3bb7d2b7 100644 --- a/aip_x1_1_launch/launch/topic_state_monitor.launch.py +++ b/aip_x1_1_launch/launch/topic_state_monitor.launch.py @@ -64,7 +64,7 @@ def generate_launch_description(): name="topic_state_monitor_pandar_front_center_outlier_filtered", parameters=[ { - "topic": "/sensing/lidar/front_center/outlier_filtered/pointcloud", + "topic": "/sensing/lidar/front_center/pointcloud", "topic_type": "sensor_msgs/msg/PointCloud2", "best_effort": True, "diag_name": "sensing_topic_status", @@ -84,25 +84,7 @@ def generate_launch_description(): name="topic_state_monitor_rough_no_ground", parameters=[ { - "topic": "/perception/obstacle_segmentation/single_frame/pointcloud_raw", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - topic_state_monitor_short_height_no_ground = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_short_height_no_ground", - parameters=[ - { - "topic": "/perception/obstacle_segmentation/plane_fitting/pointcloud", + "topic": "/perception/obstacle_segmentation/single_frame/pointcloud", "topic_type": "sensor_msgs/msg/PointCloud2", "best_effort": True, "diag_name": "sensing_topic_status", @@ -114,6 +96,24 @@ def generate_launch_description(): ], extra_arguments=[{"use_intra_process_comms": True}], ) + # topic_state_monitor_short_height_no_ground = ComposableNode( + # package="topic_state_monitor", + # plugin="topic_state_monitor::TopicStateMonitorNode", + # name="topic_state_monitor_short_height_no_ground", + # parameters=[ + # { + # "topic": "/perception/obstacle_segmentation/plane_fitting/pointcloud", + # "topic_type": "sensor_msgs/msg/PointCloud2", + # "best_effort": True, + # "diag_name": "sensing_topic_status", + # "warn_rate": 5.0, + # "error_rate": 1.0, + # "timeout": 1.0, + # "window_size": 10, + # }, + # ], + # extra_arguments=[{"use_intra_process_comms": True}], + # ) # topic monitor for tamagawa IMU topic_state_monitor_imu_data = ComposableNode( @@ -146,7 +146,7 @@ def generate_launch_description(): topic_state_monitor_top_outlier_filtered, topic_state_monitor_pandar_front_center_outlier_filtered, topic_state_monitor_rough_no_ground, - topic_state_monitor_short_height_no_ground, + # topic_state_monitor_short_height_no_ground, topic_state_monitor_imu_data, ], output="screen", From 9572b3aab9cb3f98fcebb81d52f471843380489f Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Fri, 9 Feb 2024 14:06:36 +0900 Subject: [PATCH 18/50] feat: add gnss.launch for x1_1 Signed-off-by: 1222-takeshi --- aip_x1_1_launch/launch/gnss.launch.xml | 28 +++++++++++++++++++++++ aip_x1_1_launch/launch/sensing.launch.xml | 2 +- 2 files changed, 29 insertions(+), 1 deletion(-) create mode 100644 aip_x1_1_launch/launch/gnss.launch.xml diff --git a/aip_x1_1_launch/launch/gnss.launch.xml b/aip_x1_1_launch/launch/gnss.launch.xml new file mode 100644 index 00000000..26b0de4c --- /dev/null +++ b/aip_x1_1_launch/launch/gnss.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x1_1_launch/launch/sensing.launch.xml b/aip_x1_1_launch/launch/sensing.launch.xml index 5699d526..843e2d79 100644 --- a/aip_x1_1_launch/launch/sensing.launch.xml +++ b/aip_x1_1_launch/launch/sensing.launch.xml @@ -23,7 +23,7 @@ - + From 8a9661e40f3306cd9cb2621a394252a06ceb3127 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Fri, 9 Feb 2024 14:07:39 +0900 Subject: [PATCH 19/50] feat: use pointcloud_container Signed-off-by: 1222-takeshi --- aip_x1_1_launch/launch/lidar.launch.xml | 5 +--- .../launch/pandar_node_container.launch.py | 2 +- .../launch/pointcloud_preprocessor.launch.py | 23 ++----------------- aip_x1_1_launch/launch/sensing.launch.xml | 2 -- .../launch/velodyne_node_container.launch.py | 2 +- 5 files changed, 5 insertions(+), 29 deletions(-) diff --git a/aip_x1_1_launch/launch/lidar.launch.xml b/aip_x1_1_launch/launch/lidar.launch.xml index 5c1d680a..b2457f76 100644 --- a/aip_x1_1_launch/launch/lidar.launch.xml +++ b/aip_x1_1_launch/launch/lidar.launch.xml @@ -5,7 +5,6 @@ - @@ -20,8 +19,7 @@ - - + @@ -46,7 +44,6 @@ - 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 905607f5..4f51035b 100644 --- a/aip_x1_1_launch/launch/pandar_node_container.launch.py +++ b/aip_x1_1_launch/launch/pandar_node_container.launch.py @@ -160,7 +160,7 @@ def create_parameter_dict(*args): ) container = ComposableNodeContainer( - name="pandar_node_container", + name=LaunchConfiguration("pointcloud_container_name"), namespace="pointcloud_preprocessor", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), diff --git a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py index ed382e62..eab446b9 100644 --- a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py @@ -19,7 +19,6 @@ from launch.conditions import IfCondition from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode @@ -48,31 +47,14 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - # set container to run all required components in the same process - container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[], - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) - - target_container = ( - container - if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) - else LaunchConfiguration("container_name") - ) - # load concat or passthrough filter concat_loader = LoadComposableNodes( composable_node_descriptions=[concat_component], - target_container=target_container, + target_container=LaunchConfiguration("pointcloud_container_name"), condition=IfCondition(LaunchConfiguration("use_concat_filter")), ) - return [container, concat_loader] + return [concat_loader] def generate_launch_description(): @@ -84,7 +66,6 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("base_frame", "base_link") add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False") - add_launch_arg("use_pointcloud_container", "False") add_launch_arg("container_name", "pointcloud_preprocessor_container") set_container_executable = SetLaunchConfiguration( diff --git a/aip_x1_1_launch/launch/sensing.launch.xml b/aip_x1_1_launch/launch/sensing.launch.xml index 843e2d79..1419d429 100644 --- a/aip_x1_1_launch/launch/sensing.launch.xml +++ b/aip_x1_1_launch/launch/sensing.launch.xml @@ -2,7 +2,6 @@ - @@ -12,7 +11,6 @@ - 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 aefe95b7..a660f023 100644 --- a/aip_x1_1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_1_launch/launch/velodyne_node_container.launch.py @@ -149,7 +149,7 @@ def create_parameter_dict(*args): # set container to run all required components in the same process container = ComposableNodeContainer( # need unique name, otherwise all processes in same container and the node names then clash - name="velodyne_node_container", + name=LaunchConfiguration("container_name"), namespace="pointcloud_preprocessor", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), From f4bbe2ff3c21dd9fdd9475f77dbc74563c598440 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Fri, 9 Feb 2024 14:08:07 +0900 Subject: [PATCH 20/50] fix: change topic name for ground segmentation Signed-off-by: 1222-takeshi --- .../launch/ground_segmentation/ground_segmentation.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py b/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py index a3687bec..9c76c90f 100644 --- a/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py +++ b/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py @@ -75,7 +75,7 @@ def create_additional_pipeline(self, lidar_name): plugin="pointcloud_preprocessor::CropBoxFilterComponent", name=f"{lidar_name}_crop_box_filter", remappings=[ - ("input", f"/sensing/lidar/{lidar_name}/outlier_filtered/pointcloud"), + ("input", f"/sensing/lidar/{lidar_name}/pointcloud"), ("output", f"{lidar_name}/range_cropped/pointcloud"), ], parameters=[ From a618ce207c57748deb3084c55d0c248424c0d096 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Fri, 16 Feb 2024 13:40:06 +0900 Subject: [PATCH 21/50] feat(aip_x1_launch): rename some topics Signed-off-by: 1222-takeshi --- aip_x1_launch/launch/new_livox_horizon.launch.py | 2 +- aip_x1_launch/launch/pointcloud_preprocessor.launch.py | 8 ++++---- aip_x1_launch/launch/topic_state_monitor.launch.py | 6 +++--- aip_x1_launch/launch/velodyne_node_container.launch.py | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/aip_x1_launch/launch/new_livox_horizon.launch.py b/aip_x1_launch/launch/new_livox_horizon.launch.py index 4f262a16..26826a72 100644 --- a/aip_x1_launch/launch/new_livox_horizon.launch.py +++ b/aip_x1_launch/launch/new_livox_horizon.launch.py @@ -52,7 +52,7 @@ def get_crop_box_min_range_component(context, livox_frame_id): name="crop_box_filter_min_range", remappings=[ ("input", "livox/tag_filtered/lidar" if use_tag_filter else "livox/lidar"), - ("output", "min_range_cropped/pointcloud_before_sync"), + ("output", "pointcloud"), ], parameters=[ { diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py index 639705b6..dc79d16c 100644 --- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py @@ -36,10 +36,10 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "input_topics": [ - "/sensing/lidar/top/pointcloud_before_sync", - "/sensing/lidar/front_left/min_range_cropped/pointcloud_before_sync", - "/sensing/lidar/front_right/min_range_cropped/pointcloud_before_sync", - "/sensing/lidar/front_center/min_range_cropped/pointcloud_before_sync", + "/sensing/lidar/top/pointcloud", + "/sensing/lidar/front_left/pointcloud", + "/sensing/lidar/front_right/pointcloud", + "/sensing/lidar/front_center/pointcloud", ], "output_frame": LaunchConfiguration("base_frame"), "timeout_sec": 1.0, diff --git a/aip_x1_launch/launch/topic_state_monitor.launch.py b/aip_x1_launch/launch/topic_state_monitor.launch.py index 1c17606c..3abbfc22 100644 --- a/aip_x1_launch/launch/topic_state_monitor.launch.py +++ b/aip_x1_launch/launch/topic_state_monitor.launch.py @@ -99,7 +99,7 @@ def generate_launch_description(): name="topic_state_monitor_livox_front_left_min_range_cropped", parameters=[ { - "topic": "/sensing/lidar/front_left/min_range_cropped/pointcloud", + "topic": "/sensing/lidar/front_left/pointcloud", "topic_type": "sensor_msgs/msg/PointCloud2", "best_effort": True, "diag_name": "sensing_topic_status", @@ -117,7 +117,7 @@ def generate_launch_description(): name="topic_state_monitor_livox_front_right_min_range_cropped", parameters=[ { - "topic": "/sensing/lidar/front_right/min_range_cropped/pointcloud", + "topic": "/sensing/lidar/front_right/pointcloud", "topic_type": "sensor_msgs/msg/PointCloud2", "best_effort": True, "diag_name": "sensing_topic_status", @@ -135,7 +135,7 @@ def generate_launch_description(): name="topic_state_monitor_livox_front_center_min_range_cropped", parameters=[ { - "topic": "/sensing/lidar/front_center/min_range_cropped/pointcloud", + "topic": "/sensing/lidar/front_center/pointcloud", "topic_type": "sensor_msgs/msg/PointCloud2", "best_effort": True, "diag_name": "sensing_topic_status", diff --git a/aip_x1_launch/launch/velodyne_node_container.launch.py b/aip_x1_launch/launch/velodyne_node_container.launch.py index 8dcf5787..d0fe9118 100644 --- a/aip_x1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_launch/launch/velodyne_node_container.launch.py @@ -143,7 +143,7 @@ def create_parameter_dict(*args): # set container to run all required components in the same process container = ComposableNodeContainer( # need unique name, otherwise all processes in same container and the node names then clash - name="velodyne_node_container", + name="pointcloud_container", namespace="pointcloud_preprocessor", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), From 6614eb664cbc9951dbfe443d8e0fb7e8e3894b5b Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Tue, 20 Feb 2024 17:12:48 +0900 Subject: [PATCH 22/50] feat(aip_x1, aip_x1_1): use interpolate Signed-off-by: 1222-takeshi --- .../ground_segmentation.param.yaml | 28 +++++++++---- .../ground_segmentation.launch.py | 6 ++- .../launch/velodyne_node_container.launch.py | 42 +++++++++---------- .../launch/velodyne_node_container.launch.py | 22 ++++++++-- 4 files changed, 62 insertions(+), 36 deletions(-) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index 5030bb95..0a4a0e23 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -17,20 +17,30 @@ negative: False common_ground_filter: - plugin: "ground_segmentation::RayGroundFilterComponent" + plugin: "ground_segmentation::ScanGroundFilterComponent" parameters: - initial_max_slope: 10.0 - general_max_slope: 10.0 - local_max_slope: 10.0 - min_height_threshold: 0.3 - radial_divider_angle: 1.0 - concentric_divider_distance: 0.0 - use_vehicle_footprint: True + global_slope_max_angle_deg: 10.0 + local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode + split_points_distance_tolerance: 0.2 + use_virtual_ground_point: True + split_height_distance: 0.2 + radial_divider_angle_deg: 1.0 + + # common_ground_filter: + # plugin: "ground_segmentation::RayGroundFilterComponent" + # parameters: + # initial_max_slope: 10.0 + # general_max_slope: 10.0 + # local_max_slope: 10.0 + # min_height_threshold: 0.3 + # radial_divider_angle: 1.0 + # concentric_divider_distance: 0.0 + # use_vehicle_footprint: True short_height_obstacle_detection_area_filter: parameters: min_x: 0.0 - max_x: 19.6 # max_x: 18.0m + base_link2livox_front_center distance 1.6m + max_x: 21.6 # max_x: 20.0m + base_link2xt32_front_center distance 1.6m min_y: -4.0 max_y: 4.0 min_z: -0.5 diff --git a/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py b/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py index 9c76c90f..920a221a 100644 --- a/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py +++ b/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py @@ -50,6 +50,8 @@ def __init__(self, context): def get_vehicle_info(self): # TODO: need to rename key from "ros_params" to "global_params" after Humble gp = self.context.launch_configurations.get("ros_params", {}) + if not gp: + gp = dict(self.context.launch_configurations.get("global_params", {})) p = {} p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] @@ -82,8 +84,8 @@ def create_additional_pipeline(self, lidar_name): { "input_frame": LaunchConfiguration("base_frame"), "output_frame": LaunchConfiguration("base_frame"), - "min_z": self.vehicle_info["min_height_offset"], - "max_z": self.vehicle_info["max_height_offset"], + # "min_z": self.vehicle_info["min_height_offset"], + # "max_z": self.vehicle_info["max_height_offset"], }, self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"], ], 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 a660f023..38fdae48 100644 --- a/aip_x1_1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_1_launch/launch/velodyne_node_container.launch.py @@ -104,35 +104,35 @@ 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="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::DistortionCorrectorComponent", - name="distortion_corrector_node", + package="velodyne_pointcloud", + plugin="velodyne_pointcloud::Interpolate", + name="velodyne_interpolate_node", remappings=[ - ("~/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"), + ("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="pointcloud_preprocessor", + # plugin="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", "self_cropped/pointcloud_ex"), + # ("~/output/pointcloud", "rectified/pointcloud_ex"), + # ], + # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + # ) + # ) + nodes.append( ComposableNode( package="pointcloud_preprocessor", diff --git a/aip_x1_launch/launch/velodyne_node_container.launch.py b/aip_x1_launch/launch/velodyne_node_container.launch.py index d0fe9118..c171567d 100644 --- a/aip_x1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_launch/launch/velodyne_node_container.launch.py @@ -104,16 +104,30 @@ def create_parameter_dict(*args): ) ) + # nodes.append( + # ComposableNode( + # package="pointcloud_preprocessor", + # plugin="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", "self_cropped/pointcloud_ex"), + # ("~/output/pointcloud", "rectified/pointcloud_ex"), + # ], + # 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", "self_cropped/pointcloud_ex"), - ("~/output/pointcloud", "rectified/pointcloud_ex"), + ("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")}], ) From 9270eed1730817b7599ca232b5986cbb94255ac7 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Mon, 26 Feb 2024 13:26:00 +0900 Subject: [PATCH 23/50] feat(velodyne_node_container): use distortion_corrector Signed-off-by: 1222-takeshi --- .../launch/velodyne_node_container.launch.py | 42 +++++++++---------- .../launch/velodyne_node_container.launch.py | 22 ++-------- 2 files changed, 25 insertions(+), 39 deletions(-) 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 38fdae48..a660f023 100644 --- a/aip_x1_1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_1_launch/launch/velodyne_node_container.launch.py @@ -104,35 +104,35 @@ 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="pointcloud_preprocessor", - # plugin="pointcloud_preprocessor::DistortionCorrectorComponent", - # name="distortion_corrector_node", + # package="velodyne_pointcloud", + # plugin="velodyne_pointcloud::Interpolate", + # name="velodyne_interpolate_node", # remappings=[ - # ("~/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"), + # ("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="pointcloud_preprocessor", + plugin="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", "self_cropped/pointcloud_ex"), + ("~/output/pointcloud", "rectified/pointcloud_ex"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + nodes.append( ComposableNode( package="pointcloud_preprocessor", diff --git a/aip_x1_launch/launch/velodyne_node_container.launch.py b/aip_x1_launch/launch/velodyne_node_container.launch.py index c171567d..d0fe9118 100644 --- a/aip_x1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_launch/launch/velodyne_node_container.launch.py @@ -104,30 +104,16 @@ def create_parameter_dict(*args): ) ) - # nodes.append( - # ComposableNode( - # package="pointcloud_preprocessor", - # plugin="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", "self_cropped/pointcloud_ex"), - # ("~/output/pointcloud", "rectified/pointcloud_ex"), - # ], - # 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=[ - ("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")}], ) From cb63d9fd7915a04b224bff3fad74f3bd42190c73 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Mon, 26 Feb 2024 17:31:05 +0900 Subject: [PATCH 24/50] feat(ground_segmentation): change crop parameter Signed-off-by: 1222-takeshi --- .../config/ground_segmentation/ground_segmentation.param.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index 0a4a0e23..9ff8500f 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -13,6 +13,7 @@ max_x: 100.0 min_y: -50.0 max_y: 50.0 + max_z: 2.5 min_z: -0.5 negative: False From a42ed83ef7d087f414e6c723b82607b868435f0f Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Wed, 28 Feb 2024 13:38:53 +0900 Subject: [PATCH 25/50] feat(obstacle_segmentation): relax ground_segmentation threshold Signed-off-by: 1222-takeshi --- .../ground_segmentation.param.yaml | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index 9ff8500f..2795ceea 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -25,7 +25,12 @@ split_points_distance_tolerance: 0.2 use_virtual_ground_point: True split_height_distance: 0.2 - radial_divider_angle_deg: 1.0 + non_ground_height_threshold: 0.20 + grid_size_m: 0.5 + grid_mode_switch_radius: 20.0 + gnd_grid_buffer_size: 4 + detection_range_z_max: 2.5 + elevation_grid_mode: true # common_ground_filter: # plugin: "ground_segmentation::RayGroundFilterComponent" @@ -54,9 +59,9 @@ min_points: 400 min_inliers: 200 max_iterations: 50 - height_threshold: 0.15 + height_threshold: 0.10 plane_slope_threshold: 10.0 - voxel_size_x: 0.2 - voxel_size_y: 0.2 - voxel_size_z: 0.2 + voxel_size_x: 0.1 + voxel_size_y: 0.1 + voxel_size_z: 0.1 debug: False From 5f05c26492f9baa1b06112e4747ba7688165969c Mon Sep 17 00:00:00 2001 From: Takeshi Miura Date: Fri, 5 Apr 2024 13:54:56 +0900 Subject: [PATCH 26/50] chore(ground_segmentation): change parameter name (#229) * chore(ground_segmentation): change parameter name Signed-off-by: 1222-takeshi * ci(pre-commit): autofix --------- Signed-off-by: 1222-takeshi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ground_segmentation.param.yaml | 12 ++++++------ .../launch/pandar_node_container.launch.py | 2 ++ .../launch/pointcloud_preprocessor.launch.py | 2 +- common_sensor_launch/package.xml | 1 + 4 files changed, 10 insertions(+), 7 deletions(-) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index 2795ceea..0f3ea457 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -9,12 +9,12 @@ common_crop_box_filter: parameters: - min_x: -50.0 - max_x: 100.0 - min_y: -50.0 - max_y: 50.0 - max_z: 2.5 - min_z: -0.5 + min_x: -100.0 + max_x: 150.0 + min_y: -70.0 + max_y: 70.0 + margin_max_z: 0.0 # to extend the crop box max_z from vehicle_height + margin_min_z: -2.5 # to extend the crop box min_z from ground negative: False common_ground_filter: 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 4f51035b..fc102338 100644 --- a/aip_x1_1_launch/launch/pandar_node_container.launch.py +++ b/aip_x1_1_launch/launch/pandar_node_container.launch.py @@ -39,6 +39,7 @@ def get_pandar_monitor_info(): p = yaml.safe_load(f)["/**"]["ros__parameters"] return p + def get_vehicle_info(context): # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py @@ -56,6 +57,7 @@ def get_vehicle_info(context): p["max_height_offset"] = gp["vehicle_height"] 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 eab446b9..dd2a2c17 100644 --- a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py @@ -31,7 +31,7 @@ def launch_setup(context, *args, **kwargs): name="concatenate_data", remappings=[ ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), - ("output", "concatenated/pointcloud") + ("output", "concatenated/pointcloud"), ], parameters=[ { diff --git a/common_sensor_launch/package.xml b/common_sensor_launch/package.xml index c0c1170b..016a653f 100644 --- a/common_sensor_launch/package.xml +++ b/common_sensor_launch/package.xml @@ -17,6 +17,7 @@ dummy_diag_publisher nebula_sensor_driver velodyne_monitor + ament_lint_auto autoware_lint_common From e42e17b26f712a35527c7148001a4a2d25467127 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Wed, 17 Apr 2024 12:26:05 +0900 Subject: [PATCH 27/50] chore(ground_segmentation): add missing parameters (#231) * chore(ground_segmentation): change parameter name Signed-off-by: 1222-takeshi * ci(pre-commit): autofix * Add missing params to ground_segmentation.param.yaml Signed-off-by: Autumn60 * ci(pre-commit): autofix * Add missing params to ground_segmentation.param.yaml Signed-off-by: Autumn60 * Fix crop_box params in ground_segmentation.param.yaml Signed-off-by: Autumn60 * Fix crop_box param in ground_segmentation.param.yaml Signed-off-by: Autumn60 * Revert crop_box params Signed-off-by: Autumn60 * Add cropbox param calc to ground_segmentation.launch.py Signed-off-by: Autumn60 * chore(ground_segmentation): change margin_max_z for cropbox Signed-off-by: 1222-takeshi --------- Signed-off-by: 1222-takeshi Signed-off-by: Autumn60 Co-authored-by: 1222-takeshi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Autumn60 --- .../ground_segmentation.param.yaml | 37 +++++++++------ .../ground_segmentation.launch.py | 46 +++++++++++++++++-- 2 files changed, 66 insertions(+), 17 deletions(-) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index 0f3ea457..0cb5133c 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -9,12 +9,12 @@ common_crop_box_filter: parameters: - min_x: -100.0 - max_x: 150.0 - min_y: -70.0 - max_y: 70.0 - margin_max_z: 0.0 # to extend the crop box max_z from vehicle_height - margin_min_z: -2.5 # to extend the crop box min_z from ground + min_x: -50.0 + max_x: 100.0 + min_y: -50.0 + max_y: 50.0 + margin_max_z: 0.63 # to extend the crop box max_z from vehicle_height + margin_min_z: -0.5 # to extend the crop box min_z from ground negative: False common_ground_filter: @@ -31,17 +31,26 @@ gnd_grid_buffer_size: 4 detection_range_z_max: 2.5 elevation_grid_mode: true + use_recheck_ground_cluster: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 # common_ground_filter: # plugin: "ground_segmentation::RayGroundFilterComponent" # parameters: - # initial_max_slope: 10.0 + # min_x: -0.01 + # max_x: +0.01 + # min_y: -0.01 + # max_y: +0.01 + # use_vehicle_footprint: True # general_max_slope: 10.0 # local_max_slope: 10.0 - # min_height_threshold: 0.3 + # initial_max_slope: 10.0 # radial_divider_angle: 1.0 + # min_height_threshold: 0.3 # concentric_divider_distance: 0.0 - # use_vehicle_footprint: True + # reclass_distance_threshold: 0.1 short_height_obstacle_detection_area_filter: parameters: @@ -55,13 +64,15 @@ ransac_ground_filter: parameters: - outlier_threshold: 0.1 - min_points: 400 - min_inliers: 200 +# base_frame: base_link + unit_axis: "z" max_iterations: 50 - height_threshold: 0.10 + min_trial: 200 + min_points: 400 + outlier_threshold: 0.1 plane_slope_threshold: 10.0 voxel_size_x: 0.1 voxel_size_y: 0.1 voxel_size_z: 0.1 + height_threshold: 0.10 debug: False diff --git a/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py b/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py index 920a221a..8cc51174 100644 --- a/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py +++ b/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py @@ -70,6 +70,18 @@ def get_vehicle_mirror_info(self): return p def create_additional_pipeline(self, lidar_name): + max_z = ( + self.vehicle_info["max_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_max_z" + ] + ) + min_z = ( + self.vehicle_info["min_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_min_z" + ] + ) components = [] components.append( ComposableNode( @@ -84,8 +96,8 @@ def create_additional_pipeline(self, lidar_name): { "input_frame": LaunchConfiguration("base_frame"), "output_frame": LaunchConfiguration("base_frame"), - # "min_z": self.vehicle_info["min_height_offset"], - # "max_z": self.vehicle_info["max_height_offset"], + "max_z": max_z, + "min_z": min_z, }, self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"], ], @@ -116,6 +128,18 @@ def create_additional_pipeline(self, lidar_name): return components def create_ransac_pipeline(self): + max_z = ( + self.vehicle_info["max_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_max_z" + ] + ) + min_z = ( + self.vehicle_info["min_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_min_z" + ] + ) components = [] components.append( ComposableNode( @@ -131,6 +155,8 @@ def create_ransac_pipeline(self): { "input_frame": LaunchConfiguration("base_frame"), "output_frame": LaunchConfiguration("base_frame"), + "max_z": max_z, + "min_z": min_z, }, self.ground_segmentation_param["short_height_obstacle_detection_area_filter"][ "parameters" @@ -184,6 +210,18 @@ def create_ransac_pipeline(self): return components def create_common_pipeline(self, input_topic, output_topic): + max_z = ( + self.vehicle_info["max_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_max_z" + ] + ) + min_z = ( + self.vehicle_info["min_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_min_z" + ] + ) components = [] components.append( ComposableNode( @@ -198,8 +236,8 @@ def create_common_pipeline(self, input_topic, output_topic): { "input_frame": LaunchConfiguration("base_frame"), "output_frame": LaunchConfiguration("base_frame"), - "min_z": self.vehicle_info["min_height_offset"], - "max_z": self.vehicle_info["max_height_offset"], + "max_z": max_z, + "min_z": min_z, }, self.ground_segmentation_param["common_crop_box_filter"]["parameters"], ], From 25e24cc924b45f31c14dca8e871f152065b052c5 Mon Sep 17 00:00:00 2001 From: Takeshi Miura Date: Thu, 2 May 2024 12:31:38 +0900 Subject: [PATCH 28/50] feat: use nebula driver for aip_x1_1 sensor configuration (#235) * feat: use nebula driver for aip_x1_1 Signed-off-by: 1222-takeshi * ci(pre-commit): autofix * chore: remove comment out Signed-off-by: 1222-takeshi * chore: remove unnecessary changes Signed-off-by: 1222-takeshi --------- Signed-off-by: 1222-takeshi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- aip_x1_1_launch/launch/lidar.launch.xml | 26 ++++++++++++++----------- aip_x1_1_launch/package.xml | 6 +----- common_sensor_launch/package.xml | 1 - 3 files changed, 16 insertions(+), 17 deletions(-) diff --git a/aip_x1_1_launch/launch/lidar.launch.xml b/aip_x1_1_launch/launch/lidar.launch.xml index b2457f76..72e0c41d 100644 --- a/aip_x1_1_launch/launch/lidar.launch.xml +++ b/aip_x1_1_launch/launch/lidar.launch.xml @@ -1,6 +1,7 @@ + @@ -12,10 +13,11 @@ - + - + + @@ -25,18 +27,20 @@ - - - - - - + + + + + - - + + + + - + + diff --git a/aip_x1_1_launch/package.xml b/aip_x1_1_launch/package.xml index 8e7f2600..f20fb08e 100644 --- a/aip_x1_1_launch/package.xml +++ b/aip_x1_1_launch/package.xml @@ -10,22 +10,18 @@ ament_cmake_auto + common_sensor_launch compare_map_segmentation elevation_map_loader ground_segmentation imu_corrector individual_params occupancy_grid_map_outlier_filter - pandar_driver - pandar_pointcloud pointcloud_preprocessor rclcpp_components ros2_socketcan tamagawa_imu_driver topic_state_monitor - velodyne_driver - velodyne_monitor - velodyne_pointcloud ament_lint_auto autoware_lint_common diff --git a/common_sensor_launch/package.xml b/common_sensor_launch/package.xml index 016a653f..c0c1170b 100644 --- a/common_sensor_launch/package.xml +++ b/common_sensor_launch/package.xml @@ -17,7 +17,6 @@ dummy_diag_publisher nebula_sensor_driver velodyne_monitor - ament_lint_auto autoware_lint_common From fa404390105e66c4e318bfb0fca48f963546f84f Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Thu, 2 May 2024 18:47:16 +0900 Subject: [PATCH 29/50] chore: remove aip_x1 and rename aip_x1_1 to aip_x1 Signed-off-by: 1222-takeshi --- aip_x1_1_description/CMakeLists.txt | 11 - .../config/sensor_kit_calibration.yaml | 29 -- .../config/sensors_calibration.yaml | 8 - aip_x1_1_description/package.xml | 18 -- aip_x1_1_description/urdf/sensor_kit.xacro | 79 ------ aip_x1_1_description/urdf/sensors.xacro | 21 -- aip_x1_1_launch/CMakeLists.txt | 15 -- .../sensor_kit.param.yaml | 80 ------ aip_x1_1_launch/launch/gnss.launch.xml | 28 -- aip_x1_1_launch/launch/imu.launch.xml | 38 --- aip_x1_1_launch/launch/lidar.launch.xml | 56 ---- .../launch/pointcloud_preprocessor.launch.py | 87 ------ aip_x1_1_launch/launch/sensing.launch.xml | 36 --- .../launch/topic_state_monitor.launch.py | 155 ----------- .../launch/velodyne_VLP16.launch.xml | 41 --- .../launch/velodyne_node_container.launch.py | 250 ------------------ aip_x1_1_launch/package.xml | 32 --- aip_x1_description/CMakeLists.txt | 2 +- .../config/sensor_kit_calibration.yaml | 35 +-- aip_x1_description/package.xml | 8 +- aip_x1_description/urdf/sensor_kit.xacro | 43 +-- aip_x1_description/urdf/sensors.xacro | 6 +- aip_x1_launch/CMakeLists.txt | 3 +- .../sensor_kit.param.yaml | 63 +---- .../sensor_kit.param.yaml | 30 --- .../elevation_map_parameters.yaml | 0 .../ground_segmentation.param.yaml | 0 .../config/gyro_bias_estimator.param.yaml | 6 - aip_x1_launch/data/traffic_light_camera.yaml | 20 -- aip_x1_launch/launch/camera.launch.xml | 24 -- .../ground_segmentation.launch.py | 0 aip_x1_launch/launch/imu.launch.xml | 7 +- aip_x1_launch/launch/lidar.launch.xml | 55 ++-- .../launch/new_livox_horizon.launch.py | 127 --------- .../launch/pandar_node_container.launch.py | 0 .../launch/pointcloud_preprocessor.launch.py | 5 +- aip_x1_launch/launch/sensing.launch.xml | 17 +- .../launch/topic_state_monitor.launch.py | 126 +++------ .../launch/topic_state_monitor.launch.xml | 97 ------- .../launch/velodyne_VLP16.launch.xml | 2 +- .../launch/velodyne_node_container.launch.py | 2 +- aip_x1_launch/package.xml | 11 +- 42 files changed, 116 insertions(+), 1557 deletions(-) delete mode 100644 aip_x1_1_description/CMakeLists.txt delete mode 100644 aip_x1_1_description/config/sensor_kit_calibration.yaml delete mode 100644 aip_x1_1_description/config/sensors_calibration.yaml delete mode 100644 aip_x1_1_description/package.xml delete mode 100644 aip_x1_1_description/urdf/sensor_kit.xacro delete mode 100644 aip_x1_1_description/urdf/sensors.xacro delete mode 100644 aip_x1_1_launch/CMakeLists.txt delete mode 100644 aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml delete mode 100644 aip_x1_1_launch/launch/gnss.launch.xml delete mode 100644 aip_x1_1_launch/launch/imu.launch.xml delete mode 100644 aip_x1_1_launch/launch/lidar.launch.xml delete mode 100644 aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py delete mode 100644 aip_x1_1_launch/launch/sensing.launch.xml delete mode 100644 aip_x1_1_launch/launch/topic_state_monitor.launch.py delete mode 100644 aip_x1_1_launch/launch/velodyne_VLP16.launch.xml delete mode 100644 aip_x1_1_launch/launch/velodyne_node_container.launch.py delete mode 100644 aip_x1_1_launch/package.xml delete mode 100644 aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml rename {aip_x1_1_launch => aip_x1_launch}/config/ground_segmentation/elevation_map_parameters.yaml (100%) rename {aip_x1_1_launch => aip_x1_launch}/config/ground_segmentation/ground_segmentation.param.yaml (100%) delete mode 100644 aip_x1_launch/config/gyro_bias_estimator.param.yaml delete mode 100644 aip_x1_launch/data/traffic_light_camera.yaml delete mode 100644 aip_x1_launch/launch/camera.launch.xml rename {aip_x1_1_launch => aip_x1_launch}/launch/ground_segmentation/ground_segmentation.launch.py (100%) delete mode 100644 aip_x1_launch/launch/new_livox_horizon.launch.py rename {aip_x1_1_launch => aip_x1_launch}/launch/pandar_node_container.launch.py (100%) delete mode 100644 aip_x1_launch/launch/topic_state_monitor.launch.xml diff --git a/aip_x1_1_description/CMakeLists.txt b/aip_x1_1_description/CMakeLists.txt deleted file mode 100644 index 9e5669bf..00000000 --- a/aip_x1_1_description/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(aip_x1_1_description) - -find_package(ament_cmake_auto REQUIRED) - -ament_auto_find_build_dependencies() - -ament_auto_package(INSTALL_TO_SHARE - urdf - config -) diff --git a/aip_x1_1_description/config/sensor_kit_calibration.yaml b/aip_x1_1_description/config/sensor_kit_calibration.yaml deleted file mode 100644 index 70133656..00000000 --- a/aip_x1_1_description/config/sensor_kit_calibration.yaml +++ /dev/null @@ -1,29 +0,0 @@ -sensor_kit_base_link: - velodyne_top_base_link: - x: 0.000 - y: 0.000 - z: 0.000 - roll: 0.000 - pitch: 0.000 - yaw: 0.000 - pandar_xt32_front_center_base_link: - x: 1.130 - y: 0.038 - z: -1.400 - roll: -0.000 - pitch: -0.00 - yaw: 1.6225 - tamagawa/imu_link: - x: 0.0 - y: 0.0 - z: 0.0 - roll: 3.14159 - pitch: 0.0 - yaw: 0.0 - gnss_link: - x: 0.0 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 diff --git a/aip_x1_1_description/config/sensors_calibration.yaml b/aip_x1_1_description/config/sensors_calibration.yaml deleted file mode 100644 index 796672f1..00000000 --- a/aip_x1_1_description/config/sensors_calibration.yaml +++ /dev/null @@ -1,8 +0,0 @@ -base_link: - sensor_kit_base_link: - x: 0.555 - y: 0.000 - z: 1.810 - roll: 0.000 - pitch: 0.000 - yaw: 0.000 diff --git a/aip_x1_1_description/package.xml b/aip_x1_1_description/package.xml deleted file mode 100644 index 382b29e2..00000000 --- a/aip_x1_1_description/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - aip_x1_1_description - 0.1.0 - The aip_x1_1_description package - - Yohei Mishina - Apache 2 - - ament_cmake_auto - - pandar_description - velodyne_description - - - ament_cmake - - diff --git a/aip_x1_1_description/urdf/sensor_kit.xacro b/aip_x1_1_description/urdf/sensor_kit.xacro deleted file mode 100644 index 7629d304..00000000 --- a/aip_x1_1_description/urdf/sensor_kit.xacro +++ /dev/null @@ -1,79 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_x1_1_description/urdf/sensors.xacro b/aip_x1_1_description/urdf/sensors.xacro deleted file mode 100644 index 6690fb8f..00000000 --- a/aip_x1_1_description/urdf/sensors.xacro +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - diff --git a/aip_x1_1_launch/CMakeLists.txt b/aip_x1_1_launch/CMakeLists.txt deleted file mode 100644 index 707bd8ce..00000000 --- a/aip_x1_1_launch/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(aip_x1_1_launch) - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml deleted file mode 100644 index e32c0fb8..00000000 --- a/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml +++ /dev/null @@ -1,80 +0,0 @@ -/**: - ros__parameters: - sensing: - type: diagnostic_aggregator/AnalyzerGroup - path: sensing - analyzers: - node_alive_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: node_alive_monitoring - analyzers: - topic_status: - type: diagnostic_aggregator/GenericAnalyzer - path: topic_status - contains: [": sensing_topic_status"] - timeout: 1.0 - lidar: - type: diagnostic_aggregator/AnalyzerGroup - path: lidar - analyzers: - velodyne: - type: diagnostic_aggregator/AnalyzerGroup - path: velodyne - analyzers: - health_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: health_monitoring - analyzers: - connection: - type: diagnostic_aggregator/GenericAnalyzer - path: connection - contains: [": velodyne_connection"] - timeout: 3.0 - - temperature: - type: diagnostic_aggregator/GenericAnalyzer - path: temperature - contains: [": velodyne_temperature"] - timeout: 3.0 - - rpm: - type: diagnostic_aggregator/GenericAnalyzer - path: rpm - contains: [": velodyne_rpm"] - timeout: 3.0 - front: - type: diagnostic_aggregator/AnalyzerGroup - path: front - analyzers: - health_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: health_monitoring - analyzers: - connection: - type: diagnostic_aggregator/GenericAnalyzer - path: connection - contains: [": pandar_connection"] - timeout: 1.0 - temperature: - type: diagnostic_aggregator/GenericAnalyzer - path: temperature - contains: [": pandar_temperature"] - timeout: 1.0 - ptp: - type: diagnostic_aggregator/GenericAnalyzer - path: ptp - contains: [": pandar_ptp"] - timeout: 1.0 - imu: - type: diagnostic_aggregator/AnalyzerGroup - path: imu - analyzers: - bias_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: bias_monitoring - analyzers: - gyro_bias_validator: - type: diagnostic_aggregator/GenericAnalyzer - path: gyro_bias_validator - contains: [": gyro_bias_validator"] - timeout: 1.0 diff --git a/aip_x1_1_launch/launch/gnss.launch.xml b/aip_x1_1_launch/launch/gnss.launch.xml deleted file mode 100644 index 26b0de4c..00000000 --- a/aip_x1_1_launch/launch/gnss.launch.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_x1_1_launch/launch/imu.launch.xml b/aip_x1_1_launch/launch/imu.launch.xml deleted file mode 100644 index 2008eda8..00000000 --- a/aip_x1_1_launch/launch/imu.launch.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_x1_1_launch/launch/lidar.launch.xml b/aip_x1_1_launch/launch/lidar.launch.xml deleted file mode 100644 index 72e0c41d..00000000 --- a/aip_x1_1_launch/launch/lidar.launch.xml +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py deleted file mode 100644 index dd2a2c17..00000000 --- a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py +++ /dev/null @@ -1,87 +0,0 @@ -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# 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 launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import LoadComposableNodes -from launch_ros.descriptions import ComposableNode - - -def launch_setup(context, *args, **kwargs): - # set concat filter as a component - concat_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concatenate_data", - remappings=[ - ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), - ("output", "concatenated/pointcloud"), - ], - parameters=[ - { - "input_topics": [ - "/sensing/lidar/top/pointcloud", - "/sensing/lidar/front_center/pointcloud", - ], - "output_frame": LaunchConfiguration("base_frame"), - "timeout_sec": 1.0, - "input_twist_topic_type": "twist", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # load concat or passthrough filter - concat_loader = LoadComposableNodes( - composable_node_descriptions=[concat_component], - target_container=LaunchConfiguration("pointcloud_container_name"), - condition=IfCondition(LaunchConfiguration("use_concat_filter")), - ) - - return [concat_loader] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None): - launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) - - add_launch_arg("base_frame", "base_link") - add_launch_arg("use_multithread", "False") - add_launch_arg("use_intra_process", "False") - add_launch_arg("container_name", "pointcloud_preprocessor_container") - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - - return launch.LaunchDescription( - launch_arguments - + [set_container_executable, set_container_mt_executable] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/aip_x1_1_launch/launch/sensing.launch.xml b/aip_x1_1_launch/launch/sensing.launch.xml deleted file mode 100644 index 1419d429..00000000 --- a/aip_x1_1_launch/launch/sensing.launch.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_x1_1_launch/launch/topic_state_monitor.launch.py b/aip_x1_1_launch/launch/topic_state_monitor.launch.py deleted file mode 100644 index 3bb7d2b7..00000000 --- a/aip_x1_1_launch/launch/topic_state_monitor.launch.py +++ /dev/null @@ -1,155 +0,0 @@ -# Copyright 2021 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# 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. - -from launch.launch_description import LaunchDescription -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - - -def generate_launch_description(): - # Topic Monitor For Front Lidar PointCloud - topic_state_monitor_pandar_front_center = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_pandar_front_center", - parameters=[ - { - "topic": "/sensing/lidar/front_center/pointcloud_raw", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - # Topic Monitor For Concat PointCloud - topic_state_monitor_top_outlier_filtered = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_top_outlier_filtered", - parameters=[ - { - "topic": "/sensing/lidar/top/pointcloud", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - topic_state_monitor_pandar_front_center_outlier_filtered = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_pandar_front_center_outlier_filtered", - parameters=[ - { - "topic": "/sensing/lidar/front_center/pointcloud", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - # Topic Monitor for NoGroundFilter - topic_state_monitor_rough_no_ground = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_rough_no_ground", - parameters=[ - { - "topic": "/perception/obstacle_segmentation/single_frame/pointcloud", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - # topic_state_monitor_short_height_no_ground = ComposableNode( - # package="topic_state_monitor", - # plugin="topic_state_monitor::TopicStateMonitorNode", - # name="topic_state_monitor_short_height_no_ground", - # parameters=[ - # { - # "topic": "/perception/obstacle_segmentation/plane_fitting/pointcloud", - # "topic_type": "sensor_msgs/msg/PointCloud2", - # "best_effort": True, - # "diag_name": "sensing_topic_status", - # "warn_rate": 5.0, - # "error_rate": 1.0, - # "timeout": 1.0, - # "window_size": 10, - # }, - # ], - # extra_arguments=[{"use_intra_process_comms": True}], - # ) - - # topic monitor for tamagawa IMU - topic_state_monitor_imu_data = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_imu_data", - parameters=[ - { - "topic": "/sensing/imu/imu_data", - "topic_type": "sensor_msgs/msg/Imu", - "best_effort": False, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - name="topic_state_monitor_container", - namespace="topic_state_monitor", - package="rclcpp_components", - executable="component_container", - composable_node_descriptions=[ - topic_state_monitor_pandar_front_center, - topic_state_monitor_top_outlier_filtered, - topic_state_monitor_pandar_front_center_outlier_filtered, - topic_state_monitor_rough_no_ground, - # topic_state_monitor_short_height_no_ground, - topic_state_monitor_imu_data, - ], - output="screen", - ) - - return LaunchDescription([container]) diff --git a/aip_x1_1_launch/launch/velodyne_VLP16.launch.xml b/aip_x1_1_launch/launch/velodyne_VLP16.launch.xml deleted file mode 100644 index 8ad01878..00000000 --- a/aip_x1_1_launch/launch/velodyne_VLP16.launch.xml +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_x1_1_launch/launch/velodyne_node_container.launch.py b/aip_x1_1_launch/launch/velodyne_node_container.launch.py deleted file mode 100644 index a660f023..00000000 --- a/aip_x1_1_launch/launch/velodyne_node_container.launch.py +++ /dev/null @@ -1,250 +0,0 @@ -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# 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 launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes -from launch_ros.descriptions import ComposableNode - - -def get_vehicle_info(context): - gp = context.launch_configurations.get("ros_params", {}) - if not gp: - gp = dict(context.launch_configurations.get("global_params", {})) - p = {} - p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] - p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] - p["min_longitudinal_offset"] = -gp["rear_overhang"] - p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] - p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) - p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] - p["min_height_offset"] = 0.0 - p["max_height_offset"] = gp["vehicle_height"] - return p - - -def launch_setup(context, *args, **kwargs): - def create_parameter_dict(*args): - result = {} - for x in args: - result[x] = LaunchConfiguration(x) - return result - - nodes = [] - - # turn packets into pointcloud as in - # https://github.com/ros-drivers/velodyne/blob/ros2/velodyne_pointcloud/launch/velodyne_convert_node-VLP16-composed-launch.py - nodes.append( - ComposableNode( - package="velodyne_pointcloud", - plugin="velodyne_pointcloud::Convert", - name="velodyne_convert_node", - parameters=[ - { - **create_parameter_dict( - "calibration", - "min_range", - "max_range", - "num_points_thresholds", - "invalid_intensity", - "frame_id", - "scan_phase", - "view_direction", - "view_width", - ), - } - ], - remappings=[ - ("velodyne_points", "pointcloud_raw"), - ("velodyne_points_ex", "pointcloud_raw_ex"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - cropbox_parameters = create_parameter_dict("input_frame", "output_frame") - cropbox_parameters["negative"] = True - - vehicle_info = get_vehicle_info(context) - cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] - cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] - cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] - cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] - cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] - cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] - - nodes.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_self", - remappings=[ - ("input", "pointcloud_raw_ex"), - ("output", "self_cropped/pointcloud_ex"), - ], - parameters=[cropbox_parameters], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - # 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="pointcloud_preprocessor", - plugin="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", "self_cropped/pointcloud_ex"), - ("~/output/pointcloud", "rectified/pointcloud_ex"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - nodes.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::RingOutlierFilterComponent", - name="ring_outlier_filter", - remappings=[ - ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - # need unique name, otherwise all processes in same container and the node names then clash - name=LaunchConfiguration("container_name"), - namespace="pointcloud_preprocessor", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=nodes, - ) - - driver_component = ComposableNode( - package="velodyne_driver", - plugin="velodyne_driver::VelodyneDriver", - # node is created in a global context, need to avoid name clash - name="velodyne_driver", - parameters=[ - { - **create_parameter_dict( - "device_ip", - "gps_time", - "read_once", - "read_fast", - "repeat_delay", - "frame_id", - "model", - "rpm", - "port", - "pcap", - "scan_phase", - ), - } - ], - ) - - # one way to add a ComposableNode conditional on a launch argument to a - # container. The `ComposableNode` itself doesn't accept a condition - loader = LoadComposableNodes( - composable_node_descriptions=[driver_component], - target_container=container, - condition=launch.conditions.IfCondition(LaunchConfiguration("launch_driver")), - ) - - return [container, loader] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - # a default_value of None is equivalent to not passing that kwarg at all - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - add_launch_arg("model", description="velodyne model name") - add_launch_arg("launch_driver", "True", "do launch driver") - add_launch_arg("calibration", description="path to calibration file") - add_launch_arg("device_ip", "192.168.1.201", "device ip address") - add_launch_arg("scan_phase", "0.0") - add_launch_arg("base_frame", "base_link", "base frame id") - add_launch_arg("container_name", "velodyne_composable_node_container", "container name") - add_launch_arg("min_range", description="minimum view range") - add_launch_arg("max_range", description="maximum view range") - add_launch_arg("pcap", "") - add_launch_arg("port", "2368", description="device port number") - add_launch_arg("read_fast", "False") - add_launch_arg("read_once", "False") - add_launch_arg("repeat_delay", "0.0") - add_launch_arg("rpm", "600.0", "rotational frequency") - add_launch_arg("laserscan_ring", "-1") - add_launch_arg("laserscan_resolution", "0.007") - add_launch_arg("num_points_thresholds", "300") - add_launch_arg("invalid_intensity") - add_launch_arg("frame_id", "velodyne", "velodyne frame id") - add_launch_arg("gps_time", "False") - add_launch_arg("view_direction", description="the center of lidar angle") - add_launch_arg("view_width", description="lidar angle: 0~6.28 [rad]") - 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( - "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" - ) - add_launch_arg("use_multithread", "False", "use multithread") - add_launch_arg("use_intra_process", "False", "use ROS2 component container communication") - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - - return launch.LaunchDescription( - launch_arguments - + [set_container_executable, set_container_mt_executable] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/aip_x1_1_launch/package.xml b/aip_x1_1_launch/package.xml deleted file mode 100644 index f20fb08e..00000000 --- a/aip_x1_1_launch/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - aip_x1_1_launch - 0.1.0 - The aip_x1_1_launch package - - Yohei Mishina - Apache License 2.0 - - ament_cmake_auto - - common_sensor_launch - compare_map_segmentation - elevation_map_loader - ground_segmentation - imu_corrector - individual_params - occupancy_grid_map_outlier_filter - pointcloud_preprocessor - rclcpp_components - ros2_socketcan - tamagawa_imu_driver - topic_state_monitor - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/aip_x1_description/CMakeLists.txt b/aip_x1_description/CMakeLists.txt index 56f4d802..9e5669bf 100644 --- a/aip_x1_description/CMakeLists.txt +++ b/aip_x1_description/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(aip_x1_description) +project(aip_x1_1_description) find_package(ament_cmake_auto REQUIRED) diff --git a/aip_x1_description/config/sensor_kit_calibration.yaml b/aip_x1_description/config/sensor_kit_calibration.yaml index 93dbfbfe..70133656 100644 --- a/aip_x1_description/config/sensor_kit_calibration.yaml +++ b/aip_x1_description/config/sensor_kit_calibration.yaml @@ -6,27 +6,20 @@ sensor_kit_base_link: roll: 0.000 pitch: 0.000 yaw: 0.000 - livox_front_left_base_link: - x: 1.054 - y: 0.260 - z: -1.330 - roll: 0.000 - pitch: 0.000 - yaw: 1.047 - livox_front_center_base_link: - x: 1.054 - y: 0.000 - z: -1.330 - roll: 0.000 - pitch: 0.000 - yaw: 0.000 - livox_front_right_base_link: - x: 1.054 - y: -0.260 - z: -1.330 - roll: 0.000 - pitch: 0.000 - yaw: -1.047 + pandar_xt32_front_center_base_link: + x: 1.130 + y: 0.038 + z: -1.400 + roll: -0.000 + pitch: -0.00 + yaw: 1.6225 + tamagawa/imu_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 3.14159 + pitch: 0.0 + yaw: 0.0 gnss_link: x: 0.0 y: 0.0 diff --git a/aip_x1_description/package.xml b/aip_x1_description/package.xml index 327796c0..382b29e2 100644 --- a/aip_x1_description/package.xml +++ b/aip_x1_description/package.xml @@ -1,15 +1,15 @@ - aip_x1_description + aip_x1_1_description 0.1.0 - The aip_xx1_description package + The aip_x1_1_description package - Yukihiro Saito + Yohei Mishina Apache 2 ament_cmake_auto - livox_description + pandar_description velodyne_description diff --git a/aip_x1_description/urdf/sensor_kit.xacro b/aip_x1_description/urdf/sensor_kit.xacro index 649ba65f..7629d304 100644 --- a/aip_x1_description/urdf/sensor_kit.xacro +++ b/aip_x1_description/urdf/sensor_kit.xacro @@ -5,7 +5,7 @@ - + @@ -18,7 +18,7 @@ - + - - - - + - - - + + + - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py b/aip_x1_launch/launch/ground_segmentation/ground_segmentation.launch.py similarity index 100% rename from aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py rename to aip_x1_launch/launch/ground_segmentation/ground_segmentation.launch.py diff --git a/aip_x1_launch/launch/imu.launch.xml b/aip_x1_launch/launch/imu.launch.xml index 9579dd64..9d328090 100644 --- a/aip_x1_launch/launch/imu.launch.xml +++ b/aip_x1_launch/launch/imu.launch.xml @@ -1,8 +1,4 @@ - - - - @@ -10,7 +6,7 @@ - + @@ -19,7 +15,6 @@ - diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml index ee0d93d0..72e0c41d 100644 --- a/aip_x1_launch/launch/lidar.launch.xml +++ b/aip_x1_launch/launch/lidar.launch.xml @@ -1,23 +1,22 @@ + + - - - - - + + @@ -26,40 +25,32 @@ - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + - + - + diff --git a/aip_x1_launch/launch/new_livox_horizon.launch.py b/aip_x1_launch/launch/new_livox_horizon.launch.py deleted file mode 100644 index 26826a72..00000000 --- a/aip_x1_launch/launch/new_livox_horizon.launch.py +++ /dev/null @@ -1,127 +0,0 @@ -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# 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 launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes -from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode -import yaml - - -def get_livox_tag_filter_component(): - # livox tag filter - livox_tag_filter_component = ComposableNode( - package="livox_tag_filter", - plugin="livox_tag_filter::LivoxTagFilterNode", - name="livox_tag_filter", - remappings=[ - ("input", "livox/lidar"), - ("output", "livox/tag_filtered/lidar"), - ], - parameters=[ - { - "ignore_tags": [1, 2, 20, 21, 22, 23, 24], - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - return livox_tag_filter_component - - -def get_crop_box_min_range_component(context, livox_frame_id): - use_tag_filter = IfCondition(LaunchConfiguration("use_tag_filter")).evaluate(context) - crop_box_min_range_component = ComposableNode( - package="autoware_pointcloud_preprocessor", - plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_min_range", - remappings=[ - ("input", "livox/tag_filtered/lidar" if use_tag_filter else "livox/lidar"), - ("output", "pointcloud"), - ], - parameters=[ - { - "input_frame": livox_frame_id, - "output_frame": LaunchConfiguration("frame_id"), - "min_x": 0.0, - "max_x": LaunchConfiguration("min_range"), - "min_y": -2.0, - "max_y": 2.0, - "min_z": -2.0, - "max_z": 2.0, - "negative": True, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - return crop_box_min_range_component - - -def launch_setup(context, *args, **kwargs): - lidar_config_path = LaunchConfiguration("lidar_config_file").perform(context) - with open(lidar_config_path, "r") as f: - params = yaml.safe_load(f)["/**"]["ros__parameters"] - - livox_driver_node = Node( - executable="lidar_ros_driver_node", - package="lidar_ros_driver", - name="livox_horizon", - remappings=[ - ("livox/cloud", "livox/lidar"), - ("livox/imu_packet", "livox/imu"), - ], - parameters=[params], - condition=IfCondition(LaunchConfiguration("launch_driver")), - output="screen", - ) - - container = ComposableNodeContainer( - name="livox_horizon", - namespace="livox_horizon", - package="rclcpp_components", - executable="component_container", - composable_node_descriptions=[ - get_crop_box_min_range_component(context, params["frame_id"]), - ], - output="screen", - ) - - livox_tag_filter_loader = LoadComposableNodes( - composable_node_descriptions=[get_livox_tag_filter_component()], - target_container=container, - condition=IfCondition(LaunchConfiguration("use_tag_filter")), - ) - - return [livox_driver_node, container, livox_tag_filter_loader] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None): - launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) - - add_launch_arg("launch_driver", "true") - add_launch_arg("base_frame", "base_link") - add_launch_arg("use_tag_filter", "true") - add_launch_arg("lidar_config_file") - - # x1 additional setting - add_launch_arg("min_range", "1.5") - - return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/aip_x1_1_launch/launch/pandar_node_container.launch.py b/aip_x1_launch/launch/pandar_node_container.launch.py similarity index 100% rename from aip_x1_1_launch/launch/pandar_node_container.launch.py rename to aip_x1_launch/launch/pandar_node_container.launch.py diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py index dc79d16c..085a45d9 100644 --- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py @@ -37,8 +37,6 @@ def launch_setup(context, *args, **kwargs): { "input_topics": [ "/sensing/lidar/top/pointcloud", - "/sensing/lidar/front_left/pointcloud", - "/sensing/lidar/front_right/pointcloud", "/sensing/lidar/front_center/pointcloud", ], "output_frame": LaunchConfiguration("base_frame"), @@ -54,6 +52,7 @@ def launch_setup(context, *args, **kwargs): concat_loader = LoadComposableNodes( composable_node_descriptions=[concat_component], target_container=LaunchConfiguration("pointcloud_container_name"), + condition=IfCondition(LaunchConfiguration("use_concat_filter")), ) return [concat_loader] @@ -68,7 +67,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("base_frame", "base_link") add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False") - add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("container_name", "pointcloud_preprocessor_container") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/aip_x1_launch/launch/sensing.launch.xml b/aip_x1_launch/launch/sensing.launch.xml index b8b04a0d..1419d429 100644 --- a/aip_x1_launch/launch/sensing.launch.xml +++ b/aip_x1_launch/launch/sensing.launch.xml @@ -5,32 +5,23 @@ - - - - + - - - - - - + - - + @@ -38,7 +29,7 @@ - + diff --git a/aip_x1_launch/launch/topic_state_monitor.launch.py b/aip_x1_launch/launch/topic_state_monitor.launch.py index 3abbfc22..3bb7d2b7 100644 --- a/aip_x1_launch/launch/topic_state_monitor.launch.py +++ b/aip_x1_launch/launch/topic_state_monitor.launch.py @@ -18,50 +18,14 @@ def generate_launch_description(): - # Topic Monitor For Livox Raw PointCloud - topic_state_monitor_livox_front_center = ComposableNode( + # Topic Monitor For Front Lidar PointCloud + topic_state_monitor_pandar_front_center = ComposableNode( package="topic_state_monitor", plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_livox_front_center", + name="topic_state_monitor_pandar_front_center", parameters=[ { - "topic": "/sensing/lidar/front_center/livox/lidar", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - topic_state_monitor_livox_front_left = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_livox_front_left", - parameters=[ - { - "topic": "/sensing/lidar/front_left/livox/lidar", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - topic_state_monitor_livox_front_right = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_livox_front_right", - parameters=[ - { - "topic": "/sensing/lidar/front_right/livox/lidar", + "topic": "/sensing/lidar/front_center/pointcloud_raw", "topic_type": "sensor_msgs/msg/PointCloud2", "best_effort": True, "diag_name": "sensing_topic_status", @@ -93,46 +57,11 @@ def generate_launch_description(): ], extra_arguments=[{"use_intra_process_comms": True}], ) - topic_state_monitor_livox_front_left_min_range_cropped = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_livox_front_left_min_range_cropped", - parameters=[ - { - "topic": "/sensing/lidar/front_left/pointcloud", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - topic_state_monitor_livox_front_right_min_range_cropped = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_livox_front_right_min_range_cropped", - parameters=[ - { - "topic": "/sensing/lidar/front_right/pointcloud", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - topic_state_monitor_livox_front_center_min_range_cropped = ComposableNode( + + topic_state_monitor_pandar_front_center_outlier_filtered = ComposableNode( package="topic_state_monitor", plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_livox_front_center_min_range_cropped", + name="topic_state_monitor_pandar_front_center_outlier_filtered", parameters=[ { "topic": "/sensing/lidar/front_center/pointcloud", @@ -167,15 +96,35 @@ def generate_launch_description(): ], extra_arguments=[{"use_intra_process_comms": True}], ) - topic_state_monitor_short_height_no_ground = ComposableNode( + # topic_state_monitor_short_height_no_ground = ComposableNode( + # package="topic_state_monitor", + # plugin="topic_state_monitor::TopicStateMonitorNode", + # name="topic_state_monitor_short_height_no_ground", + # parameters=[ + # { + # "topic": "/perception/obstacle_segmentation/plane_fitting/pointcloud", + # "topic_type": "sensor_msgs/msg/PointCloud2", + # "best_effort": True, + # "diag_name": "sensing_topic_status", + # "warn_rate": 5.0, + # "error_rate": 1.0, + # "timeout": 1.0, + # "window_size": 10, + # }, + # ], + # extra_arguments=[{"use_intra_process_comms": True}], + # ) + + # topic monitor for tamagawa IMU + topic_state_monitor_imu_data = ComposableNode( package="topic_state_monitor", plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_short_height_no_ground", + name="topic_state_monitor_imu_data", parameters=[ { - "topic": "/perception/obstacle_segmentation/plane_fitting/pointcloud", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, + "topic": "/sensing/imu/imu_data", + "topic_type": "sensor_msgs/msg/Imu", + "best_effort": False, "diag_name": "sensing_topic_status", "warn_rate": 5.0, "error_rate": 1.0, @@ -193,15 +142,12 @@ def generate_launch_description(): package="rclcpp_components", executable="component_container", composable_node_descriptions=[ - topic_state_monitor_livox_front_center, - topic_state_monitor_livox_front_left, - topic_state_monitor_livox_front_right, + topic_state_monitor_pandar_front_center, topic_state_monitor_top_outlier_filtered, - topic_state_monitor_livox_front_left_min_range_cropped, - topic_state_monitor_livox_front_right_min_range_cropped, - topic_state_monitor_livox_front_center_min_range_cropped, + topic_state_monitor_pandar_front_center_outlier_filtered, topic_state_monitor_rough_no_ground, - topic_state_monitor_short_height_no_ground, + # topic_state_monitor_short_height_no_ground, + topic_state_monitor_imu_data, ], output="screen", ) diff --git a/aip_x1_launch/launch/topic_state_monitor.launch.xml b/aip_x1_launch/launch/topic_state_monitor.launch.xml deleted file mode 100644 index aa89fccd..00000000 --- a/aip_x1_launch/launch/topic_state_monitor.launch.xml +++ /dev/null @@ -1,97 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_x1_launch/launch/velodyne_VLP16.launch.xml b/aip_x1_launch/launch/velodyne_VLP16.launch.xml index 3156db29..8ad01878 100644 --- a/aip_x1_launch/launch/velodyne_VLP16.launch.xml +++ b/aip_x1_launch/launch/velodyne_VLP16.launch.xml @@ -16,7 +16,7 @@ - + diff --git a/aip_x1_launch/launch/velodyne_node_container.launch.py b/aip_x1_launch/launch/velodyne_node_container.launch.py index d0fe9118..3d1724a7 100644 --- a/aip_x1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_launch/launch/velodyne_node_container.launch.py @@ -143,7 +143,7 @@ def create_parameter_dict(*args): # set container to run all required components in the same process container = ComposableNodeContainer( # need unique name, otherwise all processes in same container and the node names then clash - name="pointcloud_container", + name=LaunchConfiguration("container_name"), namespace="pointcloud_preprocessor", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), diff --git a/aip_x1_launch/package.xml b/aip_x1_launch/package.xml index 84e7ee19..ec99a3fa 100644 --- a/aip_x1_launch/package.xml +++ b/aip_x1_launch/package.xml @@ -1,11 +1,11 @@ - aip_x1_launch + aip_x1_1_launch 0.1.0 - The aip_x1_launch package + The aip_x1_1_launch package - Hiroki OTA + Yohei Mishina Apache License 2.0 ament_cmake_auto @@ -14,10 +14,7 @@ autoware_imu_corrector autoware_pointcloud_preprocessor tamagawa_imu_driver - topic_tools - ublox_gps - usb_cam - vehicle_velocity_converter + topic_state_monitor ament_lint_auto autoware_lint_common From 3f538198d9821788c813e1838f7ef9f6ca0a0330 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Tue, 7 May 2024 13:59:07 +0900 Subject: [PATCH 30/50] chore: rename aip_x1_1 to aip_x1 Signed-off-by: 1222-takeshi --- aip_x1_description/CMakeLists.txt | 2 +- aip_x1_description/package.xml | 6 +++--- aip_x1_description/urdf/sensor_kit.xacro | 2 +- aip_x1_description/urdf/sensors.xacro | 4 ++-- aip_x1_launch/CMakeLists.txt | 2 +- .../ground_segmentation/ground_segmentation.launch.py | 4 ++-- aip_x1_launch/launch/lidar.launch.xml | 6 +++--- aip_x1_launch/launch/sensing.launch.xml | 8 ++++---- aip_x1_launch/launch/velodyne_VLP16.launch.xml | 2 +- aip_x1_launch/package.xml | 6 +++--- 10 files changed, 21 insertions(+), 21 deletions(-) diff --git a/aip_x1_description/CMakeLists.txt b/aip_x1_description/CMakeLists.txt index 9e5669bf..56f4d802 100644 --- a/aip_x1_description/CMakeLists.txt +++ b/aip_x1_description/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(aip_x1_1_description) +project(aip_x1_description) find_package(ament_cmake_auto REQUIRED) diff --git a/aip_x1_description/package.xml b/aip_x1_description/package.xml index 382b29e2..f860fc5e 100644 --- a/aip_x1_description/package.xml +++ b/aip_x1_description/package.xml @@ -1,10 +1,10 @@ - aip_x1_1_description + aip_x1_description 0.1.0 - The aip_x1_1_description package + The aip_x1_description package - Yohei Mishina + Takeshi Miura Apache 2 ament_cmake_auto diff --git a/aip_x1_description/urdf/sensor_kit.xacro b/aip_x1_description/urdf/sensor_kit.xacro index 7629d304..df95f2fc 100644 --- a/aip_x1_description/urdf/sensor_kit.xacro +++ b/aip_x1_description/urdf/sensor_kit.xacro @@ -5,7 +5,7 @@ - + diff --git a/aip_x1_description/urdf/sensors.xacro b/aip_x1_description/urdf/sensors.xacro index 6690fb8f..56f048b4 100644 --- a/aip_x1_description/urdf/sensors.xacro +++ b/aip_x1_description/urdf/sensors.xacro @@ -1,8 +1,8 @@ - - + + diff --git a/aip_x1_launch/CMakeLists.txt b/aip_x1_launch/CMakeLists.txt index 707bd8ce..c384a1a6 100644 --- a/aip_x1_launch/CMakeLists.txt +++ b/aip_x1_launch/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(aip_x1_1_launch) +project(aip_x1_launch) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() diff --git a/aip_x1_launch/launch/ground_segmentation/ground_segmentation.launch.py b/aip_x1_launch/launch/ground_segmentation/ground_segmentation.launch.py index 8cc51174..31d985f5 100644 --- a/aip_x1_launch/launch/ground_segmentation/ground_segmentation.launch.py +++ b/aip_x1_launch/launch/ground_segmentation/ground_segmentation.launch.py @@ -34,7 +34,7 @@ def __init__(self, context): self.context = context self.vehicle_info = self.get_vehicle_info() ground_segmentation_param_path = os.path.join( - get_package_share_directory("aip_x1_1_launch"), + get_package_share_directory("aip_x1_launch"), "config/ground_segmentation/ground_segmentation.param.yaml", ) with open(ground_segmentation_param_path, "r") as f: @@ -369,7 +369,7 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic): "inpaint_radius": 1.0, "param_file_path": PathJoinSubstitution( [ - FindPackageShare("aip_x1_1_launch"), + FindPackageShare("aip_x1_launch"), "config", "ground_segmentation", "elevation_map_parameters.yaml", diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml index 72e0c41d..348f1cba 100644 --- a/aip_x1_launch/launch/lidar.launch.xml +++ b/aip_x1_launch/launch/lidar.launch.xml @@ -38,19 +38,19 @@ - + - + - + diff --git a/aip_x1_launch/launch/sensing.launch.xml b/aip_x1_launch/launch/sensing.launch.xml index 1419d429..0f23a733 100644 --- a/aip_x1_launch/launch/sensing.launch.xml +++ b/aip_x1_launch/launch/sensing.launch.xml @@ -8,7 +8,7 @@ - + @@ -16,12 +16,12 @@ - + - + @@ -29,7 +29,7 @@ - + diff --git a/aip_x1_launch/launch/velodyne_VLP16.launch.xml b/aip_x1_launch/launch/velodyne_VLP16.launch.xml index 8ad01878..3156db29 100644 --- a/aip_x1_launch/launch/velodyne_VLP16.launch.xml +++ b/aip_x1_launch/launch/velodyne_VLP16.launch.xml @@ -16,7 +16,7 @@ - + diff --git a/aip_x1_launch/package.xml b/aip_x1_launch/package.xml index ec99a3fa..1961d9ea 100644 --- a/aip_x1_launch/package.xml +++ b/aip_x1_launch/package.xml @@ -1,11 +1,11 @@ - aip_x1_1_launch + aip_x1_launch 0.1.0 - The aip_x1_1_launch package + The aip_x1_launch package - Yohei Mishina + Takeshi Miura Apache License 2.0 ament_cmake_auto From 3ea091cb50a8d966f6da83039ee30e65b5b65d17 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Tue, 7 May 2024 15:24:47 +0900 Subject: [PATCH 31/50] chore: remove unused file and fix bug Signed-off-by: 1222-takeshi --- aip_x1_description/urdf/sensor_kit.xacro | 2 +- aip_x1_description/urdf/sensors.xacro | 2 +- .../sensor_kit.param.yaml | 4 +- .../sensor_kit.param.yaml | 33 + .../elevation_map_parameters.yaml | 30 - .../ground_segmentation.param.yaml | 78 --- .../config/gyro_bias_estimator.param.yaml | 6 + .../ground_segmentation.launch.py | 573 ------------------ aip_x1_launch/launch/lidar.launch.xml | 1 - .../launch/pandar_node_container.launch.py | 231 ------- .../launch/topic_state_monitor.launch.py | 155 ----- .../launch/velodyne_VLP16.launch.xml | 41 -- .../launch/velodyne_node_container.launch.py | 245 -------- 13 files changed, 43 insertions(+), 1358 deletions(-) create mode 100644 aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml delete mode 100644 aip_x1_launch/config/ground_segmentation/elevation_map_parameters.yaml delete mode 100644 aip_x1_launch/config/ground_segmentation/ground_segmentation.param.yaml create mode 100644 aip_x1_launch/config/gyro_bias_estimator.param.yaml delete mode 100644 aip_x1_launch/launch/ground_segmentation/ground_segmentation.launch.py delete mode 100644 aip_x1_launch/launch/pandar_node_container.launch.py delete mode 100644 aip_x1_launch/launch/topic_state_monitor.launch.py delete mode 100644 aip_x1_launch/launch/velodyne_VLP16.launch.xml delete mode 100644 aip_x1_launch/launch/velodyne_node_container.launch.py diff --git a/aip_x1_description/urdf/sensor_kit.xacro b/aip_x1_description/urdf/sensor_kit.xacro index df95f2fc..8845547e 100644 --- a/aip_x1_description/urdf/sensor_kit.xacro +++ b/aip_x1_description/urdf/sensor_kit.xacro @@ -18,7 +18,7 @@ - + - + - diff --git a/aip_x1_launch/launch/pandar_node_container.launch.py b/aip_x1_launch/launch/pandar_node_container.launch.py deleted file mode 100644 index fc102338..00000000 --- a/aip_x1_launch/launch/pandar_node_container.launch.py +++ /dev/null @@ -1,231 +0,0 @@ -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# 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 -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes -from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode -import yaml - - -def get_pandar_monitor_info(): - path = os.path.join( - get_package_share_directory("pandar_monitor"), - "config", - "pandar_monitor.param.yaml", - ) - with open(path, "r") as f: - p = yaml.safe_load(f)["/**"]["ros__parameters"] - return p - - -def get_vehicle_info(context): - # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support - # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py - gp = context.launch_configurations.get("ros_params", {}) - if not gp: - gp = dict(context.launch_configurations.get("global_params", {})) - p = {} - p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] - p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] - p["min_longitudinal_offset"] = -gp["rear_overhang"] - p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] - p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) - p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] - p["min_height_offset"] = -0.3 # margin to crop pointcloud under vehicle - p["max_height_offset"] = gp["vehicle_height"] - return p - - -def launch_setup(context, *args, **kwargs): - def create_parameter_dict(*args): - result = {} - for x in args: - result[x] = LaunchConfiguration(x) - return result - - monitor_node = Node( - executable="pandar_monitor_node", - package="pandar_monitor", - name="pandar_monitor", - parameters=[ - { - "ip_address": LaunchConfiguration("device_ip"), - } - ] - + [get_pandar_monitor_info()], - condition=IfCondition(LaunchConfiguration("launch_driver")), - output="screen", - ) - - driver_component = ComposableNode( - package="pandar_driver", - plugin="pandar_driver::PandarDriver", - name="pandar_driver", - parameters=[ - { - **create_parameter_dict( - "pcap", "device_ip", "lidar_port", "gps_port", "scan_phase", "model", "frame_id" - ) - } - ], - ) - - pointcloud_component = ComposableNode( - package="pandar_pointcloud", - plugin="pandar_pointcloud::PandarCloud", - name="pandar_cloud", - parameters=[ - { - **create_parameter_dict( - "model", - "scan_phase", - "angle_range", - "distance_range", - "device_ip", - "calibration", - "return_mode", - ) - } - ], - remappings=[("pandar_points", "pointcloud_raw"), ("pandar_points_ex", "pointcloud_raw_ex")], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - cropbox_parameters = create_parameter_dict("input_frame", "output_frame") - cropbox_parameters["negative"] = True - - vehicle_info = get_vehicle_info(context) - cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] - cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] - cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] - cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] - cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] - cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] - - self_crop_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_self", - remappings=[ - ("input", "pointcloud_raw_ex"), - ("output", "self_cropped/pointcloud_ex"), - ], - parameters=[cropbox_parameters], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - undistort_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::DistortionCorrectorComponent", - name="distortion_corrector_node", - remappings=[ - ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), - ("~/input/imu", "/sensing/imu/imu_data"), - ("~/input/velocity_report", "/vehicle/status/velocity_status"), - ("~/input/pointcloud", "self_cropped/pointcloud_ex"), - ("~/output/pointcloud", "rectified/pointcloud_ex"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - ring_outlier_filter_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::RingOutlierFilterComponent", - name="ring_outlier_filter", - remappings=[ - ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - container = ComposableNodeContainer( - name=LaunchConfiguration("pointcloud_container_name"), - namespace="pointcloud_preprocessor", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[ - pointcloud_component, - self_crop_component, - undistort_component, - ring_outlier_filter_component, - ], - ) - - driver_loader = LoadComposableNodes( - composable_node_descriptions=[driver_component], - target_container=container, - condition=launch.conditions.IfCondition(LaunchConfiguration("launch_driver")), - ) - - return [ - container, - driver_loader, - monitor_node, - ] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None): - launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) - - add_launch_arg("model") - add_launch_arg("launch_driver", "true") - add_launch_arg("calibration", "") - add_launch_arg("device_ip", "192.168.1.201") - add_launch_arg("scan_phase", "0.0") - add_launch_arg("angle_range", "[0.0, 360.0]") - add_launch_arg("distance_range", "[0.05, 200.0]") - add_launch_arg("return_mode", "Dual") - add_launch_arg("base_frame", "base_link") - add_launch_arg("container_name", "pandar_composable_node_container") - add_launch_arg("pcap", "") - add_launch_arg("lidar_port", "2321") - add_launch_arg("gps_port", "10121") - add_launch_arg("frame_id", "pandar") - add_launch_arg("input_frame", LaunchConfiguration("base_frame")) - add_launch_arg("output_frame", LaunchConfiguration("base_frame")) - add_launch_arg("use_multithread", "False") - add_launch_arg("use_intra_process", "False") - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - - return launch.LaunchDescription( - launch_arguments - + [set_container_executable, set_container_mt_executable] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/aip_x1_launch/launch/topic_state_monitor.launch.py b/aip_x1_launch/launch/topic_state_monitor.launch.py deleted file mode 100644 index 3bb7d2b7..00000000 --- a/aip_x1_launch/launch/topic_state_monitor.launch.py +++ /dev/null @@ -1,155 +0,0 @@ -# Copyright 2021 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# 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. - -from launch.launch_description import LaunchDescription -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - - -def generate_launch_description(): - # Topic Monitor For Front Lidar PointCloud - topic_state_monitor_pandar_front_center = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_pandar_front_center", - parameters=[ - { - "topic": "/sensing/lidar/front_center/pointcloud_raw", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - # Topic Monitor For Concat PointCloud - topic_state_monitor_top_outlier_filtered = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_top_outlier_filtered", - parameters=[ - { - "topic": "/sensing/lidar/top/pointcloud", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - topic_state_monitor_pandar_front_center_outlier_filtered = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_pandar_front_center_outlier_filtered", - parameters=[ - { - "topic": "/sensing/lidar/front_center/pointcloud", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - # Topic Monitor for NoGroundFilter - topic_state_monitor_rough_no_ground = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_rough_no_ground", - parameters=[ - { - "topic": "/perception/obstacle_segmentation/single_frame/pointcloud", - "topic_type": "sensor_msgs/msg/PointCloud2", - "best_effort": True, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - # topic_state_monitor_short_height_no_ground = ComposableNode( - # package="topic_state_monitor", - # plugin="topic_state_monitor::TopicStateMonitorNode", - # name="topic_state_monitor_short_height_no_ground", - # parameters=[ - # { - # "topic": "/perception/obstacle_segmentation/plane_fitting/pointcloud", - # "topic_type": "sensor_msgs/msg/PointCloud2", - # "best_effort": True, - # "diag_name": "sensing_topic_status", - # "warn_rate": 5.0, - # "error_rate": 1.0, - # "timeout": 1.0, - # "window_size": 10, - # }, - # ], - # extra_arguments=[{"use_intra_process_comms": True}], - # ) - - # topic monitor for tamagawa IMU - topic_state_monitor_imu_data = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_imu_data", - parameters=[ - { - "topic": "/sensing/imu/imu_data", - "topic_type": "sensor_msgs/msg/Imu", - "best_effort": False, - "diag_name": "sensing_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 1.0, - "window_size": 10, - }, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - name="topic_state_monitor_container", - namespace="topic_state_monitor", - package="rclcpp_components", - executable="component_container", - composable_node_descriptions=[ - topic_state_monitor_pandar_front_center, - topic_state_monitor_top_outlier_filtered, - topic_state_monitor_pandar_front_center_outlier_filtered, - topic_state_monitor_rough_no_ground, - # topic_state_monitor_short_height_no_ground, - topic_state_monitor_imu_data, - ], - output="screen", - ) - - return LaunchDescription([container]) diff --git a/aip_x1_launch/launch/velodyne_VLP16.launch.xml b/aip_x1_launch/launch/velodyne_VLP16.launch.xml deleted file mode 100644 index 3156db29..00000000 --- a/aip_x1_launch/launch/velodyne_VLP16.launch.xml +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_x1_launch/launch/velodyne_node_container.launch.py b/aip_x1_launch/launch/velodyne_node_container.launch.py deleted file mode 100644 index 3d1724a7..00000000 --- a/aip_x1_launch/launch/velodyne_node_container.launch.py +++ /dev/null @@ -1,245 +0,0 @@ -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# 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 launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes -from launch_ros.descriptions import ComposableNode - - -def get_vehicle_info(context): - gp = context.launch_configurations.get("ros_params", {}) - if not gp: - gp = dict(context.launch_configurations.get("global_params", {})) - p = {} - p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] - p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] - p["min_longitudinal_offset"] = -gp["rear_overhang"] - p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] - p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) - p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] - p["min_height_offset"] = 0.0 - p["max_height_offset"] = gp["vehicle_height"] - return p - - -def launch_setup(context, *args, **kwargs): - def create_parameter_dict(*args): - result = {} - for x in args: - result[x] = LaunchConfiguration(x) - return result - - nodes = [] - - # turn packets into pointcloud as in - # https://github.com/ros-drivers/velodyne/blob/ros2/velodyne_pointcloud/launch/velodyne_convert_node-VLP16-composed-launch.py - nodes.append( - ComposableNode( - package="velodyne_pointcloud", - plugin="velodyne_pointcloud::Convert", - name="velodyne_convert_node", - parameters=[ - { - **create_parameter_dict( - "calibration", - "min_range", - "max_range", - "num_points_thresholds", - "invalid_intensity", - "frame_id", - "scan_phase", - "view_direction", - "view_width", - ), - } - ], - remappings=[ - ("velodyne_points", "pointcloud_raw"), - ("velodyne_points_ex", "pointcloud_raw_ex"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - cropbox_parameters = create_parameter_dict("input_frame", "output_frame") - cropbox_parameters["negative"] = True - - vehicle_info = get_vehicle_info(context) - cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] - cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] - cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] - cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] - cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] - cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] - - nodes.append( - ComposableNode( - package="autoware_pointcloud_preprocessor", - plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_self", - remappings=[ - ("input", "pointcloud_raw_ex"), - ("output", "self_cropped/pointcloud_ex"), - ], - parameters=[cropbox_parameters], - 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", "self_cropped/pointcloud_ex"), - ("~/output/pointcloud", "rectified/pointcloud_ex"), - ], - 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): - ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")} - else: - ring_outlier_filter_parameters = { - "output_frame": "" - } # keep the output frame as the input frame - nodes.append( - ComposableNode( - package="autoware_pointcloud_preprocessor", - plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent", - name="ring_outlier_filter", - remappings=[ - ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud_before_sync"), - ], - parameters=[ring_outlier_filter_parameters], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - # need unique name, otherwise all processes in same container and the node names then clash - name=LaunchConfiguration("container_name"), - namespace="pointcloud_preprocessor", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=nodes, - ) - - driver_component = ComposableNode( - package="velodyne_driver", - plugin="velodyne_driver::VelodyneDriver", - # node is created in a global context, need to avoid name clash - name="velodyne_driver", - parameters=[ - { - **create_parameter_dict( - "device_ip", - "gps_time", - "read_once", - "read_fast", - "repeat_delay", - "frame_id", - "model", - "rpm", - "port", - "pcap", - "scan_phase", - ), - } - ], - ) - - # one way to add a ComposableNode conditional on a launch argument to a - # container. The `ComposableNode` itself doesn't accept a condition - loader = LoadComposableNodes( - composable_node_descriptions=[driver_component], - target_container=container, - condition=launch.conditions.IfCondition(LaunchConfiguration("launch_driver")), - ) - - return [container, loader] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - # a default_value of None is equivalent to not passing that kwarg at all - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - add_launch_arg("model", description="velodyne model name") - add_launch_arg("launch_driver", "True", "do launch driver") - add_launch_arg("calibration", description="path to calibration file") - add_launch_arg("device_ip", "192.168.1.201", "device ip address") - add_launch_arg("scan_phase", "0.0") - add_launch_arg("base_frame", "base_link", "base frame id") - add_launch_arg("container_name", "velodyne_composable_node_container", "container name") - add_launch_arg("min_range", description="minimum view range") - add_launch_arg("max_range", description="maximum view range") - add_launch_arg("pcap", "") - add_launch_arg("port", "2368", description="device port number") - add_launch_arg("read_fast", "False") - add_launch_arg("read_once", "False") - add_launch_arg("repeat_delay", "0.0") - add_launch_arg("rpm", "600.0", "rotational frequency") - add_launch_arg("laserscan_ring", "-1") - add_launch_arg("laserscan_resolution", "0.007") - add_launch_arg("num_points_thresholds", "300") - add_launch_arg("invalid_intensity") - add_launch_arg("frame_id", "velodyne", "velodyne frame id") - add_launch_arg("gps_time", "False") - add_launch_arg("view_direction", description="the center of lidar angle") - add_launch_arg("view_width", description="lidar angle: 0~6.28 [rad]") - 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( - "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" - ) - add_launch_arg("use_multithread", "False", "use multithread") - add_launch_arg("use_intra_process", "False", "use ROS2 component container communication") - add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - - return launch.LaunchDescription( - launch_arguments - + [set_container_executable, set_container_mt_executable] - + [OpaqueFunction(function=launch_setup)] - ) From 78ad120dc2928ed3f1fb2772725b7daab2e0103e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 7 May 2024 06:25:06 +0000 Subject: [PATCH 32/50] ci(pre-commit): autofix --- aip_x1_launch/config/gyro_bias_estimator.param.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/aip_x1_launch/config/gyro_bias_estimator.param.yaml b/aip_x1_launch/config/gyro_bias_estimator.param.yaml index 49e3f644..d552569f 100644 --- a/aip_x1_launch/config/gyro_bias_estimator.param.yaml +++ b/aip_x1_launch/config/gyro_bias_estimator.param.yaml @@ -1,6 +1,6 @@ -/**: - ros__parameters: - gyro_bias_threshold: 0.008 # [rad/s] - timer_callback_interval_sec: 0.5 # [sec] - diagnostics_updater_interval_sec: 0.5 # [sec] +/**: + ros__parameters: + gyro_bias_threshold: 0.008 # [rad/s] + timer_callback_interval_sec: 0.5 # [sec] + diagnostics_updater_interval_sec: 0.5 # [sec] straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] From 6f95b4beec41b3c0d84f16cdb2c4484333c0d780 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Tue, 7 May 2024 15:54:45 +0900 Subject: [PATCH 33/50] fix: diag list Signed-off-by: 1222-takeshi --- .../sensor_kit.param.yaml | 57 ++++++++++++++++++- .../sensor_kit.param.yaml | 10 +++- 2 files changed, 61 insertions(+), 6 deletions(-) diff --git a/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml index 65942c94..ab515513 100644 --- a/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml +++ b/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -54,17 +54,68 @@ type: diagnostic_aggregator/GenericAnalyzer path: connection contains: [": pandar_connection"] - timeout: 1.0 + timeout: 5.0 temperature: type: diagnostic_aggregator/GenericAnalyzer path: temperature contains: [": pandar_temperature"] - timeout: 1.0 + timeout: 5.0 ptp: type: diagnostic_aggregator/GenericAnalyzer path: ptp contains: [": pandar_ptp"] - timeout: 1.0 + timeout: 5.0 + + gnss: + type: diagnostic_aggregator/AnalyzerGroup + path: gnss + analyzers: + health_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: health_monitoring + analyzers: + connection: + type: diagnostic_aggregator/GenericAnalyzer + path: connection + contains: [": gnss_connection"] + timeout: 3.0 + + data: + type: diagnostic_aggregator/GenericAnalyzer + path: data + contains: [": gnss_data"] + timeout: 3.0 + + antenna: + type: diagnostic_aggregator/GenericAnalyzer + path: antenna + contains: [": gnss_antenna"] + timeout: 3.0 + + tx_usage: + type: diagnostic_aggregator/GenericAnalyzer + path: tx_usage + contains: [": gnss_tx_usage"] + timeout: 3.0 + + spoofing: + type: diagnostic_aggregator/GenericAnalyzer + path: spoofing + contains: [": gnss_spoofing"] + timeout: 3.0 + + jamming: + type: diagnostic_aggregator/GenericAnalyzer + path: jamming + contains: [": gnss_jamming"] + timeout: 3.0 + + fix_topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: fix_topic_status + contains: [": fix topic status"] + timeout: 3.0 + imu: type: diagnostic_aggregator/AnalyzerGroup path: imu diff --git a/aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml index 8d4946b4..75712024 100644 --- a/aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml +++ b/aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -13,7 +13,13 @@ ros__parameters: required_diags: # gnss - gnss: default + gnss_connection: default + gnss_data: default + gnss_antenna: default + gnss_tx_usage: default + gnss_spoofing: default + gnss_jamming: default + fix topic status: default # pandar pandar_connection: default @@ -25,8 +31,6 @@ velodyne_temperature: default velodyne_rpm: default - concat_status: default - sensing_topic_status: default # imu From 85e6e96025976cb5bfbf61b91d6c7851cb0b04df Mon Sep 17 00:00:00 2001 From: asa-naki Date: Thu, 18 Jul 2024 17:59:31 +0900 Subject: [PATCH 34/50] feat: add input_offset parameter to pointcloud preprocessor launch Signed-off-by: asa-naki --- aip_x1_launch/launch/pointcloud_preprocessor.launch.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py index 085a45d9..c807986b 100644 --- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py @@ -40,7 +40,11 @@ def launch_setup(context, *args, **kwargs): "/sensing/lidar/front_center/pointcloud", ], "output_frame": LaunchConfiguration("base_frame"), - "timeout_sec": 1.0, + "input_offset": [ + 0.055, + 0.025, + ], + "timeout_sec": 0.095, "input_twist_topic_type": "twist", "publish_synchronized_pointcloud": True, } From 05028c019c53ca4564c51fa280da4258de6b0425 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Fri, 19 Jul 2024 14:09:19 +0900 Subject: [PATCH 35/50] feat: nebula_hw_monitor is not started except when simulation is running Signed-off-by: 1222-takeshi --- aip_x1_launch/launch/lidar.launch.xml | 1 + common_sensor_launch/launch/nebula_node_container.launch.py | 1 + 2 files changed, 2 insertions(+) diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml index dce0a58a..781c360d 100644 --- a/aip_x1_launch/launch/lidar.launch.xml +++ b/aip_x1_launch/launch/lidar.launch.xml @@ -41,6 +41,7 @@ + diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index e9660646..62717622 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -154,6 +154,7 @@ def create_parameter_dict(*args): ), }, ], + condition=IfCondition(LaunchConfiguration("launch_driver")), extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) From c4a3b2c594bda82eeef12ceac69c7741caeb0598 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Tue, 9 Jul 2024 10:40:13 +0900 Subject: [PATCH 36/50] feat(x2_gen1_nebula): changed LiDAR driver to nebula (#252) * change lidar driver to nebula from hesai Signed-off-by: kotaro-hihara * Set scan_phase 10 deg behind ideal FOVend to prevent dust at the edge of FOV (left,front) Signed-off-by: kotaro-hihara * ci(pre-commit): autofix --------- Signed-off-by: kotaro-hihara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- aip_x2_launch/launch/lidar.launch.xml | 325 +++++++++------ .../launch/nebula_node_container.launch.py | 380 ++++++++++++++++++ 2 files changed, 576 insertions(+), 129 deletions(-) create mode 100644 aip_x2_launch/launch/nebula_node_container.launch.py diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index 9ccf52af..c4297b05 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -4,207 +4,273 @@ + - + - - + + - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + - + + - - + + - - - - - - - - + + + + + + + + + + + - - - - + + + + + + + + + + - + - - + + - - - - - - - - - - - - - + + + + + + + + + + + - + + + + + + + + + + - + - - + + - - - - - - - - - - - + + + + + + + + + + + - + + + + + + + + + + - + - - + + - - - + + + - - - - - - - - - + + + + + + + - + + + + + + + + + + - + - - + + - - - + + + - - - - - - - + + + + + + + - + + + + + + + + + + - + - - + + - - - + + + - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - + - - + + - - - + + + - - - - - - - - - + + + + + + + + + + + + + + + + + + - + @@ -213,7 +279,8 @@ - + + diff --git a/aip_x2_launch/launch/nebula_node_container.launch.py b/aip_x2_launch/launch/nebula_node_container.launch.py new file mode 100644 index 00000000..53e7140e --- /dev/null +++ b/aip_x2_launch/launch/nebula_node_container.launch.py @@ -0,0 +1,380 @@ +# Copyright 2023 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# 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 +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +import yaml + + +def get_lidar_make(sensor_name): + if sensor_name[:6].lower() == "pandar": + return "Hesai", ".csv" + elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: + return "Velodyne", ".yaml" + return "unrecognized_sensor_model" + + +def get_vehicle_info(context): + # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support + # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py + gp = context.launch_configurations.get("ros_params", {}) + if not gp: + gp = dict(context.launch_configurations.get("global_params", {})) + p = {} + p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] + p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] + p["min_longitudinal_offset"] = -gp["rear_overhang"] + p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] + p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) + p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] + p["min_height_offset"] = 0.0 + p["max_height_offset"] = gp["vehicle_height"] + return p + + +def get_vehicle_mirror_info(context): + path = LaunchConfiguration("vehicle_mirror_param_file").perform(context) + with open(path, "r") as f: + p = yaml.safe_load(f)["/**"]["ros__parameters"] + return p + + +def launch_setup(context, *args, **kwargs): + def load_composable_node_param(param_path): + with open(LaunchConfiguration(param_path).perform(context), "r") as f: + return yaml.safe_load(f)["/**"]["ros__parameters"] + + def create_parameter_dict(*args): + result = {} + for x in args: + result[x] = LaunchConfiguration(x) + return result + + # Model and make + sensor_model = LaunchConfiguration("sensor_model").perform(context) + sensor_make, sensor_extension = get_lidar_make(sensor_model) + nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") + + # Calibration file + sensor_calib_fp = os.path.join( + nebula_decoders_share_dir, + "calibration", + sensor_make.lower(), + sensor_model + sensor_extension, + ) + assert os.path.exists( + sensor_calib_fp + ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) + + nodes = [] + + nodes.append( + ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + ) + + nodes.append( + ComposableNode( + package="nebula_ros", + plugin=sensor_make + "HwMonitorRosWrapper", + name=sensor_make.lower() + "_hardware_monitor_ros_wrapper_node", + parameters=[ + { + "sensor_model": LaunchConfiguration("sensor_model"), + "return_mode": LaunchConfiguration("return_mode"), + "frame_id": LaunchConfiguration("frame_id"), + "scan_phase": LaunchConfiguration("scan_phase"), + "sensor_ip": LaunchConfiguration("sensor_ip"), + "host_ip": LaunchConfiguration("host_ip"), + "data_port": LaunchConfiguration("data_port"), + "gnss_port": LaunchConfiguration("gnss_port"), + "packet_mtu_size": LaunchConfiguration("packet_mtu_size"), + "rotation_speed": LaunchConfiguration("rotation_speed"), + "cloud_min_angle": LaunchConfiguration("cloud_min_angle"), + "cloud_max_angle": LaunchConfiguration("cloud_max_angle"), + "diag_span": LaunchConfiguration("diag_span"), + "dual_return_distance_threshold": LaunchConfiguration( + "dual_return_distance_threshold" + ), + "delay_monitor_ms": LaunchConfiguration("delay_monitor_ms"), + }, + ], + condition=IfCondition(LaunchConfiguration("launch_driver")), + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + nodes.append( + ComposableNode( + package="nebula_ros", + plugin=sensor_make + "DriverRosWrapper", + name=sensor_make.lower() + "_driver_ros_wrapper_node", + parameters=[ + { + "calibration_file": sensor_calib_fp, + "sensor_model": sensor_model, + **create_parameter_dict( + "host_ip", + "sensor_ip", + "data_port", + "return_mode", + "min_range", + "max_range", + "frame_id", + "scan_phase", + "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=[ + ("aw_points", "pointcloud_raw"), + ("aw_points_ex", "pointcloud_raw_ex"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") + cropbox_parameters["negative"] = True + + vehicle_info = get_vehicle_info(context) + cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] + cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] + cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] + cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] + cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] + cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] + + nodes.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_self", + remappings=[ + ("input", "pointcloud_raw_ex"), + # ("output", "self_cropped_temp/pointcloud_ex"), + ("output", "self_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + mirror_info = load_composable_node_param("vehicle_mirror_param_file") + right = mirror_info["right"] + cropbox_parameters.update( + min_x=right["min_longitudinal_offset"], + max_x=right["max_longitudinal_offset"], + min_y=right["min_lateral_offset"], + max_y=right["max_lateral_offset"], + min_z=right["min_height_offset"], + max_z=right["max_height_offset"], + ) + + nodes.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_mirror_right", + remappings=[ + ("input", "self_cropped/pointcloud_ex"), + ("output", "right_mirror_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + left = mirror_info["left"] + cropbox_parameters.update( + min_x=left["min_longitudinal_offset"], + max_x=left["max_longitudinal_offset"], + min_y=left["min_lateral_offset"], + max_y=left["max_lateral_offset"], + min_z=left["min_height_offset"], + max_z=left["max_height_offset"], + ) + + nodes.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_mirror_left", + remappings=[ + ("input", "right_mirror_cropped/pointcloud_ex"), + ("output", "mirror_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + nodes.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="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")}], + ) + ) + + nodes.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::RingOutlierFilterComponent", + name="ring_outlier_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud_before_sync"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + # set container to run all required components in the same process + 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")), + ) + + return [container, driver_component_loader] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + # a default_value of None is equivalent to not passing that kwarg at all + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + add_launch_arg("sensor_model", description="sensor model name") + add_launch_arg("config_file", "", description="sensor configuration file") + add_launch_arg("launch_driver", "True", "do launch driver") + add_launch_arg("setup_sensor", "False", "configure sensor") + add_launch_arg("sensor_ip", "192.168.1.201", "device ip address") + add_launch_arg("host_ip", "255.255.255.255", "host ip address") + add_launch_arg("scan_phase", "0.0") + add_launch_arg("base_frame", "base_link", "base frame id") + add_launch_arg("min_range", "0.3", "minimum view range for Velodyne sensors") + add_launch_arg("max_range", "300.0", "maximum view range for Velodyne sensors") + add_launch_arg("cloud_min_angle", "0", "minimum view angle setting on device") + add_launch_arg("cloud_max_angle", "360", "maximum view angle setting on device") + add_launch_arg("data_port", "2368", "device data port number") + add_launch_arg("gnss_port", "2380", "device gnss port number") + add_launch_arg("packet_mtu_size", "1500", "packet mtu size") + add_launch_arg("rotation_speed", "600", "rotational frequency") + add_launch_arg("dual_return_distance_threshold", "0.1", "dual return distance threshold") + add_launch_arg("frame_id", "lidar", "frame id") + 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( + "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" + ) + add_launch_arg("diag_span", "1000") + add_launch_arg("delay_monitor_ms", "2000") + add_launch_arg("use_multithread", "False", "use multithread") + add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") + add_launch_arg("container_name", "nebula_node_container") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) From 4d93b170444dad6c94a50c2e0b605af65ad16dd9 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Wed, 10 Jul 2024 20:54:58 +0900 Subject: [PATCH 37/50] fix(aip_x2_launch): read callibration from individual_params (#255) * read callibration from individual_params Signed-off-by: kotaro-hihara * ci(pre-commit): autofix --------- Signed-off-by: kotaro-hihara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- aip_x2_launch/launch/lidar.launch.xml | 16 ++++++++-------- .../launch/nebula_node_container.launch.py | 17 +++-------------- 2 files changed, 11 insertions(+), 22 deletions(-) diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index c4297b05..4dc5dc87 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -37,7 +37,7 @@ - + @@ -70,7 +70,7 @@ - + @@ -103,7 +103,7 @@ - + @@ -136,7 +136,7 @@ - + @@ -169,7 +169,7 @@ - + @@ -202,7 +202,7 @@ - + @@ -235,7 +235,7 @@ - + @@ -268,7 +268,7 @@ - + diff --git a/aip_x2_launch/launch/nebula_node_container.launch.py b/aip_x2_launch/launch/nebula_node_container.launch.py index 53e7140e..08eebe40 100644 --- a/aip_x2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_launch/launch/nebula_node_container.launch.py @@ -75,18 +75,6 @@ def create_parameter_dict(*args): # Model and make sensor_model = LaunchConfiguration("sensor_model").perform(context) sensor_make, sensor_extension = get_lidar_make(sensor_model) - nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") - - # Calibration file - sensor_calib_fp = os.path.join( - nebula_decoders_share_dir, - "calibration", - sensor_make.lower(), - sensor_model + sensor_extension, - ) - assert os.path.exists( - sensor_calib_fp - ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) nodes = [] @@ -136,7 +124,6 @@ def create_parameter_dict(*args): name=sensor_make.lower() + "_driver_ros_wrapper_node", parameters=[ { - "calibration_file": sensor_calib_fp, "sensor_model": sensor_model, **create_parameter_dict( "host_ip", @@ -157,6 +144,7 @@ def create_parameter_dict(*args): "ptp_transport_type", "ptp_switch_type", "ptp_domain", + "calibration_file", ), "launch_hw": True, }, @@ -290,7 +278,6 @@ def create_parameter_dict(*args): parameters=[ { "sensor_model": sensor_model, - "calibration_file": sensor_calib_fp, **create_parameter_dict( "sensor_ip", "host_ip", @@ -309,6 +296,7 @@ def create_parameter_dict(*args): "ptp_transport_type", "ptp_switch_type", "ptp_domain", + "calibration_file", ), } ], @@ -360,6 +348,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("use_multithread", "False", "use multithread") add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") add_launch_arg("container_name", "nebula_node_container") + add_launch_arg("calibration_file", "") set_container_executable = SetLaunchConfiguration( "container_executable", From fe2e70741d164f6d32749c3d44ce9798ce6c729a Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Thu, 11 Jul 2024 16:40:33 +0900 Subject: [PATCH 38/50] fix(aip_x2_launch): set use_multithread and use_intra_process to True (#256) set use_multithread and use_intra_process to True Signed-off-by: kotaro-hihara --- aip_x2_launch/launch/nebula_node_container.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aip_x2_launch/launch/nebula_node_container.launch.py b/aip_x2_launch/launch/nebula_node_container.launch.py index 08eebe40..7fb66c08 100644 --- a/aip_x2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_launch/launch/nebula_node_container.launch.py @@ -345,8 +345,8 @@ def add_launch_arg(name: str, default_value=None, description=None): ) add_launch_arg("diag_span", "1000") add_launch_arg("delay_monitor_ms", "2000") - add_launch_arg("use_multithread", "False", "use multithread") - add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") + add_launch_arg("use_multithread", "True", "use multithread") + add_launch_arg("use_intra_process", "True", "use ROS 2 component container communication") add_launch_arg("container_name", "nebula_node_container") add_launch_arg("calibration_file", "") From a53113b84079206cdf9ec19bbef17bcf1dd02cb4 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Fri, 12 Jul 2024 15:14:43 +0900 Subject: [PATCH 39/50] fix(aip_x2_launch): update lidar angle setting for j6_gen1_02 (#254) update lidar angle setting for j6_gen1_02 --- aip_x2_launch/launch/lidar.launch.xml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index 4dc5dc87..7d761f44 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -52,9 +52,9 @@ - - - + + + @@ -250,9 +250,9 @@ - - - + + + From e3a55b9d6cadc8c9f332c3595bfd3fdc0c2464a1 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Tue, 16 Jul 2024 19:25:57 +0900 Subject: [PATCH 40/50] update dummy diag name --- aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml index 02ec3b1a..888cd52c 100644 --- a/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml +++ b/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -21,7 +21,7 @@ pandar_ptp: default "left_upper: visibility_validation": default blockage_validation: default - concat_status: default + "concatenate_data: concat_status": default sensing_topic_status: default From 143eebcf9873701631210b6fbd8ffbf227e97bd1 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Wed, 24 Jul 2024 20:49:47 +0900 Subject: [PATCH 41/50] chore(blockage_diag): update param for aip_x1_launch (#268) * add enable_blockage_diag argument to lidar.launch.xml Signed-off-by: Autumn60 * ci(pre-commit): autofix --------- Signed-off-by: Autumn60 Co-authored-by: Autumn60 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- aip_x1_launch/launch/lidar.launch.xml | 2 ++ aip_x1_launch/launch/pointcloud_preprocessor.launch.py | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml index 781c360d..f1796100 100644 --- a/aip_x1_launch/launch/lidar.launch.xml +++ b/aip_x1_launch/launch/lidar.launch.xml @@ -7,6 +7,7 @@ + @@ -22,6 +23,7 @@ + diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py index c807986b..7afbb68d 100644 --- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py @@ -43,7 +43,7 @@ def launch_setup(context, *args, **kwargs): "input_offset": [ 0.055, 0.025, - ], + ], "timeout_sec": 0.095, "input_twist_topic_type": "twist", "publish_synchronized_pointcloud": True, From c19b2d42114da7206b21b2c9d918a5eb01f96d5b Mon Sep 17 00:00:00 2001 From: Sho Iwasawa <41606073+zusizusi@users.noreply.github.com> Date: Wed, 7 Aug 2024 14:35:21 +0300 Subject: [PATCH 42/50] revert(common_sensor_launch): revert PR of changing vlp16 param (#251) (#253) (#281) Revert "fix(common_sensor_launch): change vlp16 param (#251) (#253)" This reverts commit e536068be9e9a1657662152c84fc00de18558cb7. Signed-off-by: Sho Iwasawa --- common_sensor_launch/launch/velodyne_VLP16.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common_sensor_launch/launch/velodyne_VLP16.launch.xml b/common_sensor_launch/launch/velodyne_VLP16.launch.xml index 4a6cd6c1..a105b0a2 100644 --- a/common_sensor_launch/launch/velodyne_VLP16.launch.xml +++ b/common_sensor_launch/launch/velodyne_VLP16.launch.xml @@ -37,7 +37,7 @@ - + From 3a9adac5ae9bf4a671cd7c23ac23500eb69fc9df Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Tue, 13 Aug 2024 17:40:23 +0900 Subject: [PATCH 43/50] fix: change container name Signed-off-by: 1222-takeshi --- aip_x1_launch/launch/pointcloud_preprocessor.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py index 7afbb68d..e8d2e7da 100644 --- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py @@ -71,7 +71,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("base_frame", "base_link") add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False") - add_launch_arg("container_name", "pointcloud_preprocessor_container") + add_launch_arg("pointcloud_container_name", "pointcloud_container") set_container_executable = SetLaunchConfiguration( "container_executable", From b48f5f9d7c3fc34c913060862d3b0d519892413b Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Thu, 29 Aug 2024 10:03:06 +0900 Subject: [PATCH 44/50] fix: config for awsim (#289) * delete unnecessary arg passing Signed-off-by: Autumn60 * fix gnss_poser param Signed-off-by: Autumn60 * Adjust gnss launching condition Signed-off-by: Autumn60 * fix invalid tag usage Signed-off-by: Autumn60 * ci(pre-commit): autofix --------- Signed-off-by: Autumn60 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- aip_x1_launch/config/gnss_poser.param.yaml | 8 ++++++++ aip_x1_launch/launch/gnss.launch.xml | 12 +++++------- aip_x1_launch/launch/sensing.launch.xml | 3 +-- 3 files changed, 14 insertions(+), 9 deletions(-) create mode 100644 aip_x1_launch/config/gnss_poser.param.yaml diff --git a/aip_x1_launch/config/gnss_poser.param.yaml b/aip_x1_launch/config/gnss_poser.param.yaml new file mode 100644 index 00000000..5d4e8fda --- /dev/null +++ b/aip_x1_launch/config/gnss_poser.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + base_frame: base_link + gnss_base_frame: gnss_link + map_frame: map + buff_epoch: 1 + use_gnss_ins_orientation: false + gnss_pose_pub_method: 0 # 0: Median, 1: Average diff --git a/aip_x1_launch/launch/gnss.launch.xml b/aip_x1_launch/launch/gnss.launch.xml index c99d0afb..27483b46 100644 --- a/aip_x1_launch/launch/gnss.launch.xml +++ b/aip_x1_launch/launch/gnss.launch.xml @@ -1,10 +1,9 @@ - - - + + @@ -14,14 +13,13 @@ + - - + + - - diff --git a/aip_x1_launch/launch/sensing.launch.xml b/aip_x1_launch/launch/sensing.launch.xml index 0f23a733..775a8566 100644 --- a/aip_x1_launch/launch/sensing.launch.xml +++ b/aip_x1_launch/launch/sensing.launch.xml @@ -12,7 +12,6 @@ - @@ -21,7 +20,7 @@ - + From b7959f539a224cf60198bd2c7cc6dda42eac4d0c Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Thu, 29 Aug 2024 10:41:55 +0900 Subject: [PATCH 45/50] chore: replace use_awsim to use_gnss (#292) replace use_awsim to use_gnss Signed-off-by: Autumn60 --- aip_x1_launch/launch/sensing.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aip_x1_launch/launch/sensing.launch.xml b/aip_x1_launch/launch/sensing.launch.xml index 775a8566..50d56354 100644 --- a/aip_x1_launch/launch/sensing.launch.xml +++ b/aip_x1_launch/launch/sensing.launch.xml @@ -4,7 +4,7 @@ - + @@ -20,7 +20,7 @@ - + From 2cbfd49f16a4e386078a6e8df36f55fe369c5f14 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Tue, 17 Sep 2024 15:27:37 +0900 Subject: [PATCH 46/50] Revert "update dummy diag name" This reverts commit e3a55b9d6cadc8c9f332c3595bfd3fdc0c2464a1. --- aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml index 888cd52c..02ec3b1a 100644 --- a/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml +++ b/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -21,7 +21,7 @@ pandar_ptp: default "left_upper: visibility_validation": default blockage_validation: default - "concatenate_data: concat_status": default + concat_status: default sensing_topic_status: default From 4f93b230c7b829b8cb1d7fe821576e49ead661b3 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 26 Sep 2024 10:44:16 +0900 Subject: [PATCH 47/50] Revert "fix(aip_x2_launch): update lidar angle setting for j6_gen1_02 (#254)" This reverts commit a53113b84079206cdf9ec19bbef17bcf1dd02cb4. --- aip_x2_launch/launch/lidar.launch.xml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index 7d761f44..4dc5dc87 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -52,9 +52,9 @@ - - - + + + @@ -250,9 +250,9 @@ - - - + + + From 9a9a39bca37b4505ec434afdcdb2cf37cc84be2f Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 26 Sep 2024 10:44:23 +0900 Subject: [PATCH 48/50] Revert "fix(aip_x2_launch): set use_multithread and use_intra_process to True (#256)" This reverts commit fe2e70741d164f6d32749c3d44ce9798ce6c729a. --- aip_x2_launch/launch/nebula_node_container.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aip_x2_launch/launch/nebula_node_container.launch.py b/aip_x2_launch/launch/nebula_node_container.launch.py index 7fb66c08..08eebe40 100644 --- a/aip_x2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_launch/launch/nebula_node_container.launch.py @@ -345,8 +345,8 @@ def add_launch_arg(name: str, default_value=None, description=None): ) add_launch_arg("diag_span", "1000") add_launch_arg("delay_monitor_ms", "2000") - add_launch_arg("use_multithread", "True", "use multithread") - add_launch_arg("use_intra_process", "True", "use ROS 2 component container communication") + add_launch_arg("use_multithread", "False", "use multithread") + add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") add_launch_arg("container_name", "nebula_node_container") add_launch_arg("calibration_file", "") From a570fd85279fee35262210d603e5e4fa26d2b6c4 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 26 Sep 2024 10:44:28 +0900 Subject: [PATCH 49/50] Revert "fix(aip_x2_launch): read callibration from individual_params (#255)" This reverts commit 4d93b170444dad6c94a50c2e0b605af65ad16dd9. --- aip_x2_launch/launch/lidar.launch.xml | 16 ++++++++-------- .../launch/nebula_node_container.launch.py | 17 ++++++++++++++--- 2 files changed, 22 insertions(+), 11 deletions(-) diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index 4dc5dc87..c4297b05 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -37,7 +37,7 @@ - + @@ -70,7 +70,7 @@ - + @@ -103,7 +103,7 @@ - + @@ -136,7 +136,7 @@ - + @@ -169,7 +169,7 @@ - + @@ -202,7 +202,7 @@ - + @@ -235,7 +235,7 @@ - + @@ -268,7 +268,7 @@ - + diff --git a/aip_x2_launch/launch/nebula_node_container.launch.py b/aip_x2_launch/launch/nebula_node_container.launch.py index 08eebe40..53e7140e 100644 --- a/aip_x2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_launch/launch/nebula_node_container.launch.py @@ -75,6 +75,18 @@ def create_parameter_dict(*args): # Model and make sensor_model = LaunchConfiguration("sensor_model").perform(context) sensor_make, sensor_extension = get_lidar_make(sensor_model) + nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") + + # Calibration file + sensor_calib_fp = os.path.join( + nebula_decoders_share_dir, + "calibration", + sensor_make.lower(), + sensor_model + sensor_extension, + ) + assert os.path.exists( + sensor_calib_fp + ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) nodes = [] @@ -124,6 +136,7 @@ def create_parameter_dict(*args): name=sensor_make.lower() + "_driver_ros_wrapper_node", parameters=[ { + "calibration_file": sensor_calib_fp, "sensor_model": sensor_model, **create_parameter_dict( "host_ip", @@ -144,7 +157,6 @@ def create_parameter_dict(*args): "ptp_transport_type", "ptp_switch_type", "ptp_domain", - "calibration_file", ), "launch_hw": True, }, @@ -278,6 +290,7 @@ def create_parameter_dict(*args): parameters=[ { "sensor_model": sensor_model, + "calibration_file": sensor_calib_fp, **create_parameter_dict( "sensor_ip", "host_ip", @@ -296,7 +309,6 @@ def create_parameter_dict(*args): "ptp_transport_type", "ptp_switch_type", "ptp_domain", - "calibration_file", ), } ], @@ -348,7 +360,6 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("use_multithread", "False", "use multithread") add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") add_launch_arg("container_name", "nebula_node_container") - add_launch_arg("calibration_file", "") set_container_executable = SetLaunchConfiguration( "container_executable", From 6efc127ed7f23a169ffa662259634f1da18cb5dd Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 26 Sep 2024 10:44:33 +0900 Subject: [PATCH 50/50] Revert "feat(x2_gen1_nebula): changed LiDAR driver to nebula (#252)" This reverts commit c4a3b2c594bda82eeef12ceac69c7741caeb0598. --- aip_x2_launch/launch/lidar.launch.xml | 325 ++++++--------- .../launch/nebula_node_container.launch.py | 380 ------------------ 2 files changed, 129 insertions(+), 576 deletions(-) delete mode 100644 aip_x2_launch/launch/nebula_node_container.launch.py diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index c4297b05..9ccf52af 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -4,273 +4,207 @@ - - + - - + + - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + - + - - - + + - - - - - - - - - - - + + + + + + + + - - - - - - - - - - + + + + - + - - + + - - - - - - - - - - - + + + + + + + + + + + + + - - - - - - - - - - + - + - - + + - - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - - + - + - - + + - - - + + + - - - - - - - + + + + + + + + + - - - - - - - - - - + - + - - + + - - - + + + - - - - - - - + + + + + + + - - - - - - - - - - + - + - - + + - - - + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + - + - - + + - - - + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + - + @@ -279,8 +213,7 @@ - - + diff --git a/aip_x2_launch/launch/nebula_node_container.launch.py b/aip_x2_launch/launch/nebula_node_container.launch.py deleted file mode 100644 index 53e7140e..00000000 --- a/aip_x2_launch/launch/nebula_node_container.launch.py +++ /dev/null @@ -1,380 +0,0 @@ -# Copyright 2023 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# 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 -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes -from launch_ros.descriptions import ComposableNode -import yaml - - -def get_lidar_make(sensor_name): - if sensor_name[:6].lower() == "pandar": - return "Hesai", ".csv" - elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: - return "Velodyne", ".yaml" - return "unrecognized_sensor_model" - - -def get_vehicle_info(context): - # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support - # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py - gp = context.launch_configurations.get("ros_params", {}) - if not gp: - gp = dict(context.launch_configurations.get("global_params", {})) - p = {} - p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] - p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] - p["min_longitudinal_offset"] = -gp["rear_overhang"] - p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] - p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) - p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] - p["min_height_offset"] = 0.0 - p["max_height_offset"] = gp["vehicle_height"] - return p - - -def get_vehicle_mirror_info(context): - path = LaunchConfiguration("vehicle_mirror_param_file").perform(context) - with open(path, "r") as f: - p = yaml.safe_load(f)["/**"]["ros__parameters"] - return p - - -def launch_setup(context, *args, **kwargs): - def load_composable_node_param(param_path): - with open(LaunchConfiguration(param_path).perform(context), "r") as f: - return yaml.safe_load(f)["/**"]["ros__parameters"] - - def create_parameter_dict(*args): - result = {} - for x in args: - result[x] = LaunchConfiguration(x) - return result - - # Model and make - sensor_model = LaunchConfiguration("sensor_model").perform(context) - sensor_make, sensor_extension = get_lidar_make(sensor_model) - nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") - - # Calibration file - sensor_calib_fp = os.path.join( - nebula_decoders_share_dir, - "calibration", - sensor_make.lower(), - sensor_model + sensor_extension, - ) - assert os.path.exists( - sensor_calib_fp - ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) - - nodes = [] - - nodes.append( - ComposableNode( - package="glog_component", - plugin="GlogComponent", - name="glog_component", - ) - ) - - nodes.append( - ComposableNode( - package="nebula_ros", - plugin=sensor_make + "HwMonitorRosWrapper", - name=sensor_make.lower() + "_hardware_monitor_ros_wrapper_node", - parameters=[ - { - "sensor_model": LaunchConfiguration("sensor_model"), - "return_mode": LaunchConfiguration("return_mode"), - "frame_id": LaunchConfiguration("frame_id"), - "scan_phase": LaunchConfiguration("scan_phase"), - "sensor_ip": LaunchConfiguration("sensor_ip"), - "host_ip": LaunchConfiguration("host_ip"), - "data_port": LaunchConfiguration("data_port"), - "gnss_port": LaunchConfiguration("gnss_port"), - "packet_mtu_size": LaunchConfiguration("packet_mtu_size"), - "rotation_speed": LaunchConfiguration("rotation_speed"), - "cloud_min_angle": LaunchConfiguration("cloud_min_angle"), - "cloud_max_angle": LaunchConfiguration("cloud_max_angle"), - "diag_span": LaunchConfiguration("diag_span"), - "dual_return_distance_threshold": LaunchConfiguration( - "dual_return_distance_threshold" - ), - "delay_monitor_ms": LaunchConfiguration("delay_monitor_ms"), - }, - ], - condition=IfCondition(LaunchConfiguration("launch_driver")), - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - nodes.append( - ComposableNode( - package="nebula_ros", - plugin=sensor_make + "DriverRosWrapper", - name=sensor_make.lower() + "_driver_ros_wrapper_node", - parameters=[ - { - "calibration_file": sensor_calib_fp, - "sensor_model": sensor_model, - **create_parameter_dict( - "host_ip", - "sensor_ip", - "data_port", - "return_mode", - "min_range", - "max_range", - "frame_id", - "scan_phase", - "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=[ - ("aw_points", "pointcloud_raw"), - ("aw_points_ex", "pointcloud_raw_ex"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - cropbox_parameters = create_parameter_dict("input_frame", "output_frame") - cropbox_parameters["negative"] = True - - vehicle_info = get_vehicle_info(context) - cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] - cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] - cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] - cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] - cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] - cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] - - nodes.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_self", - remappings=[ - ("input", "pointcloud_raw_ex"), - # ("output", "self_cropped_temp/pointcloud_ex"), - ("output", "self_cropped/pointcloud_ex"), - ], - parameters=[cropbox_parameters], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - mirror_info = load_composable_node_param("vehicle_mirror_param_file") - right = mirror_info["right"] - cropbox_parameters.update( - min_x=right["min_longitudinal_offset"], - max_x=right["max_longitudinal_offset"], - min_y=right["min_lateral_offset"], - max_y=right["max_lateral_offset"], - min_z=right["min_height_offset"], - max_z=right["max_height_offset"], - ) - - nodes.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_mirror_right", - remappings=[ - ("input", "self_cropped/pointcloud_ex"), - ("output", "right_mirror_cropped/pointcloud_ex"), - ], - parameters=[cropbox_parameters], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - left = mirror_info["left"] - cropbox_parameters.update( - min_x=left["min_longitudinal_offset"], - max_x=left["max_longitudinal_offset"], - min_y=left["min_lateral_offset"], - max_y=left["max_lateral_offset"], - min_z=left["min_height_offset"], - max_z=left["max_height_offset"], - ) - - nodes.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_mirror_left", - remappings=[ - ("input", "right_mirror_cropped/pointcloud_ex"), - ("output", "mirror_cropped/pointcloud_ex"), - ], - parameters=[cropbox_parameters], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - nodes.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="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")}], - ) - ) - - nodes.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::RingOutlierFilterComponent", - name="ring_outlier_filter", - remappings=[ - ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud_before_sync"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - # set container to run all required components in the same process - 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")), - ) - - return [container, driver_component_loader] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - # a default_value of None is equivalent to not passing that kwarg at all - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - add_launch_arg("sensor_model", description="sensor model name") - add_launch_arg("config_file", "", description="sensor configuration file") - add_launch_arg("launch_driver", "True", "do launch driver") - add_launch_arg("setup_sensor", "False", "configure sensor") - add_launch_arg("sensor_ip", "192.168.1.201", "device ip address") - add_launch_arg("host_ip", "255.255.255.255", "host ip address") - add_launch_arg("scan_phase", "0.0") - add_launch_arg("base_frame", "base_link", "base frame id") - add_launch_arg("min_range", "0.3", "minimum view range for Velodyne sensors") - add_launch_arg("max_range", "300.0", "maximum view range for Velodyne sensors") - add_launch_arg("cloud_min_angle", "0", "minimum view angle setting on device") - add_launch_arg("cloud_max_angle", "360", "maximum view angle setting on device") - add_launch_arg("data_port", "2368", "device data port number") - add_launch_arg("gnss_port", "2380", "device gnss port number") - add_launch_arg("packet_mtu_size", "1500", "packet mtu size") - add_launch_arg("rotation_speed", "600", "rotational frequency") - add_launch_arg("dual_return_distance_threshold", "0.1", "dual return distance threshold") - add_launch_arg("frame_id", "lidar", "frame id") - 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( - "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" - ) - add_launch_arg("diag_span", "1000") - add_launch_arg("delay_monitor_ms", "2000") - add_launch_arg("use_multithread", "False", "use multithread") - add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") - add_launch_arg("container_name", "nebula_node_container") - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - - return launch.LaunchDescription( - launch_arguments - + [set_container_executable, set_container_mt_executable] - + [OpaqueFunction(function=launch_setup)] - )