From 9419363edd30a20892b5192b0971268eba6e7ac0 Mon Sep 17 00:00:00 2001
From: Henrique Scharlau Coelho <henriquescharlaucoelho@gmail.com>
Date: Mon, 18 Dec 2023 22:12:19 -0300
Subject: [PATCH] added "fake_odom"(ground truth with noise)

---
 twil_2dnav/launch/nav2_navigator.launch.xml | 2 +-
 twil_description/xacro/twil.urdf.xacro      | 9 +++++++++
 2 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/twil_2dnav/launch/nav2_navigator.launch.xml b/twil_2dnav/launch/nav2_navigator.launch.xml
index 88174cc..6920046 100644
--- a/twil_2dnav/launch/nav2_navigator.launch.xml
+++ b/twil_2dnav/launch/nav2_navigator.launch.xml
@@ -52,7 +52,7 @@
 
     <node pkg="robot_localization" exec="ekf_node" name="ekf_filter_node">
         <param name="use_sim_time" value="$(var use_sim_time)"/>
-        <param name="odom0" value="/ground_truth"/>
+        <param name="odom0" value="/fake_odom"/>
         <param from="$(find-pkg-share twil_2dnav)/config/ekf_params.yaml"/>
     </node>
 
diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro
index f22efb3..a0d9d3c 100644
--- a/twil_description/xacro/twil.urdf.xacro
+++ b/twil_description/xacro/twil.urdf.xacro
@@ -174,6 +174,15 @@
 		<!--update_rate>100</update_rate-->
 	</plugin>
 
+	<plugin name="fake_odom" filename="libgazebo_ros_p3d.so">
+		<frame_name>map</frame_name>
+		<body_name>twil_origin</body_name>
+        <gaussian_noise>0.2</gaussian_noise>
+		<ros>
+			<remapping>odom:=fake_odom</remapping>
+		</ros>
+	</plugin>
+
   </gazebo>
 
   <xacro:if value="$(arg gps)">
-- 
2.12.0