From 3898d0624227ba4f7313f75b414da869b6187291 Mon Sep 17 00:00:00 2001
From: Marq Rasmussen <marq.razz@gmail.com>
Date: Wed, 11 Dec 2024 09:58:15 -0500
Subject: [PATCH] Add ground truth odometry (#10)

---
 .../c300_bringup/launch/gazebo_c300.launch.py |  1 +
 c300/c300_description/urdf/c300.gazebo.xacro  | 24 +++++++++++++++----
 .../launch/gazebo_c3pzero.launch.py           |  3 ++-
 3 files changed, 22 insertions(+), 6 deletions(-)

diff --git a/c300/c300_bringup/launch/gazebo_c300.launch.py b/c300/c300_bringup/launch/gazebo_c300.launch.py
index 9954d64..a0ca02f 100644
--- a/c300/c300_bringup/launch/gazebo_c300.launch.py
+++ b/c300/c300_bringup/launch/gazebo_c300.launch.py
@@ -147,6 +147,7 @@ def generate_launch_description():
             "/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan",
             "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
             "/imu@sensor_msgs/msg/Imu[ignition.msgs.IMU",
+            "/gt_odom@nav_msgs/msg/Odometry[gz.msgs.Odometry",
         ],
         output="screen",
     )
diff --git a/c300/c300_description/urdf/c300.gazebo.xacro b/c300/c300_description/urdf/c300.gazebo.xacro
index 702a652..7703475 100644
--- a/c300/c300_description/urdf/c300.gazebo.xacro
+++ b/c300/c300_description/urdf/c300.gazebo.xacro
@@ -2,11 +2,17 @@
 <robot name="c300_gazebo" xmlns:xacro="http://ros.org/wiki/xacro" xmlns:gz="http://gazebosim.org/schema">
 
   <!--
-Gazebo plugins can only be included once. Use this xacro as an example of how to run ros2_control in Gazebo.
-Required Gazebo plugins:
-  - gz_ros2_control::GazeboSimROS2ControlPlugin
-    required to read and publish the robots joint states
--->
+  Gazebo plugins can only be included once. Use this xacro to start the required plugins to simulate the mobile base in Gazebo.
+  Required Gazebo plugins:
+    - gz_ros2_control::GazeboSimROS2ControlPlugin
+      required to read and publish the robots joint states
+    - gz::sim::systems::Sensors
+      required to simulate the Lidar and any camera sensors
+    - gz::sim::systems::Imu
+      required to simulate the IMU sensor
+    - gz::sim::systems::OdometryPublisher
+      optional plugin to publish ground truth odometry data
+  -->
 <xacro:macro name="c300_gazebo" params="simulation_controllers">
 
   <gazebo>
@@ -22,6 +28,14 @@ Required Gazebo plugins:
       <render_engine>ogre2</render_engine>
     </plugin>
     <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"/>
+    <plugin filename="gz-sim-odometry-publisher-system"
+            name="gz::sim::systems::OdometryPublisher">
+      <odom_frame>odom</odom_frame>
+      <robot_base_frame>base_link</robot_base_frame>
+      <odom_publish_frequency>50</odom_publish_frequency>
+      <odom_topic>gt_odom</odom_topic>
+      <dimensions>3</dimensions>
+    </plugin>
   </gazebo>
 
   <gazebo reference="imu_link">
diff --git a/c3pzero/c3pzero_bringup/launch/gazebo_c3pzero.launch.py b/c3pzero/c3pzero_bringup/launch/gazebo_c3pzero.launch.py
index 930f81f..8bcd384 100644
--- a/c3pzero/c3pzero_bringup/launch/gazebo_c3pzero.launch.py
+++ b/c3pzero/c3pzero_bringup/launch/gazebo_c3pzero.launch.py
@@ -131,7 +131,7 @@ def generate_launch_description():
             "-y",
             "0.0",
             "-z",
-            "0.0",
+            "0.172",
             "-R",
             "0.0",
             "-P",
@@ -156,6 +156,7 @@ def generate_launch_description():
             "/left_arm_wrist_camera/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked",
             "/left_arm_wrist_camera/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo",
             "/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan",
+            "/gt_odom@nav_msgs/msg/Odometry[gz.msgs.Odometry",
             "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
         ],
         remappings=[