Merge branch 'tier4/universe' into mau/fix/sensing/vehicle_velocity_c…

…onverter

aip_x1_description/config/sensor_kit_calibration.yaml

@@ -6,24 +6,24 @@ 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
+    z: 0.0
+    roll: 0.0
+    pitch: 0.0
+    yaw: 0.0

aip_x1_description/package.xml

@@ -2,14 +2,14 @@
 <package format="2">
   <name>aip_x1_description</name>
   <version>0.1.0</version>
-  <description>The aip_xx1_description package</description>
+  <description>The aip_x1_description package</description>
 
-  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <maintainer email="takeshi.miura@tier4.jp">Takeshi Miura</maintainer>
   <license>Apache 2</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
-  <depend>livox_description</depend>
+  <depend>pandar_description</depend>
   <depend>velodyne_description</depend>
 
   <export>

aip_x1_description/urdf/sensor_kit.xacro

@@ -2,6 +2,7 @@
 <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:include filename="$(find imu_description)/urdf/imu.xacro"/>
 
     <xacro:arg name="gpu" default="false"/>
     <xacro:arg name="config_dir" default="$(find aip_x1_description)/config"/>
@@ -36,36 +37,43 @@
             ${calibration['sensor_kit_base_link']['velodyne_top_base_link']['yaw']}"
       />
     </xacro:VLP-16>
-    <xacro:include filename="$(find livox_description)/urdf/livox_horizon.xacro"/>
-    <xacro:livox_horizon_macro
-      name="livox_front_left"
+
+    <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']['livox_front_left_base_link']['x']}"
-      y="${calibration['sensor_kit_base_link']['livox_front_left_base_link']['y']}"
-      z="${calibration['sensor_kit_base_link']['livox_front_left_base_link']['z']}"
-      roll="${calibration['sensor_kit_base_link']['livox_front_left_base_link']['roll']}"
-      pitch="${calibration['sensor_kit_base_link']['livox_front_left_base_link']['pitch']}"
-      yaw="${calibration['sensor_kit_base_link']['livox_front_left_base_link']['yaw']}"
+      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:livox_horizon_macro
-      name="livox_front_center"
+    <!-- gnss -->
+    <xacro:imu_macro
+      name="gnss"
       parent="sensor_kit_base_link"
-      x="${calibration['sensor_kit_base_link']['livox_front_center_base_link']['x']}"
-      y="${calibration['sensor_kit_base_link']['livox_front_center_base_link']['y']}"
-      z="${calibration['sensor_kit_base_link']['livox_front_center_base_link']['z']}"
-      roll="${calibration['sensor_kit_base_link']['livox_front_center_base_link']['roll']}"
-      pitch="${calibration['sensor_kit_base_link']['livox_front_center_base_link']['pitch']}"
-      yaw="${calibration['sensor_kit_base_link']['livox_front_center_base_link']['yaw']}"
+      namespace=""
+      x="${calibration['sensor_kit_base_link']['gnss_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['gnss_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['gnss_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['gnss_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['gnss_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['gnss_link']['yaw']}"
+      fps="100"
     />
-    <xacro:livox_horizon_macro
-      name="livox_front_right"
+    <!-- imu -->
+    <xacro:imu_macro
+      name="tamagawa/imu"
       parent="sensor_kit_base_link"
-      x="${calibration['sensor_kit_base_link']['livox_front_right_base_link']['x']}"
-      y="${calibration['sensor_kit_base_link']['livox_front_right_base_link']['y']}"
-      z="${calibration['sensor_kit_base_link']['livox_front_right_base_link']['z']}"
-      roll="${calibration['sensor_kit_base_link']['livox_front_right_base_link']['roll']}"
-      pitch="${calibration['sensor_kit_base_link']['livox_front_right_base_link']['pitch']}"
-      yaw="${calibration['sensor_kit_base_link']['livox_front_right_base_link']['yaw']}"
+      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_launch/CMakeLists.txt

@@ -11,6 +11,5 @@ endif()
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
-  data
   config
 )

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
@@ -33,10 +42,9 @@
                       path: rpm
                       contains: [": velodyne_rpm"]
                       timeout: 3.0
-
-            livox:
+            pandar:
               type: diagnostic_aggregator/AnalyzerGroup
-              path: livox
+              path: pandar
               analyzers:
                 health_monitoring:
                   type: diagnostic_aggregator/AnalyzerGroup
@@ -45,62 +53,68 @@
                     connection:
                       type: diagnostic_aggregator/GenericAnalyzer
                       path: connection
