From c400d3617c7b975ec3e81b913ca782e9aeb35ac4 Mon Sep 17 00:00:00 2001
From: David Conner <robotics@cnu.edu>
Date: Fri, 9 Jun 2023 15:03:54 -0400
Subject: [PATCH] Add ground truth topic

---
 .../models/turtlebot3_burger/model.sdf        | 18 ++++++++++++-
 .../models/turtlebot3_waffle/model.sdf        | 27 ++++++++++++++-----
 2 files changed, 38 insertions(+), 7 deletions(-)

diff --git a/turtlebot3_gazebo/models/turtlebot3_burger/model.sdf b/turtlebot3_gazebo/models/turtlebot3_burger/model.sdf
index bd803e49..fd9eaa4d 100644
--- a/turtlebot3_gazebo/models/turtlebot3_burger/model.sdf
+++ b/turtlebot3_gazebo/models/turtlebot3_burger/model.sdf
@@ -403,6 +403,22 @@
       <update_rate>30</update_rate>
       <joint_name>wheel_left_joint</joint_name>
       <joint_name>wheel_right_joint</joint_name>
-    </plugin>      
+    </plugin>
+
+    <!-- publish ground truth -->
+    <plugin name="turtlebot3_ground_truth" filename="libgazebo_ros_p3d.so">
+        <always_on>true</always_on>
+        <update_rate>50.0</update_rate>
+        <body_name>base_footprint</body_name>
+        <gaussian_noise>0.001</gaussian_noise>
+        <frame_name>world</frame_name>
+        <xyz_offset>0 0 0</xyz_offset>
+        <rpy_offset>0 0 0</rpy_offset>
+        <ros>
+          <!--https://github.com/ros-simulation/gazebo_ros_pkgs/issues/1318-->
+          <remapping>odom:=ground_truth</remapping>
+        </ros>
+    </plugin>
+
   </model>
 </sdf>
diff --git a/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf b/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
index fb5ab6c2..9a52b39c 100644
--- a/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
+++ b/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
@@ -1,6 +1,6 @@
 <?xml version="1.0" ?>
 <sdf version="1.5">
-  <model name="turtlebot3_waffle">  
+  <model name="turtlebot3_waffle">
   <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
 
     <link name="base_footprint"/>
@@ -95,7 +95,7 @@
       </sensor>
     </link>
 
-    <link name="base_scan">    
+    <link name="base_scan">
       <inertial>
         <pose>-0.052 0 0.111 0 0 0</pose>
         <inertia>
@@ -237,7 +237,7 @@
         </inertia>
         <mass>0.1</mass>
       </inertial>
-    
+
       <collision name="wheel_right_collision">
         <pose>0.0 -0.144 0.023 -1.57 0 0</pose>
         <geometry>
@@ -401,7 +401,7 @@
             <!-- <hack_baseline>0.07</hack_baseline> -->
           </plugin>
       </sensor>
-    </link>    
+    </link>
 
     <joint name="base_joint" type="fixed">
       <parent>base_footprint</parent>
@@ -444,7 +444,7 @@
       <axis>
         <xyz>0 0 1</xyz>
       </axis>
-    </joint>    
+    </joint>
 
     <joint name="lidar_joint" type="fixed">
       <parent>base_link</parent>
@@ -514,7 +514,22 @@
       <update_rate>30</update_rate>
       <joint_name>wheel_left_joint</joint_name>
       <joint_name>wheel_right_joint</joint_name>
-    </plugin>    
+    </plugin>
+
+    <!-- publish ground truth -->
+    <plugin name="turtlebot3_ground_truth" filename="libgazebo_ros_p3d.so">
+        <always_on>true</always_on>
+        <update_rate>50.0</update_rate>
+        <body_name>base_footprint</body_name>
+        <gaussian_noise>0.001</gaussian_noise>
+        <frame_name>world</frame_name>
+        <xyz_offset>0 0 0</xyz_offset>
+        <rpy_offset>0 0 0</rpy_offset>
+        <ros>
+          <!--https://github.com/ros-simulation/gazebo_ros_pkgs/issues/1318-->
+          <remapping>odom:=ground_truth</remapping>
+        </ros>
+    </plugin>
 
   </model>
 </sdf>