Improves odometry. (#47)

Signed-off-by: Franco Cipollone <[email protected]>

andino_gz/config/bridge_config.yaml

@@ -8,8 +8,9 @@
   ros_type_name: "nav_msgs/msg/Odometry"
   gz_type_name: "gz.msgs.Odometry"
   direction: GZ_TO_ROS
+# odom <-> base_link transform
 - ros_topic_name: "/tf"
-  gz_topic_name: "/model/andino/tf"
+  gz_topic_name: "/model/andino/pose"
   ros_type_name: "tf2_msgs/msg/TFMessage"
   gz_type_name: "gz.msgs.Pose_V"
   direction: GZ_TO_ROS

andino_gz/urdf/andino_gz.urdf.xacro

@@ -10,13 +10,31 @@
       <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>
+      <wheel_radius>0.033</wheel_radius>
+      <!--
+      Odometry in the diff drive plugin is quite bad, therefore 
+      a dedicated OdometryPublisher plugin is being used.
+      As it can not be disabled we publish odom info to an unused topic
+      -->
+      <odom_topic>/unused/diff_drive_odometry</odom_topic>
+      <tf_topic>/unused/diff_drive_tf</tf_topic>
       <odom_publish_frequency>1</odom_publish_frequency>
-      <frame_id>odom</frame_id>
+      <frame_id>odom_diff_drive</frame_id>
       <child_frame_id>base_link</child_frame_id>
     </plugin>
   </gazebo>
 
+  <!-- OdometryPublisher being used instead of Diff Drive's odometry -->
+  <gazebo>
+    <plugin
+      filename="ignition-gazebo-odometry-publisher-system"
+      name="ignition::gazebo::systems::OdometryPublisher">
+      <odom_publish_frequency>20</odom_publish_frequency>
+      <robot_base_frame>base_link</robot_base_frame>
+      <odom_frame>odom</odom_frame>
+    </plugin>
+  </gazebo>
+
   <gazebo>
     <plugin
       filename="ignition-gazebo-joint-state-publisher-system"