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..70133656 --- /dev/null +++ b/aip_x1_1_description/config/sensor_kit_calibration.yaml @@ -0,0 +1,29 @@ +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 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..7629d304 --- /dev/null +++ b/aip_x1_1_description/urdf/sensor_kit.xacro @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..e32c0fb8 --- /dev/null +++ b/aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -0,0 +1,80 @@ +/**: + 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/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..069f4302 --- /dev/null +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -0,0 +1,74 @@ +/**: + ros__parameters: + additional_lidars: [] + ransac_input_topics: [ + "/sensing/lidar/front_center/pointcloud" + ] + use_single_frame_filter: True + use_time_series_filter: False + + 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 + negative: False + + common_ground_filter: + plugin: "ground_segmentation::ScanGroundFilterComponent" + parameters: + 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 + 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" + # parameters: + # 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 + # initial_max_slope: 10.0 + # radial_divider_angle: 1.0 + # min_height_threshold: 0.3 + # concentric_divider_distance: 0.0 + # reclass_distance_threshold: 0.1 + + short_height_obstacle_detection_area_filter: + parameters: + min_x: 0.0 + 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 + max_z: 0.5 + negative: False + + ransac_ground_filter: + parameters: +# base_frame: base_link + unit_axis: "z" + max_iterations: 50 + 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/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/ground_segmentation/ground_segmentation.launch.py b/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py new file mode 100644 index 00000000..920a221a --- /dev/null +++ b/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py @@ -0,0 +1,535 @@ +# 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", {}) + 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"] + 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}/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..2008eda8 --- /dev/null +++ b/aip_x1_1_launch/launch/imu.launch.xml @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..b2457f76 --- /dev/null +++ b/aip_x1_1_launch/launch/lidar.launch.xml @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..fc102338 --- /dev/null +++ b/aip_x1_1_launch/launch/pandar_node_container.launch.py @@ -0,0 +1,231 @@ +# 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_1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py new file mode 100644 index 00000000..dd2a2c17 --- /dev/null +++ b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py @@ -0,0 +1,87 @@ +# 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 new file mode 100644 index 00000000..1419d429 --- /dev/null +++ b/aip_x1_1_launch/launch/sensing.launch.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..3bb7d2b7 --- /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/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 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..a660f023 --- /dev/null +++ b/aip_x1_1_launch/launch/velodyne_node_container.launch.py @@ -0,0 +1,250 @@ +# 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 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 + + 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 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/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml index dafe8bbc..24b3a6e6 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 @@ -34,9 +43,9 @@ contains: [": velodyne_rpm"] timeout: 3.0 - livox: + front: type: diagnostic_aggregator/AnalyzerGroup - path: livox + path: front analyzers: health_monitoring: type: diagnostic_aggregator/AnalyzerGroup @@ -101,7 +110,6 @@ # path: time_sync # contains: [": livox_time_sync"] # timeout: 3.0 - imu: type: diagnostic_aggregator/AnalyzerGroup path: imu 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/new_livox_horizon.launch.py b/aip_x1_launch/launch/new_livox_horizon.launch.py index de3f8b9a..e09d7350 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"), + ("output", "pointcloud"), ], parameters=[ { diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py index ff012c8a..07dfb5a0 100644 --- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py @@ -37,9 +37,9 @@ def launch_setup(context, *args, **kwargs): { "input_topics": [ "/sensing/lidar/top/pointcloud", - "/sensing/lidar/front_left/min_range_cropped/pointcloud", - "/sensing/lidar/front_right/min_range_cropped/pointcloud", - "/sensing/lidar/front_center/min_range_cropped/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/sensing.launch.xml b/aip_x1_launch/launch/sensing.launch.xml index 1c294fdf..b8b04a0d 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,10 +26,11 @@ + - + diff --git a/aip_x1_launch/launch/topic_state_monitor.launch.py b/aip_x1_launch/launch/topic_state_monitor.launch.py index b45b0f4e..8bb1bf68 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 56b35116..9add7047 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", {})) @@ -121,6 +119,20 @@ 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", @@ -137,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="pointcloud_container", namespace="pointcloud_preprocessor", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), diff --git a/common_sensor_launch/package.xml b/common_sensor_launch/package.xml index c4b612ff..020add57 100644 --- a/common_sensor_launch/package.xml +++ b/common_sensor_launch/package.xml @@ -11,10 +11,10 @@ ament_cmake_auto dummy_diag_publisher - nebula_sensor_driver radar_tracks_msgs_converter radar_tracks_noise_filter velodyne_monitor + ament_lint_auto autoware_lint_common