update aip_xx1 for gen2

Signed-off-by: N-Eiki <[email protected]>
tier4 · Jun 19, 2024 · 143346d · 143346d
1 parent 9d07d76
commit 143346d
Show file tree

Hide file tree

Showing 14 changed files with 440 additions and 59 deletions.
diff --git a/aip_xx1_gen2_description/CMakeLists.txt b/aip_xx1_gen2_description/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(aip_xx1_gen2_description)
+project(aip_xx1_description)
 
 find_package(ament_cmake_auto REQUIRED)
 

diff --git a/aip_xx1_gen2_description/package.xml b/aip_xx1_gen2_description/package.xml
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>aip_xx1_gen2_description</name>
+  <name>aip_xx1_description</name>
   <version>0.1.0</version>
-  <description>The aip_xx1_gen2_description package</description>
+  <description>The aip_xx1_description package</description>
 
   <maintainer email="[email protected]">Yukihiro Saito</maintainer>
   <license>Apache 2</license>

diff --git a/aip_xx1_gen2_launch/CMakeLists.txt b/aip_xx1_gen2_launch/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(aip_xx1_gen2_launch)
+project(aip_xx1_launch)
 
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()

diff --git a/aip_xx1_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_xx1_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml
@@ -18,6 +18,15 @@
           type: diagnostic_aggregator/AnalyzerGroup
           path: lidar
           analyzers:
+            performance_monitoring:
+              type: diagnostic_aggregator/AnalyzerGroup
+              path: performance_monitoring
+              analyzers:
+                blockage:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: blockage
+                  contains: [": blockage_validation"]
+                  timeout: 1.0
             velodyne:
               type: diagnostic_aggregator/AnalyzerGroup
               path: velodyne

diff --git a/aip_xx1_gen2_launch/config/gyro_bias_estimator.param.yaml b/aip_xx1_gen2_launch/config/gyro_bias_estimator.param.yaml
@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    gyro_bias_threshold: 0.008 # [rad/s]
+    timer_callback_interval_sec: 0.5 # [sec]
+    diagnostics_updater_interval_sec: 0.5 # [sec]
+    straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]
diff --git a/aip_xx1_gen2_launch/config/lidar_gen2.yaml b/aip_xx1_gen2_launch/config/lidar_gen2.yaml
@@ -0,0 +1,69 @@
+launches:
+  - sensor_type: hesai_OT128
+    namespace: top
+    parameters:
+      max_range: 300.0
+      sensor_frame: hesai_top
+      sensor_ip: 192.168.1.201
+      data_port: 2368
+      scan_phase: 160
+      vertical_bins: 128
+  - sensor_type: hesai_XT32
+    namespace: front_left
+    parameters:
+      max_range: 300.0
+      sensor_frame: hesai_front_left
+      sensor_ip: 192.168.1.21
+      data_port: 2369
+      scan_phase: 50.0
+      cloud_min_angle: 50
+      cloud_max_angle: 320
+      vertical_bins: 16
+      horizontal_ring_id: 0
+  - sensor_type: hesai_XT32
+    namespace: front_right
+    parameters:
+      max_range: 300.0
+      sensor_frame: hesai_front_right
+      sensor_ip: 192.168.1.22
+      data_port: 2370
+      scan_phase: 310.0
+      cloud_min_angle: 40
+      cloud_max_angle: 310
+  - sensor_type: hesai_XT32
+    namespace: side_left
+    parameters:
+      max_range: 10.0
+      sensor_frame: hesai_side_left
+      sensor_ip: 192.168.1.23
+      data_port: 2371
+      scan_phase: 90.0
+      cloud_min_angle: 90
+      cloud_max_angle: 270
+  - sensor_type: hesai_XT32
+    namespace: side_right
+    parameters:
+      max_range: 10.0
+      sensor_frame: hesai_side_right
+      sensor_ip: 192.168.1.24
+      data_port: 2372
+      scan_phase: 270.0
+      cloud_min_angle: 90
+      cloud_max_angle: 270
+
+preprocessor:
+  input_topics:
+    - /sensing/lidar/top/pointcloud_before_sync
+    - /sensing/lidar/side_left/pointcloud_before_sync
+    - /sensing/lidar/side_right/pointcloud_before_sync
+    - /sensing/lidar/front_left/pointcloud_before_sync
+    - /sensing/lidar/front_right/pointcloud_before_sync
+  input_offset:
+    - 0.035
+    - 0.025
+    - 0.025
+    - 0.025
+    - 0.025
+  timeout_sec: 0.095
+  input_twist_topic_type: twist
+  publish_synchronized_pointcloud: true
diff --git a/aip_xx1_gen2_launch/config/lidar_launch.yaml b/aip_xx1_gen2_launch/config/lidar_launch.yaml
@@ -0,0 +1,70 @@
+launches:
+  - sensor_type: velodyne_VLS128
+    namespace: top
+    parameters:
+      max_range: 250.0
+      sensor_frame: velodyne_top
+      sensor_ip: 192.168.1.201
+      data_port: 2368
+      scan_phase: 300.0
+      vertical_bins: 128
+      horizontal_ring_id: 64
+      horizontal_resolution: 0.4
+      is_channel_order_top2down: false
+  - sensor_type: velodyne_VLP16
+    namespace: left
+    parameters:
+      max_range: 5.0
+      sensor_frame: velodyne_left
+      sensor_ip: 192.168.1.202
+      data_port: 2369
+      scan_phase: 180.0
+      cloud_min_angle: 300
+      cloud_max_angle: 60
+      vertical_bins: 16
+      horizontal_ring_id: 0
+      horizontal_resolution: 0.4
+      is_channel_order_top2down: false
+  - sensor_type: velodyne_VLP16
+    namespace: right
+    parameters:
+      max_range: 5.0
+      sensor_frame: velodyne_right
+      sensor_ip: 192.168.1.203
+      data_port: 2370
+      scan_phase: 180.0
+      cloud_min_angle: 300
+      cloud_max_angle: 60
+      vertical_bins: 16
+      horizontal_ring_id: 0
+      horizontal_resolution: 0.4
+      is_channel_order_top2down: false
+  - sensor_type: velodyne_VLP16
+    namespace: rear
+    parameters:
+      max_range: 1.5
+      sensor_frame: velodyne_rear
+      sensor_ip: 192.168.1.204
+      data_port: 2371
+      scan_phase: 180.0
+      cloud_min_angle: 300
+      cloud_max_angle: 60
+      vertical_bins: 16
+      horizontal_ring_id: 0
+      horizontal_resolution: 0.4
+      is_channel_order_top2down: false
+
+preprocessor:
+  input_topics:
+    - /sensing/lidar/top/pointcloud
+    - /sensing/lidar/left/pointcloud
+    - /sensing/lidar/right/pointcloud
+    - /sensing/lidar/rear/pointcloud
+  input_offset:
+    - 0.035
+    - 0.025
+    - 0.025
+    - 0.025
+  timeout_sec: 0.095
+  input_twist_topic_type: twist
+  publish_synchronized_pointcloud: false
diff --git a/aip_xx1_gen2_launch/launch/imu.launch.xml b/aip_xx1_gen2_launch/launch/imu.launch.xml
@@ -1,14 +1,27 @@
 <launch>
 
