Fix IMUs and GPS antennas TF (#20)

* Add `*_mount` and `*_sensor` chains for IMUs/GPS - Define `imu_rear_mount` -> `imu_rear_sensor` and `imu_fps_r_mount` -> `imu_fps_r_sensor` TFs - `*_mount` frames describe the physical sensor orientation - `*_sensor` frames describe the orientation that is consistent with the microstrain IMU ROS driver ENU measurements - Fix `imu_rear_mount` Y translation based on in-house measurements - Fix `gps_antenna\_*_mount` translation based on in-house measurements - Define `gps_antenna\_*_mount` -> `gps_antenna\_*_sensor` TF based on GNSS antenna hight relative to mount * Remove `oxts*` frames and parameters as the sensor is no longer mounted in the vehicle
ipab-rad · Nov 11, 2024 · a45d5b7 · a45d5b7
1 parent 3135d6c
commit a45d5b7
Show file tree

Hide file tree

Showing 6 changed files with 70 additions and 24 deletions.
diff --git a/av_car_description/CHANGELOG.rst b/av_car_description/CHANGELOG.rst
@@ -2,6 +2,26 @@
 Changelog for package av_car_description
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+Forthcoming
+-----------
+* Add *_mount and *_sensor chains for IMUs/GPS
+  - Define `imu_rear_mount` -> `imu_rear_sensor` and
+  `imu_fps_r_mount` -> `imu_fps_r_sensor` TFs
+  - `*_mount` frames describe the physical sensor
+  orientation
+  - `*_sensor` frames describe the orientation that is
+  consistent with the microstrain IMU ROS driver ENU measurements
+  - Fix `imu_rear_mount` Y translation based on in-house
+  measurements
+  - Fix `gps_antenna\_*_mount` translation based on in-house
+  measurements
+  - Define `gps_antenna\_*_mount` -> `gps_antenna\_*_sensor` TF
+    based on GNSS antenna hight relative to mount
+* Remove `oxts*` frames and parameters as the sensor is no longer
+  mounted in the vehicle
+  
+* Contributors: Hector Cruz
+
 1.6.0 (2024-10-18)
 ------------------
 * Refactor LIDAR mounting structure based on LIDAR calibration.

diff --git a/av_car_description/urdf/gps.xacro b/av_car_description/urdf/gps.xacro
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-  <xacro:macro name="gps" params="name *origin">
+  <xacro:macro name="gps" params="name parent *origin">
     <joint name="${name}_joint" type="fixed">
       <xacro:insert_block name="origin"/>
-      <parent link="${roof_datum}"/>
+      <parent link="${parent}"/>
       <child link="${name}"/>
     </joint>
     <link name="${name}">

diff --git a/av_car_description/urdf/imu.xacro b/av_car_description/urdf/imu.xacro
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-  <xacro:macro name="imu" params="imu *origin">
+  <xacro:macro name="imu" params="imu parent *origin">
     <joint name="${imu}_joint" type="fixed">
       <xacro:insert_block name="origin"/>
-      <parent link="${roof_datum}"/>
+      <parent link="${parent}"/>
       <child link="${imu}"/>
     </joint>
     <link name="${imu}">

diff --git a/av_car_description/urdf/mondeo_dca/mondeo_dca_params.xacro b/av_car_description/urdf/mondeo_dca/mondeo_dca_params.xacro
@@ -47,18 +47,13 @@
   <xacro:property name="wheel_weight" value="10"/>
 
   <!-- SENSOR OFFSETS -->
-  <xacro:property name="oxts_x" value="${-0.59 - roof_datum_x}"/>
-  <!-- Relative to roof_datum -->
-  <xacro:property name="oxts_y" value="-0.07"/>
-  <!-- Relative to roof_datum -->
-  <xacro:property name="oxts_z" value="${0.9 - roof_datum_z - base_link_z}"/>
-  <!-- Relative to roof_datum -->
-
   <!-- The GPS antenna has been measured relative to the rear axle. The numerical values below gives these
        measurements. However, offsets are applied to make these values relative to the roof_datum -->
-  <xacro:property name="gps_antenna_base_left_x" value="${0.27 - roof_datum_x}"/>
-  <xacro:property name="gps_antenna_base_left_y" value="0.5475"/>
-  <xacro:property name="gps_antenna_base_left_z" value="${1.02 - roof_datum_z}"/>
+  <xacro:property name="gps_antenna_base_left_x" value="${-0.78 - roof_datum_x}"/>
+  <xacro:property name="gps_antenna_base_left_y" value="0.525"/>
+  <xacro:property name="gps_antenna_base_left_z" value="${0.728 - roof_datum_z}"/>
+  <!-- Height of the GNSS antenna relaitve to the mount base -->
+  <xacro:property name="gps_antenna_sensor_z" value="0.21"/>
 
   <!-- NOMENCLATURE -->
   <xacro:property name="vehicle_bonnet" value="vehicle_bonnet"/>

diff --git a/av_car_description/urdf/mondeo_dca/mondeo_dca_sensors.xacro b/av_car_description/urdf/mondeo_dca/mondeo_dca_sensors.xacro
@@ -169,28 +169,39 @@
   </xacro:camera_mount>
 
   <!-- IMU -->
-  <xacro:imu imu="imu_rear_mount">
-    <origin xyz="${oxts_x-0.19} ${oxts_y+0.23} ${oxts_z-0.44}" rpy="0 0 0"/>
+  <xacro:imu imu="imu_rear_mount" parent="${roof_datum}">
+    <origin xyz="-2.67 0.0 -1.102" rpy="0.0 3.14159 -1.5708"/>
   </xacro:imu>
 
-  <xacro:imu imu="imu_fsp_r_mount">
-    <origin xyz="-0.23515 -0.355 -0.05442" rpy="0 0 0"/>
+  <xacro:imu imu="imu_rear_sensor" parent="imu_rear_mount">
+    <origin xyz="0.0 0.0 0.0" rpy="3.14159 0.0 0.0 "/>
   </xacro:imu>
 
-  <!-- OXTS -->
-  <xacro:oxts name="oxts">
-    <origin xyz="${oxts_x} ${oxts_y} ${oxts_z}" rpy="0.0 0.0 0.0"/>
-  </xacro:oxts>
+  <xacro:imu imu="imu_fsp_r_mount" parent="${roof_datum}">
+    <origin xyz="-0.23515 -0.355 -0.05442" rpy="0.0 3.14159 -1.5708"/>
+  </xacro:imu>
+
+  <xacro:imu imu="imu_fsp_r_sensor" parent="imu_fsp_r_mount">
+    <origin xyz="0.0 0.0 0.0" rpy="3.14159 0.0 0.0"/>
+  </xacro:imu>
 
   <!-- GPS -->
-  <xacro:gps name="gps_antenna_right_mount">
+  <xacro:gps name="gps_antenna_right_mount" parent="${roof_datum}">
     <origin xyz="${gps_antenna_base_left_x} ${-gps_antenna_base_left_y} ${gps_antenna_base_left_z}" rpy="0 0 0"/>
   </xacro:gps>
 
-  <xacro:gps name="gps_antenna_left_mount">
+  <xacro:gps name="gps_antenna_right_sensor" parent="gps_antenna_right_mount">
+    <origin xyz="0.0 0.0 ${gps_antenna_sensor_z}" rpy="0 0 0"/>
+  </xacro:gps>
+
+  <xacro:gps name="gps_antenna_left_mount" parent="${roof_datum}">
     <origin xyz="${gps_antenna_base_left_x} ${gps_antenna_base_left_y} ${gps_antenna_base_left_z}" rpy="0 0 0"/>
   </xacro:gps>
 
+  <xacro:gps name="gps_antenna_left_sensor" parent="gps_antenna_left_mount">
+    <origin xyz="0.0 0.0 ${gps_antenna_sensor_z}" rpy="0 0 0"/>
+  </xacro:gps>
+
   <!-- LIDAR -->
   <xacro:lidar name="lidar_left_mount" parent="${roof_datum}">
     <origin xyz="-0.03015 0.720 -0.22242" rpy="-0.087266 0.087266 0.6562438"/>

diff --git a/av_car_meshes/CHANGELOG.rst b/av_car_meshes/CHANGELOG.rst
@@ -2,6 +2,26 @@
 Changelog for package av_car_meshes
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+Forthcoming
+-----------
+* Add *_mount and *_sensor chains for IMUs/GPS
+  - Define `imu_rear_mount` -> `imu_rear_sensor` and
+  `imu_fps_r_mount` -> `imu_fps_r_sensor` TFs
+  - `*_mount` frames describe the physical sensor
+  orientation
+  - `*_sensor` frames describe the orientation that is
+  consistent with the microstrain IMU ROS driver ENU measurements
+  - Fix `imu_rear_mount` Y translation based on in-house
+  measurements
+  - Fix `gps_antenna\_*_mount` translation based on in-house
+  measurements
+  - Define `gps_antenna\_*_mount` -> `gps_antenna\_*_sensor` TF
+    based on GNSS antenna hight relative to mount
+* Remove `oxts*` frames and parameters as the sensor is no longer
+  mounted in the vehicle
+  
+* Contributors: Hector Cruz
+
 1.6.0 (2024-10-18)
 ------------------
 * Refactor LIDAR mounting structure based on LIDAR calibration.