From 4569ca83025f1bbf61d8d73c4b0d04362fa9cca5 Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Tue, 8 Jun 2021 10:53:20 +0200
Subject: [PATCH] Add second imu frame. One for simulation and one for the real
 imu.

---
 urdf/include/sensors.xacro | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 deletions(-)

diff --git a/urdf/include/sensors.xacro b/urdf/include/sensors.xacro
index 2a00506..1cc7cd4 100644
--- a/urdf/include/sensors.xacro
+++ b/urdf/include/sensors.xacro
@@ -76,9 +76,18 @@
       <origin xyz="0.0 0.0 0.0265" rpy="0 1.0 0" />
     </xacro:realsense_d435_depth>
     
-    <xacro:bno055_imu name="imu" parent="base_footprint" resolution="low_res" sim_config="$(find iri_ana_gazebo)/config/ana_bno055_config.yaml">
-      <origin xyz="0.11928 -0.08805 0.23061" rpy="0 -1.5707 0" />
+    <xacro:bno055_imu name="imu_sim" parent="base_footprint" resolution="low_res" sim_config="$(find iri_ana_gazebo)/config/ana_bno055_config.yaml">
+      <origin xyz="0.11928 -0.08805 0.23061" rpy="0 0 0" />
     </xacro:bno055_imu>
+    
+    <link name="imu_bno055">
+    </link>
+    
+    <joint name="joint_base_footprint_to_${name}_imu" type="fixed" >
+      <parent link="base_footprint"/>
+      <child link="imu_bno055"/>
+      <origin xyz="0.11928 -0.08805 0.23061" rpy="0 -1.5709 0" />
+    </joint>
 
     <!--
     <link name="${pan_tilt_xacro_properties['load_name']}">
-- 
GitLab