MecanumDrive odometry and tf functions implementation

src/systems/mecanum_drive/MecanumDrive.cc

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2021 Open Source Robotics Foundation
+ * Copyright (C) 2022 Open Source Robotics Foundation
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -64,6 +64,13 @@ class ignition::gazebo::systems::MecanumDrivePrivate
   /// \param[in] _msg Velocity message
   public: void OnCmdVel(const ignition::msgs::Twist &_msg);
 
+  /// \brief Update odometry and publish an odometry message.
+  /// \param[in] _info System update information.
+  /// \param[in] _ecm The EntityComponentManager of the given simulation
+  /// instance.
+  public: void UpdateOdometry(const ignition::gazebo::UpdateInfo &_info,
+    const ignition::gazebo::EntityComponentManager &_ecm);
+
   /// \brief Update the linear and angular velocities.
   /// \param[in] _info System update information.
   /// \param[in] _ecm The EntityComponentManager of the given simulation
@@ -131,13 +138,13 @@ class ignition::gazebo::systems::MecanumDrivePrivate
   /// \brief Last sim time odom was published.
   public: std::chrono::steady_clock::duration lastOdomPubTime{0};
 
-  /// \brief Diff drive odometry.
-  public: math::DiffDriveOdometry odom;
+  /// \brief Mecanumn drive odometry.
+  public: math::MecanumDriveOdometry odom;
 
-  /// \brief Diff drive odometry message publisher.
+  /// \brief Mecanumn drive odometry message publisher.
   public: transport::Node::Publisher odomPub;
 
-  /// \brief Diff drive tf message publisher.
+  /// \brief Mecanumn drive tf message publisher.
   public: transport::Node::Publisher tfPub;
 
   /// \brief Linear velocity limiter.
@@ -281,8 +288,10 @@ void MecanumDrive::Configure(const Entity &_entity,
   }
 
   // Setup odometry.
-  this->dataPtr->odom.SetWheelParams(this->dataPtr->wheelSeparation,
-      this->dataPtr->wheelRadius, this->dataPtr->wheelRadius);
+  this->dataPtr->odom.SetWheelParams(this->dataPtr->wheelSeparation, 
+      this->dataPtr->wheelbase,  
+      this->dataPtr->wheelRadius, 
+      this->dataPtr->wheelRadius);
 
   // Subscribe to commands
   std::vector<std::string> topics;
@@ -489,6 +498,39 @@ void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
       *vel = components::JointVelocityCmd({this->dataPtr->backRightJointSpeed});
     }
   }
+
+  // Create the joint position components if they don't exist.
+  auto frontLeftPos = _ecm.Component<components::JointPosition>(
+      this->dataPtr->frontLeftJoints[0]);
+  if (!frontLeftPos && _ecm.HasEntity(this->dataPtr->frontLeftJoints[0]))
+  {
+    _ecm.CreateComponent(this->dataPtr->frontLeftJoints[0],
+        components::JointPosition());
+  }
+
+  auto frontRightPos = _ecm.Component<components::JointPosition>(
+      this->dataPtr->frontRightJoints[0]);
+  if (!frontRightPos && _ecm.HasEntity(this->dataPtr->frontRightJoints[0]))
+  {
+    _ecm.CreateComponent(this->dataPtr->frontRightJoints[0],
+        components::JointPosition());
+  }
+
+  auto backLeftPos = _ecm.Component<components::JointPosition>(
+      this->dataPtr->backLeftJoints[0]);
+  if (!backLeftPos && _ecm.HasEntity(this->dataPtr->backLeftJoints[0]))
+  {
+    _ecm.CreateComponent(this->dataPtr->backLeftJoints[0],
+        components::JointPosition());
+  }
+
+  auto backRightPos = _ecm.Component<components::JointPosition>(
+      this->dataPtr->backRightJoints[0]);
+  if (!backRightPos && _ecm.HasEntity(this->dataPtr->backRightJoints[0]))
+  {
+    _ecm.CreateComponent(this->dataPtr->backRightJoints[0],
+        components::JointPosition());
+  }
 }
 
 //////////////////////////////////////////////////
