Adding MecanumDrive plugin with Odom and Tf, with Tests.

.vscode/c_cpp_properties.json

@@ -0,0 +1,21 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/ros/rolling/include/**",
+        "/home/stefanomutti/Libs/gz-sim/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,8 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/opt/ros/rolling/lib/python3.10/site-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/rolling/lib/python3.10/site-packages"
+    ]
+}

log/latest

@@ -0,0 +1 @@
+latest_list

log/latest_list

@@ -0,0 +1 @@
+list_2024-03-27_10-32-07

log/list_2024-03-27_10-32-07/logger_all.log

@@ -0,0 +1,22 @@
+[0.113s] DEBUG:colcon:Command line arguments: ['/home/stefanomutti/.local/bin/colcon', 'list', '-p', '--base-paths', '/home/stefanomutti/Libs/gz-sim']
+[0.113s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='list', build_base='build', ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['/home/stefanomutti/Libs/gz-sim'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], topological_order=False, names_only=False, paths_only=True, topological_graph=False, topological_graph_dot=False, topological_graph_density=False, topological_graph_legend=False, topological_graph_dot_cluster=False, topological_graph_dot_include_skipped=False, mixin_files=None, mixin=None, verb_parser=<colcon_mixin.mixin.mixin_argument.MixinArgumentDecorator object at 0x76fd93d34400>, verb_extension=<colcon_package_information.verb.list.ListVerb object at 0x76fd93e94be0>, main=<bound method ListVerb.main of <colcon_package_information.verb.list.ListVerb object at 0x76fd93e94be0>>, mixin_verb=('list',))
+[0.133s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters
+[0.133s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters
+[0.134s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters
+[0.134s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters
+[0.134s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover
+[0.134s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/stefanomutti/Libs/gz-sim'
+[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ignore', 'ignore_ament_install']
+[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore'
+[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore_ament_install'
+[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_pkg']
+[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_pkg'
+[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_meta']
+[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_meta'
+[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ros']
+[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ros'
+[0.147s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['cmake', 'python']
+[0.147s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'cmake'
+[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'python'
+[0.165s] DEBUG:colcon.colcon_core.package_identification:Package '/home/stefanomutti/Libs/gz-sim' with type 'cmake' and name 'gz-sim8'
+[0.165s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults

src/systems/mecanum_drive/MecanumDrive.cc

@@ -28,7 +28,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>
@@ -66,12 +66,19 @@ class gz::sim::systems::MecanumDrivePrivate
   /// \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 +140,13 @@ class gz::sim::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.
@@ -280,7 +287,7 @@ 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;
@@ -319,13 +326,16 @@ void MecanumDrive::Configure(const Entity &_entity,
   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 +460,40 @@ void MecanumDrive::PreUpdate(const gz::sim::UpdateInfo &_info,
     _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 +506,110 @@ void MecanumDrive::PostUpdate(const UpdateInfo &_info,
     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