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 <[email protected]>
Signed-off-by: Morgantiz <[email protected]>

Author: Morgantiz

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,16 +183,36 @@ 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>, "
       << "defaults to \"" << this->dataPtr->robotBaseFrame << "\"" << std::endl;
   }
   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);