add front lower

tier4 · Jun 24, 2024 · 3d3587f · 3d3587f
1 parent 5ecab35
commit 3d3587f
Show file tree

Hide file tree

Showing 4 changed files with 59 additions and 20 deletions.
diff --git a/aip_x2_description/config/sensor_kit_calibration.yaml b/aip_x2_description/config/sensor_kit_calibration.yaml
@@ -118,6 +118,21 @@ sensor_kit_base_link:
     yaw: 0.0
 
   # front lower
+  pandar/front/lidar_link:
+    x: 0.94058
+    y: -1.07589
+    z: -1.91826
+    roll: 0.0
+    pitch: 0.0
+    yaw: 0.0
+  pandar/front_bottom/lidar_link:
+    x: 0.96758
+    y: -1.07589
+    z: -2.15234
+    roll: 0.0
+    pitch: 0.6981 # 40[deg]
+    yaw: 0.0
+
 
   # rear lower
 

diff --git a/aip_x2_description/config/sensors_calibration.yaml b/aip_x2_description/config/sensors_calibration.yaml
@@ -8,10 +8,10 @@ base_link:
     yaw: 0.0
 
   # radar
-  # ars548/front_center/radar_link:
-  #   x: -0.01295
-  #   y: 0.0
-  #   z: -0.07554
-  #   roll: 0.0
-  #   pitch: 0.0
-  #   yaw: 0.0
+  ars548/front_center/radar_link:
+    x: 5.69207
+    y: 0.0
+    z: 0.78894
+    roll: 0.0
+    pitch: 0.0
+    yaw: 0.0
diff --git a/aip_x2_description/urdf/sensor_kit.xacro b/aip_x2_description/urdf/sensor_kit.xacro
@@ -254,5 +254,28 @@
       fov="1.3"
     />
 
+    <!-- front lower -->
+    <xacro:PandarQT
+      name="pandar/front/lidar"
+      parent="sensor_kit_base_link"
+      x="${calibration['sensor_kit_base_link']['pandar/front/lidar_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['pandar/front/lidar_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['pandar/front/lidar_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['pandar/front/lidar_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['pandar/front/lidar_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['pandar/front/lidar_link']['yaw']}"
+    />
+
+    <xacro:PandarQT
+      name="pandar/front_bottom/lidar"
+      parent="sensor_kit_base_link"
+      x="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_link']['yaw']}"
+    />
+
   </xacro:macro>
 </robot>
diff --git a/aip_x2_description/urdf/sensors.xacro b/aip_x2_description/urdf/sensors.xacro
@@ -1,11 +1,12 @@
 <?xml version="1.0"?>
 <robot name="vehicle" xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:include filename="$(find aip_x2_description)/urdf/radar.xacro"/>
-  <xacro:arg name="config_dir" default="$(find aip_x2_description)/config"/>
-  <xacro:property name="calibration" value="${xacro.load_yaml('$(arg config_dir)/sensors_calibration.yaml')}"/>
+  <xacro:include filename="$(find aip_x2_description)/urdf/radar.xacro" />
+  <xacro:arg name="config_dir" default="$(find aip_x2_description)/config" />
+  <xacro:property name="calibration"
+    value="${xacro.load_yaml('$(arg config_dir)/sensors_calibration.yaml')}" />
 
   <!-- sensor kit -->
-  <xacro:include filename="sensor_kit.xacro"/>
+  <xacro:include filename="sensor_kit.xacro" />
   <xacro:sensor_kit_macro
     parent="base_link"
     x="${calibration['base_link']['sensor_kit_base_link']['x']}"
@@ -18,14 +19,14 @@
 
   <!-- radar -->
   <xacro:radar_macro
-  name="front_center/radar_link"
-  parent="front_unit_base_link"
-  x="${calibration['front_unit_base_link']['front_center/radar_link']['x']}"
-  y="${calibration['front_unit_base_link']['front_center/radar_link']['y']}"
-  z="${calibration['front_unit_base_link']['front_center/radar_link']['z']}"
-  roll="${calibration['front_unit_base_link']['front_center/radar_link']['roll']}"
-  pitch="${calibration['front_unit_base_link']['front_center/radar_link']['pitch']}"
-  yaw="${calibration['front_unit_base_link']['front_center/radar_link']['yaw']}"
-/>
+    name="ars548/front_center/radar"
+    parent="base_link"
+    x="${calibration['base_link']['ars548/front_center/radar_link']['x']}"
+    y="${calibration['base_link']['ars548/front_center/radar_link']['y']}"
+    z="${calibration['base_link']['ars548/front_center/radar_link']['z']}"
+    roll="${calibration['base_link']['ars548/front_center/radar_link']['roll']}"
+    pitch="${calibration['base_link']['ars548/front_center/radar_link']['pitch']}"
+    yaw="${calibration['base_link']['ars548/front_center/radar_link']['yaw']}"
+  />
 
 </robot>