+  <arg name="launch_driver" default="true" />
+  <arg name ="vehicle_id" default="$(env VEHICLE_ID default)" />
+  <arg name="imu_connection" default="$(env IMU_CONNECTION serial)" description="serial or can" />
+
   <group>
     <push-ros-namespace namespace="imu"/>
 
-    <arg name="launch_driver" default="true" />
-    <arg name="interface" default="canImu"/>
-    <arg name="receiver_interval_sec" default="0.01"/>
-    <arg name ="vehicle_id" default="$(env VEHICLE_ID default)" />
+    <!-- Use Serial Driver to connect to IMU  -->
+    <group if="$(eval &quot;'$(var imu_connection)'=='serial'&quot;)">
+      <push-ros-namespace namespace="tamagawa"/>
+      <node pkg="tamagawa_imu_driver" name="tag_serial_driver" exec="tag_serial_driver" if="$(var launch_driver)" respawn="true" respawn_delay="10.0">
+        <remap from="imu/data_raw" to="imu_raw" />
+        <param name="port" value="/dev/imu" />
+        <param name="imu_frame_id" value="tamagawa/imu_link" />
+      </node>
+    </group>
 
-    <group>
+    <!-- Use CAN to connect to IMU  -->
+    <group if="$(eval &quot;'$(var imu_connection)'=='can'&quot;)">
+      <arg name="interface" default="canImu"/>
+      <arg name="receiver_interval_sec" default="0.01"/>
+      <arg name ="vehicle_id" default="$(env VEHICLE_ID default)" />
       <push-ros-namespace namespace="tamagawa"/>
       <include file="$(find-pkg-share ros2_socketcan)/launch/socket_can_receiver.launch.py">
         <arg name="interface" value="$(var interface)"/>
@@ -21,6 +34,7 @@
       </node>
     </group>
 
+    <!-- IMU Correction launch-->
     <arg name="imu_raw_name" default="tamagawa/imu_raw"/>
     <arg name="imu_corrector_param_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_xx1/imu_corrector.param.yaml"/>
     <include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml">
@@ -29,10 +43,13 @@
       <arg name="param_file" value="$(var imu_corrector_param_file)"/>
     </include>
 
+    <!-- Gyro Bias Estimator-->
+    <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share aip_xx1_launch)/config/gyro_bias_estimator.param.yaml"/>
     <include file="$(find-pkg-share imu_corrector)/launch/gyro_bias_estimator.launch.xml">
       <arg name="input_imu_raw" value="$(var imu_raw_name)"/>
       <arg name="input_odom" value="/localization/kinematic_state"/>
       <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
+      <arg name="gyro_bias_estimator_param_file" value="$(var gyro_bias_estimator_param_file)"/>
     </include>
   </group>