Add odometry to MecanumDrive

glpuga · Aug 3, 2024 · f676237 · f676237
1 parent 9891844
commit f676237
Show file tree

Hide file tree

Showing 7 changed files with 2,106 additions and 6 deletions.
diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc
@@ -26,7 +26,7 @@
 #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>
@@ -71,6 +71,13 @@ class ignition::gazebo::systems::MecanumDrivePrivate
   public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info,
     const ignition::gazebo::EntityComponentManager &_ecm);
 
+  /// \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 Ignition communication node.
   public: transport::Node node;
 
@@ -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 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.
@@ -282,7 +289,8 @@ void MecanumDrive::Configure(const Entity &_entity,
 
   // 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;
@@ -489,6 +497,40 @@ void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
       *vel = components::JointVelocityCmd({this->dataPtr->backRightJointSpeed});
     }
   }
+
+  // Create the left and right side 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,7 @@ void MecanumDrive::PostUpdate(const UpdateInfo &_info,
     return;
 
   this->dataPtr->UpdateVelocity(_info, _ecm);
+  this->dataPtr->UpdateOdometry(_info, _ecm);
 }
 
 //////////////////////////////////////////////////
@@ -552,6 +595,112 @@ void MecanumDrivePrivate::UpdateVelocity(
     (linVel - latVel + angVel * angularLength) * invWheelRadius;
 }
 
+
+//////////////////////////////////////////////////
+void MecanumDrivePrivate::UpdateOdometry(const UpdateInfo &_info,
+    const EntityComponentManager &_ecm)
+{
+  IGN_PROFILE("MecanumnDrive::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 the left and right side.
+  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_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;
+  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::OnCmdVel(const msgs::Twist &_msg)
 {

diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
@@ -43,6 +43,7 @@ set(tests
   logical_camera_system.cc
   logical_audio_sensor_plugin.cc
   magnetometer_system.cc
+  mecanum_drive_system.cc
   model.cc
   model_photo_shoot_default_joints.cc
   model_photo_shoot_random_joints.cc