-                      contains: [": livox_connection"]
-                      timeout: 3.0
-
-                    fan_status:
+                      contains: [": pandar_connection"]
+                      timeout: 5.0
+                    temperature:
                       type: diagnostic_aggregator/GenericAnalyzer
-                      path: fan_status
-                      contains: [": livox_fan_status"]
-                      timeout: 3.0
-
-                    firmware_status:
+                      path: temperature
+                      contains: [": pandar_temperature"]
+                      timeout: 5.0
+                    ptp:
                       type: diagnostic_aggregator/GenericAnalyzer
-                      path: firmware_status
-                      contains: [": livox_firmware_status"]
-                      timeout: 3.0
+                      path: ptp
+                      contains: [": pandar_ptp"]
+                      timeout: 5.0
 
-                    internal_voltage:
-                      type: diagnostic_aggregator/GenericAnalyzer
-                      path: internal_voltage
-                      contains: [": livox_internal_voltage"]
-                      timeout: 3.0
+        gnss:
+          type: diagnostic_aggregator/AnalyzerGroup
+          path: gnss
+          analyzers:
+            health_monitoring:
+              type: diagnostic_aggregator/AnalyzerGroup
+              path: health_monitoring
+              analyzers:
+                connection:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: connection
+                  contains: [": gnss_connection"]
+                  timeout: 3.0
 
-                    motor_status:
-                      type: diagnostic_aggregator/GenericAnalyzer
-                      path: motor_status
-                      contains: [": livox_motor_status"]
-                      timeout: 3.0
+                data:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: data
+                  contains: [": gnss_data"]
+                  timeout: 3.0
 
-                    # pps_signal:
-                    #   type: diagnostic_aggregator/GenericAnalyzer
-                    #   path: pps_signal
-                    #   contains: [": livox_pps_signal"]
-                    #   timeout: 3.0
+                antenna:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: antenna
+                  contains: [": gnss_antenna"]
+                  timeout: 3.0
 
-                    # ptp_signal:
-                    #   type: diagnostic_aggregator/GenericAnalyzer
-                    #   path: ptp_signal
-                    #   contains: [": livox_ptp_signal"]
-                    #   timeout: 3.0
+                tx_usage:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: tx_usage
+                  contains: [": gnss_tx_usage"]
+                  timeout: 3.0
 
-                    service_life:
-                      type: diagnostic_aggregator/GenericAnalyzer
-                      path: service_life
-                      contains: [": livox_service_life"]
-                      timeout: 3.0
+                spoofing:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: spoofing
+                  contains: [": gnss_spoofing"]
+                  timeout: 3.0
 
-                    temperature:
-                      type: diagnostic_aggregator/GenericAnalyzer
-                      path: temperature
-                      contains: [": livox_temperature"]
-                      timeout: 3.0
+                jamming:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: jamming
+                  contains: [": gnss_jamming"]
+                  timeout: 3.0
 
-                    # time_sync:
-                    #   type: diagnostic_aggregator/GenericAnalyzer
-                    #   path: time_sync
-                    #   contains: [": livox_time_sync"]
-                    #   timeout: 3.0
+                fix_topic_status:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: fix_topic_status
+                  contains: [": fix topic status"]
+                  timeout: 3.0
 
         imu:
           type: diagnostic_aggregator/AnalyzerGroup

aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml

@@ -12,19 +12,26 @@
 /**:
   ros__parameters:
     required_diags:
+      # gnss
+      gnss_connection: default
+      gnss_data: default
+      gnss_antenna: default
+      gnss_tx_usage: default
+      gnss_spoofing: default
+      gnss_jamming: default
+      fix topic status: default
+
+      # pandar
+      pandar_connection: default
+      pandar_temperature: default
+      pandar_ptp: default
+
       # velodyne
       velodyne_connection: default
       velodyne_temperature: default
       velodyne_rpm: default
 
-      # livox
-      livox_connection: default
-      livox_fan_status: default
-      livox_firmware_status: default
-      livox_internal_voltage: default
-      livox_motor_status: default
-      livox_service_life: default
-      livox_temperature: default
+      sensing_topic_status: default
 
       # imu
       gyro_bias_estimator: default

aip_x1_launch/config/gnss_poser.param.yaml

@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    base_frame: base_link
+    gnss_base_frame: gnss_link
+    map_frame: map
+    buff_epoch: 1
+    use_gnss_ins_orientation: false
+    gnss_pose_pub_method: 0         # 0: Median, 1: Average