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 bf00b7ca..2008eda8 100644 --- a/aip_x1_launch/launch/imu.launch.xml +++ b/aip_x1_launch/launch/imu.launch.xml @@ -1,25 +1,37 @@ - - - - - - + + + + + + + + + + + + + + + + + + + + - + - - 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 e09d7350..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="pointcloud_preprocessor", - plugin="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("base_frame"), - "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 07dfb5a0..dd2a2c17 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"), @@ -53,6 +51,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] @@ -67,7 +66,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 9add7047..a660f023 100644 --- a/aip_x1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_launch/launch/velodyne_node_container.launch.py @@ -104,6 +104,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", @@ -119,20 +133,6 @@ 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", @@ -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="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 8872a716..f20fb08e 100644 --- a/aip_x1_launch/package.xml +++ b/aip_x1_launch/package.xml @@ -1,24 +1,27 @@ - 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 + common_sensor_launch compare_map_segmentation - gnss_poser + elevation_map_loader + ground_segmentation imu_corrector + individual_params + occupancy_grid_map_outlier_filter pointcloud_preprocessor + rclcpp_components + ros2_socketcan tamagawa_imu_driver - topic_tools - ublox_gps - usb_cam - vehicle_velocity_converter + topic_state_monitor ament_lint_auto autoware_lint_common