fix(aip_x1/aip_x1_1): add node_alive_monitoring

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
+)

aip_x1_1_description/config/sensor_kit_calibration.yaml

@@ -0,0 +1,22 @@
+sensor_kit_base_link:
+  velodyne_top_base_link:
+    x: 0.000
+    y: 0.000
+    z: 0.000
+    roll: 0.000
+    pitch: 0.000
+    yaw: 0.000
+  pandar_xt32_front_center_base_link:
+    x: 1.130
+    y: 0.038
+    z: -1.400
+    roll: -0.000
+    pitch: -0.00
+    yaw: 1.6225
+  tamagawa/imu_link:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    roll: 3.14159
+    pitch: 0.0
+    yaw: 0.0

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

aip_x1_1_description/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>aip_x1_1_description</name>
+  <version>0.1.0</version>
+  <description>The aip_x1_1_description package</description>
+
+  <maintainer email="[email protected]">Yohei Mishina</maintainer>
+  <license>Apache 2</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>pandar_description</depend>
+  <depend>velodyne_description</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

aip_x1_1_description/urdf/sensor_kit.xacro

@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="sensor_kit_macro" params="parent x y z roll pitch yaw">
+    <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
+
+    <xacro:arg name="gpu" default="false"/>
+    <xacro:arg name="config_dir" default="$(find aip_x1_1_description)/config"/>
+    <xacro:property name="sensor_kit_base_link" default="sensor_kit_base_link"/>
+
+    <joint name="${sensor_kit_base_link}_joint" type="fixed">
+      <origin rpy="${roll} ${pitch} ${yaw}" xyz="${x} ${y} ${z}"/>
+      <parent link="${parent}"/>
+      <child link="${sensor_kit_base_link}"/>
+    </joint>
+    <link name="${sensor_kit_base_link}">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </link>
+
+    <!-- sensor -->
+    <xacro:property name="calibration" value="${load_yaml('$(arg config_dir)/sensor_kit_calibration.yaml')}"/>
+    <!-- lidar -->
+    <xacro:VLP-16
+      parent="sensor_kit_base_link"
+      name="velodyne_top"
+      topic="/points_raw"
+      hz="10"
+      samples="220"
+      gpu="$(arg gpu)"
+    >
+      <origin
+        xyz="${calibration['sensor_kit_base_link']['velodyne_top_base_link']['x']}
+             ${calibration['sensor_kit_base_link']['velodyne_top_base_link']['y']}
+             ${calibration['sensor_kit_base_link']['velodyne_top_base_link']['z']}"
+        rpy="${calibration['sensor_kit_base_link']['velodyne_top_base_link']['roll']}
+             ${calibration['sensor_kit_base_link']['velodyne_top_base_link']['pitch']}
+            ${calibration['sensor_kit_base_link']['velodyne_top_base_link']['yaw']}"
+      />
+    </xacro:VLP-16>
+
+    <xacro:include filename="$(find pandar_description)/urdf/pandar_xt32.xacro"/>
+    <xacro:PandarXT-32
+      name="pandar_xt32_front_center"
+      parent="sensor_kit_base_link"
+      x="${calibration['sensor_kit_base_link']['pandar_xt32_front_center_base_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['pandar_xt32_front_center_base_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['pandar_xt32_front_center_base_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['pandar_xt32_front_center_base_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['pandar_xt32_front_center_base_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['pandar_xt32_front_center_base_link']['yaw']}"
+    />
+
+    <xacro:include filename="$(find imu_description)/urdf/imu.xacro"/>
+    <xacro:imu_macro
+      name="tamagawa/imu"
+      parent="sensor_kit_base_link"
+      namespace=""
+      x="${calibration['sensor_kit_base_link']['tamagawa/imu_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['tamagawa/imu_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['tamagawa/imu_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['tamagawa/imu_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['tamagawa/imu_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['tamagawa/imu_link']['yaw']}"
+      fps="100"
+    />
+  </xacro:macro>
+</robot>

aip_x1_1_description/urdf/sensors.xacro

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<robot name="vehicle" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:arg name="config_dir" default="$(find aip_x1_1_description)/config"/>
+  <xacro:include filename="$(find aip_x1_1_description)/urdf/sensor_kit.xacro"/>
+  <xacro:property name="calibration" value="${load_yaml('$(arg config_dir)/sensors_calibration.yaml')}"/>
+
+  <!-- sensor kit -->
+  <xacro:sensor_kit_macro
+    parent="base_link"
+    x="${calibration['base_link']['sensor_kit_base_link']['x']}"
+    y="${calibration['base_link']['sensor_kit_base_link']['y']}"
+    z="${calibration['base_link']['sensor_kit_base_link']['z']}"
+    roll="${calibration['base_link']['sensor_kit_base_link']['roll']}"
+    pitch="${calibration['base_link']['sensor_kit_base_link']['pitch']}"
+    yaw="${calibration['base_link']['sensor_kit_base_link']['yaw']}"
+  />
+
+  <!-- embedded sensors -->
+
+</robot>

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
+)

aip_x1_1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml

@@ -0,0 +1,67 @@
+/**:
+  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

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

aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml

@@ -0,0 +1,51 @@
+/**:
+  ros__parameters:
+    additional_lidars: []
+    ransac_input_topics: [
+      "/sensing/lidar/front_center/outlier_filtered/pointcloud"
+    ]
+    use_single_frame_filter: True
+    use_time_series_filter: False
+
+    common_crop_box_filter:
+      parameters:
+        min_x: -50.0
+        max_x: 100.0
+        min_y: -50.0
+        max_y: 50.0
+        min_z: -0.5
+        negative: False
+
+    common_ground_filter:
+      plugin: "ground_segmentation::RayGroundFilterComponent"
+      parameters:
+        initial_max_slope: 10.0
+        general_max_slope: 10.0
+        local_max_slope: 10.0
+        min_height_threshold: 0.3
+        radial_divider_angle: 1.0
+        concentric_divider_distance: 0.0
+        use_vehicle_footprint: True
+
+    short_height_obstacle_detection_area_filter:
+      parameters:
+        min_x: 0.0
+        max_x: 19.6  # max_x: 18.0m + base_link2livox_front_center distance 1.6m
+        min_y: -4.0
+        max_y: 4.0
+        min_z: -0.5
+        max_z: 0.5
+        negative: False
+
+    ransac_ground_filter:
+      parameters:
+        outlier_threshold: 0.1
+        min_points: 400
+        min_inliers: 200
+        max_iterations: 50
+        height_threshold: 0.15
+        plane_slope_threshold: 10.0
+        voxel_size_x: 0.2
+        voxel_size_y: 0.2
+        voxel_size_z: 0.2
+        debug: False