@@ -501,6 +543,105 @@ void MecanumDrive::PostUpdate(const UpdateInfo &_info,
     return;
 
   this->dataPtr->UpdateVelocity(_info, _ecm);
+  this->dataPtr->UpdateOdometry(_info, _ecm);
+}
+
+//////////////////////////////////////////////////
+void MecanumDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info,
+    const ignition::gazebo::EntityComponentManager &_ecm)
+{
+  IGN_PROFILE("MecanumDrive::UpdateOdometry");
+  // Initialize, if not already initialized.
+  if (!this->odom.Initialized())
+  {
+    this->odom.Init(std::chrono::steady_clock::time_point(_info.simTime));
+    return;
+  }
+
+  if (this->frontLeftJoints.empty() || this->frontRightJoints.empty() || this->backLeftJoints.empty() || this->backRightJoints.empty())
+    return;
+
+  // Get the first joint positions for each wheel joint.
+  auto frontLeftPos = _ecm.Component<components::JointPosition>(this->frontLeftJoints[0]);
+  auto frontRightPos = _ecm.Component<components::JointPosition>(this->frontRightJoints[0]);
+  auto backLeftPos = _ecm.Component<components::JointPosition>(this->backLeftJoints[0]);
+  auto backRightPos = _ecm.Component<components::JointPosition>(this->backRightJoints[0]);
+
+  // Abort if the joints were not found or just created.
+  if (!frontLeftPos || !frontRightPos || !backLeftPos || !backRightPos ||
+   frontLeftPos->Data().empty() || frontRightPos->Data().empty() || backLeftPos->Data().empty() || backRightPos->Data().empty())
+  {
+    return;
+  }
+
+  this->odom.Update(frontLeftPos->Data()[0], frontRightPos->Data()[0], backLeftPos->Data()[0], backRightPos->Data()[0],
+      std::chrono::steady_clock::time_point(_info.simTime));  
+
+  // Throttle publishing
+  auto diff = _info.simTime - this->lastOdomPubTime;
+  if (diff > std::chrono::steady_clock::duration::zero() &&
+      diff < this->odomPubPeriod)
+  {
+    return;
+  }
+  this->lastOdomPubTime = _info.simTime;
+
+  // Construct the odometry message and publish it.
+  msgs::Odometry msg;
+  msg.mutable_pose()->mutable_position()->set_x(this->odom.X());
+  msg.mutable_pose()->mutable_position()->set_y(this->odom.Y());
+
+  math::Quaterniond orientation(0, 0, *this->odom.Heading());
+  msgs::Set(msg.mutable_pose()->mutable_orientation(), orientation);
+
+  msg.mutable_twist()->mutable_linear()->set_x(this->odom.LinearVelocity());
+  msg.mutable_twist()->mutable_linear()->set_y(this->odom.LateralVelocity());
+  msg.mutable_twist()->mutable_angular()->set_z(*this->odom.AngularVelocity());
+
+  // Set the time stamp in the header
+  msg.mutable_header()->mutable_stamp()->CopyFrom(
+      convert<msgs::Time>(_info.simTime));
+
+  // Set the frame id.
+  auto frame = msg.mutable_header()->add_data();
+  frame->set_key("frame_id");
+  if (this->sdfFrameId.empty())
+  {
+    frame->add_value(this->model.Name(_ecm) + "/odom");
+  }
+  else
+  {
+    frame->add_value(this->sdfFrameId);
+  }
+
+  std::optional<std::string> linkName = this->canonicalLink.Name(_ecm);
+  if (this->sdfChildFrameId.empty())
+  {
+    if (linkName)
+    {
+      auto childFrame = msg.mutable_header()->add_data();
+      childFrame->set_key("child_frame_id");
+      childFrame->add_value(this->model.Name(_ecm) + "/" + *linkName);
+    }
+  }
+  else
+  {
+    auto childFrame = msg.mutable_header()->add_data();
+    childFrame->set_key("child_frame_id");
+    childFrame->add_value(this->sdfChildFrameId);
+  }
+
+  // Construct the Pose_V/tf message and publish it.
+  msgs::Pose_V tfMsg;
+  ignition::msgs::Pose *tfMsgPose = tfMsg.add_pose();
+  tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header());
+  tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position());
+  tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation());
+
+  // Publish the messages
+  this->odomPub.Publish(msg);
+  this->tfPub.Publish(tfMsg);
+
 }
 
 //////////////////////////////////////////////////

src/systems/mecanum_drive/MecanumDrive.hh

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2021 Open Source Robotics Foundation
+ * Copyright (C) 2022 Open Source Robotics Foundation
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.