From 42f6ddc6ba13c0a5ec5d1ecdc1d8e245816ed2f5 Mon Sep 17 00:00:00 2001
From: wittenator <9154515+wittenator@users.noreply.github.com>
Date: Thu, 2 Nov 2023 21:20:16 +0100
Subject: [PATCH] Added IMU

---
 src/dcaiti_control/description/imu.xacro             | 12 ++++++++++++
 src/dcaiti_control/description/robot.urdf.xacro      |  1 +
 src/dcaiti_control/launch/launch_sim.py              |  3 ++-
 src/dcaiti_control/worlds/empty.sdf.template         |  3 +++
 .../worlds/generated_worlds/AU2_skidpad.sdf          |  3 +++
 5 files changed, 21 insertions(+), 1 deletion(-)
 create mode 100644 src/dcaiti_control/description/imu.xacro

diff --git a/src/dcaiti_control/description/imu.xacro b/src/dcaiti_control/description/imu.xacro
new file mode 100644
index 0000000..3e40735
--- /dev/null
+++ b/src/dcaiti_control/description/imu.xacro
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
+    <gazebo reference="laser_frame">
+        <material>Gazebo/Red</material>
+        <sensor name="imu_sensor" type="imu">
+          <always_on>1</always_on>
+          <update_rate>100</update_rate>
+          <visualize>true</visualize>
+          <topic>/lidar/imu</topic>
+      </sensor>
+    </gazebo>  
+</robot>
\ No newline at end of file
diff --git a/src/dcaiti_control/description/robot.urdf.xacro b/src/dcaiti_control/description/robot.urdf.xacro
index 994d34d..606f84a 100644
--- a/src/dcaiti_control/description/robot.urdf.xacro
+++ b/src/dcaiti_control/description/robot.urdf.xacro
@@ -57,5 +57,6 @@
     <xacro:include filename="robot_core.xacro" />
     <xacro:include filename="bbox_camera.xacro" />
     <xacro:include filename="lidar.xacro" />
+    <xacro:include filename="imu.xacro" />
 
 </robot>
\ No newline at end of file
diff --git a/src/dcaiti_control/launch/launch_sim.py b/src/dcaiti_control/launch/launch_sim.py
index ddc09f2..181c68e 100644
--- a/src/dcaiti_control/launch/launch_sim.py
+++ b/src/dcaiti_control/launch/launch_sim.py
@@ -63,7 +63,8 @@ def generate_launch_description():
             '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
             '/boxes@vision_msgs/msg/Detection2DArray@ignition.msgs.AnnotatedAxisAligned2DBox_V',
             '/lidar@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan',
-            '/lidar/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked'
+            '/lidar/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked',
+            'lidar/imu@sensor_msgs/msg/Imu@gz.msgs.IMU'
         ],
         output='screen'
     )
diff --git a/src/dcaiti_control/worlds/empty.sdf.template b/src/dcaiti_control/worlds/empty.sdf.template
index 3329b81..994c210 100644
--- a/src/dcaiti_control/worlds/empty.sdf.template
+++ b/src/dcaiti_control/worlds/empty.sdf.template
@@ -29,6 +29,9 @@
       filename="ignition-gazebo-contact-system"
       name="gz::sim::systems::Contact">
     </plugin>
+    <plugin filename="libignition-gazebo-imu-system.so"
+        name="ignition::gazebo::systems::Imu">
+    </plugin> 
     <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
         <render_engine>ogre2</render_engine>
     </plugin>
diff --git a/src/dcaiti_control/worlds/generated_worlds/AU2_skidpad.sdf b/src/dcaiti_control/worlds/generated_worlds/AU2_skidpad.sdf
index 943d0b1..6d623f8 100644
--- a/src/dcaiti_control/worlds/generated_worlds/AU2_skidpad.sdf
+++ b/src/dcaiti_control/worlds/generated_worlds/AU2_skidpad.sdf
@@ -29,6 +29,9 @@
       filename="ignition-gazebo-contact-system"
       name="gz::sim::systems::Contact">
     </plugin>
+    <plugin filename="libignition-gazebo-imu-system.so"
+        name="ignition::gazebo::systems::Imu">
+    </plugin> 
     <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
         <render_engine>ogre2</render_engine>
     </plugin>