From 8fee629191bba3f6fcffa8987eb18c5e31c682b4 Mon Sep 17 00:00:00 2001
From: Henrique Scharlau Coelho <henriquescharlaucoelho@gmail.com>
Date: Fri, 13 Oct 2023 11:33:35 -0300
Subject: [PATCH] imu added to robot

---
 twil_description/xacro/imu.urdf.xacro  | 80 ++++++++++++++++++++++++++++++++++
 twil_description/xacro/twil.urdf.xacro |  7 +++
 2 files changed, 87 insertions(+)
 create mode 100644 twil_description/xacro/imu.urdf.xacro

diff --git a/twil_description/xacro/imu.urdf.xacro b/twil_description/xacro/imu.urdf.xacro
new file mode 100644
index 0000000..a9fdd17
--- /dev/null
+++ b/twil_description/xacro/imu.urdf.xacro
@@ -0,0 +1,80 @@
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="imu" params="name parent *origin">
+  <xacro:include filename="$(find twil_description)/xacro/eurocard.urdf.xacro" />
+    
+    <xacro:eurocard name="${name}" parent="${parent}">
+        <xacro:insert_block name="origin"/>
+    </xacro:eurocard>
+
+    <gazebo reference="imu">
+        <sensor name="imu_sensor" type="imu">
+        <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+            <ros>
+            <namespace>/demo</namespace>
+            <remapping>~/out:=imu</remapping>
+            </ros>
+            <initial_orientation_as_reference>false</initial_orientation_as_reference>
+        </plugin>
+        <always_on>true</always_on>
+        <update_rate>100</update_rate>
+        <visualize>true</visualize>
+        <imu>
+            <angular_velocity>
+            <x>
+                <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+                <bias_mean>0.0000075</bias_mean>
+                <bias_stddev>0.0000008</bias_stddev>
+                </noise>
+            </x>
+            <y>
+                <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+                <bias_mean>0.0000075</bias_mean>
+                <bias_stddev>0.0000008</bias_stddev>
+                </noise>
+            </y>
+            <z>
+                <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+                <bias_mean>0.0000075</bias_mean>
+                <bias_stddev>0.0000008</bias_stddev>
+                </noise>
+            </z>
+            </angular_velocity>
+            <linear_acceleration>
+            <x>
+                <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+                <bias_mean>0.1</bias_mean>
+                <bias_stddev>0.001</bias_stddev>
+                </noise>
+            </x>
+            <y>
+                <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+                <bias_mean>0.1</bias_mean>
+                <bias_stddev>0.001</bias_stddev>
+                </noise>
+            </y>
+            <z>
+                <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+                <bias_mean>0.1</bias_mean>
+                <bias_stddev>0.001</bias_stddev>
+                </noise>
+            </z>
+            </linear_acceleration>
+        </imu>
+        </sensor>
+    </gazebo>
+  </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro
index 8107882..0ec0cee 100644
--- a/twil_description/xacro/twil.urdf.xacro
+++ b/twil_description/xacro/twil.urdf.xacro
@@ -17,6 +17,7 @@
   <xacro:include filename="$(find twil_description)/xacro/tower.urdf.xacro" />
   <xacro:include filename="$(find twil_description)/xacro/step_motor.urdf.xacro" />
   <xacro:include filename="$(find twil_description)/xacro/sonar.urdf.xacro" />
+  <xacro:include filename="$(find twil_description)/xacro/imu.urdf.xacro" />
 
   <link name="twil_origin" />
 
@@ -104,10 +105,15 @@
     <origin xyz="0 -0.1 ${0.0015+0.3+0.003+0.1/2}" rpy="0.0 0.0 0.0" />
   </xacro:eurocard>
 
+  <xacro:imu name="imu" parent="chassis">
+    <origin xyz="0 -0.2 ${0.0015+0.3+0.003+0.1/2}" />
+  </xacro:imu>
+
   <xacro:cpu name="cpu" parent="chassis">
     <origin xyz="0 0.0 ${0.0015+0.3+0.003+0.127+0.003+0.17}" rpy="0.0 0.0 ${M_PI/2}" />
   </xacro:cpu>
 
+
   <link name="chassis_top" />
 
   <joint name="chassis_top_joint" type="fixed">
@@ -181,4 +187,5 @@
 	<xacro:include filename="$(find twil_description)/xacro/d435.urdf.xacro"/>
   </xacro:if>
 
+  
 </robot>
-- 
2.12.0