From 3aec1568f96d15ac53f52363780f630ed3334d8f Mon Sep 17 00:00:00 2001
From: Carlo Morganti <60438336+Morgantiz@users.noreply.github.com>
Date: Fri, 28 Feb 2025 12:52:56 +0100
Subject: [PATCH 1/2] Upgrades OdometryPublisher system plugin

Allows OdometryPublisher to publish odometry for a non-fixed link of the robot model. Introduces the possibility to publish the odometry of a point after a non-fixed joint.
This commit does not change the default behavior of the plugin.

Signed-off-by: Carlo Morganti <60438336+Morgantiz@users.noreply.github.com>
Signed-off-by: Morgantiz <carloski@live.it>
---
 .../odometry_publisher/OdometryPublisher.cc   | 33 ++++++++++++++++---
 1 file changed, 28 insertions(+), 5 deletions(-)

diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc
index fe5270ebe2..1c2cdb7d69 100644
--- a/src/systems/odometry_publisher/OdometryPublisher.cc
+++ b/src/systems/odometry_publisher/OdometryPublisher.cc
@@ -66,6 +66,9 @@ class gz::sim::systems::OdometryPublisherPrivate
   /// \brief Name of the world-fixed coordinate frame for the odometry message.
   public: std::string odomFrame;
 
+  /// \brief Name of the frame used as child frame id in the odometry message 
+  public: std::string odomChildFrame;
+
   /// \brief Name of the coordinate frame rigidly attached to the mobile
   /// robot base.
   public: std::string robotBaseFrame;
@@ -180,8 +183,7 @@ void OdometryPublisher::Configure(const Entity &_entity,
     this->dataPtr->gaussianNoise = _sdf->Get<double>("gaussian_noise");
   }
 
-  this->dataPtr->robotBaseFrame = this->dataPtr->model.Name(_ecm)
-    + "/" + "base_footprint";
+  this->dataPtr->robotBaseFrame = Link(this->dataPtr->model.Links(_ecm).at(0)).Name(_ecm).value();
   if (!_sdf->HasElement("robot_base_frame"))
   {
     gzdbg << "OdometryPublisher system plugin missing <robot_base_frame>, "
@@ -189,7 +191,28 @@ void OdometryPublisher::Configure(const Entity &_entity,
   }
   else
   {
-    this->dataPtr->robotBaseFrame = _sdf->Get<std::string>("robot_base_frame");
+    const std::string name = _sdf->Get<std::string>("robot_base_frame");
+    if (this->dataPtr->model.LinkByName(_ecm, name) != kNullEntity)
+    {
+      this->dataPtr->robotBaseFrame = name;
+    }
+    else
+    {
+      gzerr << " OdometryPublisher system plugin <robot_base_frame> link not found in the model, "
+            << "defaults to \"" << this->dataPtr->robotBaseFrame << "\"" << std::endl;
+    }
+  }
+
+  this->dataPtr->odomChildFrame = this->dataPtr->model.Name(_ecm)
+    + "/" + this->dataPtr->robotBaseFrame;
+  if (!_sdf->HasElement("odom_child_frame"))
+  {
+    gzdbg << "OdometryPublisher system plugin missing <odom_child_frame>, "
+      << "defaults to \"" << this->dataPtr->odomChildFrame << "\"" << std::endl;
+  }
+  else
+  {
+    this->dataPtr->odomChildFrame = _sdf->Get<std::string>("odom_child_frame");
   }
 
   this->dataPtr->dimensions = 2;
@@ -357,7 +380,7 @@ void OdometryPublisherPrivate::UpdateOdometry(
 
   // Get and set robotBaseFrame to odom transformation.
   //! [worldPose]
-  const math::Pose3d rawPose = worldPose(this->model.Entity(), _ecm);
+  const math::Pose3d rawPose = worldPose(this->model.LinkByName(_ecm, this->robotBaseFrame), _ecm);
   //! [worldPose]
   //! [setPoseMsg]
   math::Pose3d pose = rawPose * this->offset;
@@ -462,7 +485,7 @@ void OdometryPublisherPrivate::UpdateOdometry(
   frame->add_value(odomFrame);
   auto childFrame = header.add_data();
   childFrame->set_key("child_frame_id");
-  childFrame->add_value(robotBaseFrame);
+  childFrame->add_value(odomChildFrame);
 
   msg.mutable_header()->CopyFrom(header);
 

From 240ee25b4274fc49f2450da69fccc765f82c165d Mon Sep 17 00:00:00 2001
From: Morgantiz <carloski@live.it>
Date: Sat, 22 Mar 2025 11:44:19 +0100
Subject: [PATCH 2/2] fix: adds Link library include

Signed-off-by: Morgantiz <carloski@live.it>
---
 src/systems/odometry_publisher/OdometryPublisher.cc | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc
index 1c2cdb7d69..a49423821d 100644
--- a/src/systems/odometry_publisher/OdometryPublisher.cc
+++ b/src/systems/odometry_publisher/OdometryPublisher.cc
@@ -40,6 +40,7 @@
 #include "gz/sim/components/Pose.hh"
 #include "gz/sim/components/JointPosition.hh"
 #include "gz/sim/Model.hh"
+#include "gz/sim/Link.hh"
 #include "gz/sim/Util.hh"
 
 using namespace gz;