From 34f20ca437746547d0c31e8c14c6f16d2eddf994 Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Wed, 24 Apr 2024 15:36:18 -0300
Subject: [PATCH] Fixes bug with odometry.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 andino_gz/config/bridge_config.yaml | 10 +++++-----
 andino_gz/urdf/andino_gz.urdf.xacro |  7 +++----
 andino_gz/worlds/depot.sdf          |  2 +-
 andino_gz/worlds/empty.sdf          |  2 +-
 4 files changed, 10 insertions(+), 11 deletions(-)

diff --git a/andino_gz/config/bridge_config.yaml b/andino_gz/config/bridge_config.yaml
index 4949a7a..c845568 100644
--- a/andino_gz/config/bridge_config.yaml
+++ b/andino_gz/config/bridge_config.yaml
@@ -4,7 +4,7 @@
   gz_type_name: "gz.msgs.Clock"
   direction: GZ_TO_ROS
 - ros_topic_name: "/odom"
-  gz_topic_name: "/odom"
+  gz_topic_name: "/model/andino/odometry"
   ros_type_name: "nav_msgs/msg/Odometry"
   gz_type_name: "gz.msgs.Odometry"
   direction: GZ_TO_ROS
@@ -14,22 +14,22 @@
   gz_type_name: "gz.msgs.Pose_V"
   direction: GZ_TO_ROS
 - ros_topic_name: "/camera_info"
-  gz_topic_name: "/camera_info"
+  gz_topic_name: "/world/gazebo_world/model/andino/link/base_link/sensor/camera/camera_info"
   ros_type_name: "sensor_msgs/msg/CameraInfo"
   gz_type_name: "gz.msgs.CameraInfo"
   direction: GZ_TO_ROS
 - ros_topic_name: "/image_raw"
-  gz_topic_name: "/image_raw"
+  gz_topic_name: "/world/gazebo_world/model/andino/link/base_link/sensor/camera/image"
   ros_type_name: "sensor_msgs/msg/Image"
   gz_type_name: "gz.msgs.Image"
   direction: GZ_TO_ROS
 - ros_topic_name: "/scan"
-  gz_topic_name: "/scan"
+  gz_topic_name: "/world/gazebo_world/model/andino/link/base_link/sensor/sensor_ray_front/scan"
   ros_type_name: "sensor_msgs/msg/LaserScan"
   gz_type_name: "gz.msgs.LaserScan"
   direction: GZ_TO_ROS
 - ros_topic_name: "/scan/points"
-  gz_topic_name: "/scan/points"
+  gz_topic_name: "/world/gazebo_world/model/andino/link/base_link/sensor/sensor_ray_front/scan/points"
   ros_type_name: "sensor_msgs/msg/PointCloud2"
   gz_type_name: "gz.msgs.PointCloudPacked"
   direction: GZ_TO_ROS
diff --git a/andino_gz/urdf/andino_gz.urdf.xacro b/andino_gz/urdf/andino_gz.urdf.xacro
index 4f076b1..10e153f 100644
--- a/andino_gz/urdf/andino_gz.urdf.xacro
+++ b/andino_gz/urdf/andino_gz.urdf.xacro
@@ -6,12 +6,14 @@
   <gazebo>
     <plugin
       filename="ignition-gazebo-diff-drive-system"
-      name="ignition::gazebo::systems::DiffDrive"> 
+      name="ignition::gazebo::systems::DiffDrive">
       <left_joint>left_wheel_joint</left_joint>
       <right_joint>right_wheel_joint</right_joint>
       <wheel_separation>0.137</wheel_separation>
       <wheel_radius>0.0175</wheel_radius>
       <odom_publish_frequency>1</odom_publish_frequency>
+      <frame_id>odom</frame_id>
+      <child_frame_id>base_link</child_frame_id>
     </plugin>
   </gazebo>
 
@@ -37,7 +39,6 @@
         <stddev>0.01</stddev>
       </noise>
       </ray>
-      <topic>scan</topic>
       <always_on>true</always_on>
       <visualize>true</visualize>
       <update_rate>20.0</update_rate>
@@ -70,8 +71,6 @@
       <always_on>1</always_on>
       <update_rate>30</update_rate>
       <visualize>true</visualize>
-      <topic>image_raw</topic>
-      <camera_info_topic>camera_info</camera_info_topic>
     </sensor>
   </gazebo>
 
diff --git a/andino_gz/worlds/depot.sdf b/andino_gz/worlds/depot.sdf
index 9fb6a9a..3b2cf4a 100644
--- a/andino_gz/worlds/depot.sdf
+++ b/andino_gz/worlds/depot.sdf
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='ASCII'?>
 <sdf version='1.7'>
 <!-- Model taken from https://app.gazebosim.org/Fahadalbahoth/fuel/worlds/tugbot_depot -->
-  <world name='world_demo'>
+  <world name='gazebo_world'>
     <physics name='1ms' type='ignored'>
       <max_step_size>0.01</max_step_size>
       <real_time_factor>1</real_time_factor>
diff --git a/andino_gz/worlds/empty.sdf b/andino_gz/worlds/empty.sdf
index c5c3118..a89e8e1 100644
--- a/andino_gz/worlds/empty.sdf
+++ b/andino_gz/worlds/empty.sdf
@@ -1,6 +1,6 @@
 <?xml version='1.0' encoding='ASCII'?>
 <sdf version='1.7'>
-  <world name='empty'>
+  <world name='gazebo_world'>
     <physics name='1ms' type='ignored'>
       <max_step_size>0.01</max_step_size>
       <real_time_factor>1</real_time_factor>