Adding MecanumDrive plugin with Odom and Tf, with Tests.

src/systems/mecanum_drive/MecanumDrive.cc

@@ -21,14 +21,15 @@
 #include <gz/msgs/pose_v.pb.h>
 #include <gz/msgs/twist.pb.h>
 
+#include <chrono>
 #include <limits>
 #include <mutex>
 #include <set>
 #include <string>
 #include <vector>
 
 #include <gz/common/Profiler.hh>
-#include <gz/math/DiffDriveOdometry.hh>
+#include <gz/math/MecanumDriveOdometry.hh>
 #include <gz/math/Quaternion.hh>
 #include <gz/math/SpeedLimiter.hh>
 #include <gz/plugin/Register.hh>
@@ -66,12 +67,19 @@
   /// \param[in] _msg Velocity message
   public: void OnCmdVel(const gz::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 UpdateInfo &_info,
+    const EntityComponentManager &_ecm);
+
   /// \brief Update the linear and angular velocities.
   /// \param[in] _info System update information.
   /// \param[in] _ecm The EntityComponentManager of the given simulation
   /// instance.
-  public: void UpdateVelocity(const gz::sim::UpdateInfo &_info,
-    const gz::sim::EntityComponentManager &_ecm);
+  public: void UpdateVelocity(const UpdateInfo &_info,
+    const EntityComponentManager &_ecm);
 
   /// \brief Gazebo communication node.
   public: transport::Node node;
@@ -133,13 +141,13 @@
   /// \brief Last sim time odom was published.
   public: std::chrono::steady_clock::duration lastOdomPubTime{0};
 
-  /// \brief Diff drive odometry.
-  public: math::DiffDriveOdometry odom;
+  /// \brief Mecanum drive odometry.
+  public: math::MecanumDriveOdometry odom;
 
-  /// \brief Diff drive odometry message publisher.
+  /// \brief Mecanum drive odometry message publisher.
   public: transport::Node::Publisher odomPub;
 
-  /// \brief Diff drive tf message publisher.
+  /// \brief Mecanum drive tf message publisher.
   public: transport::Node::Publisher tfPub;
 
   /// \brief Linear velocity limiter.
@@ -168,8 +176,7 @@
 };
 
 //////////////////////////////////////////////////
-MecanumDrive::MecanumDrive()
-  : dataPtr(std::make_unique<MecanumDrivePrivate>())
+MecanumDrive::MecanumDrive() : dataPtr(std::make_unique<MecanumDrivePrivate>())
 {
 }
 
@@ -280,7 +287,9 @@
 
   // Setup odometry.
   this->dataPtr->odom.SetWheelParams(this->dataPtr->wheelSeparation,
-      this->dataPtr->wheelRadius, this->dataPtr->wheelRadius);
+      this->dataPtr->wheelbase,
+      this->dataPtr->wheelRadius,
+      this->dataPtr->wheelRadius);
 
   // Subscribe to commands
   std::vector<std::string> topics;
@@ -319,13 +328,16 @@
   if (_sdf->HasElement("child_frame_id"))
     this->dataPtr->sdfChildFrameId = _sdf->Get<std::string>("child_frame_id");
 
+  gzmsg << "MecanumDrive publishing odom messages on [" << odomTopic << "]"
+         << std::endl;
+
   gzmsg << "MecanumDrive subscribing to twist messages on [" << topic << "]"
          << std::endl;
 }
 
 //////////////////////////////////////////////////
-void MecanumDrive::PreUpdate(const gz::sim::UpdateInfo &_info,
-    gz::sim::EntityComponentManager &_ecm)
+void MecanumDrive::PreUpdate(const UpdateInfo &_info,
+    EntityComponentManager &_ecm)
 {
   GZ_PROFILE("MecanumDrive::PreUpdate");
 
@@ -450,6 +462,40 @@
     _ecm.SetComponentData<components::JointVelocityCmd>(joint,
       {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());
+  }
+
 }
 
 //////////////////////////////////////////////////
@@ -462,12 +508,121 @@
     return;
 
   this->dataPtr->UpdateVelocity(_info, _ecm);
+  this->dataPtr->UpdateOdometry(_info, _ecm);
+}
+
+//////////////////////////////////////////////////
+void MecanumDrivePrivate::UpdateOdometry(const UpdateInfo &_info,
+    const EntityComponentManager &_ecm)
+{
+  GZ_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;
+  gz::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);
 }
 
 //////////////////////////////////////////////////
 void MecanumDrivePrivate::UpdateVelocity(
-    const gz::sim::UpdateInfo &_info,
-    const gz::sim::EntityComponentManager &/*_ecm*/)
+    const UpdateInfo &_info,
+    const EntityComponentManager &/*_ecm*/)
 {
   GZ_PROFILE("MecanumDrive::UpdateVelocity");
 

test/integration/CMakeLists.txt

@@ -50,6 +50,7 @@ set(tests
   logical_audio_sensor_plugin.cc
   magnetometer_system.cc
   material.cc
+  mecanum_drive_system.cc
   mesh_inertia_calculation.cc
   model.cc
   model_photo_shoot_default_joints.cc