From 80688cc286634c3924fde7fd62126d94699b2a84 Mon Sep 17 00:00:00 2001
From: Jari Nippula <jarix@ssrc.tii.ae>
Date: Thu, 5 Sep 2024 18:22:09 +0300
Subject: [PATCH] Parse motor plugins from model

Iterate all plugins in the model and find motor specific ones.

If DiffDrive plugin is found, define model as Rover

If MulticopterMotorModel plugins are found, read motorNumber
and maxRotVelocity for motor velocity scailing
---
 include/gazebo_mavlink_interface.h |  4 +++
 src/gazebo_mavlink_interface.cpp   | 55 ++++++++++++++++++++++++++++++
 2 files changed, 59 insertions(+)

diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h
index 978ac4a..7f0cb19 100644
--- a/include/gazebo_mavlink_interface.h
+++ b/include/gazebo_mavlink_interface.h
@@ -160,6 +160,7 @@ namespace mavlink_interface
       float AddSimpleNoise(float value, float mean, float stddev);
       void RotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED,
         const gz::math::Quaterniond q_FLU_to_ENU);
+      void ParseMulticopterMotorModelPlugins(const std::string &sdfFilePath);
 
       static const unsigned n_out_max = 16;
 
@@ -172,6 +173,9 @@ namespace mavlink_interface
       int servo_input_index_[n_out_max];
       bool input_is_cmd_vel_{false};
 
+      int motor_vel_scalings_[n_out_max] {1};
+      bool is_rover_{false};
+
       /// \brief gz communication node and publishers.
       gz::transport::Node node;
       gz::transport::Node::Publisher servo_control_pub_[n_out_max];
diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp
index 55b1358..9650a40 100644
--- a/src/gazebo_mavlink_interface.cpp
+++ b/src/gazebo_mavlink_interface.cpp
@@ -51,6 +51,58 @@ GazeboMavlinkInterface::~GazeboMavlinkInterface() {
   mavlink_interface_->close();
 }
 
+void GazeboMavlinkInterface::ParseMulticopterMotorModelPlugins(const std::string &sdfFilePath)
+{
+  // Load the SDF file
+  sdf::Root root;
+  sdf::Errors errors = root.Load(sdfFilePath);
+  if (!errors.empty())
+  {
+    for (const auto &error : errors)
+    {
+      gzerr << "Error: " << error.Message() << std::endl;
+    }
+    return;
+  }
+
+  const sdf::Model *model = root.Model();
+  if (!model)
+  {
+    gzerr << "No models found in SDF file." << std::endl;
+    return;
+  }
+
+  // Iterate through all plugins in the model
+  for (const sdf::Plugin plugin : model->Plugins())
+  {
+    // Check if the plugin is a MulticopterMotorModel
+    if (plugin.Name() == "gz::sim::systems::DiffDrive") {
+      is_rover_ = true;
+      gzmsg << "Detected rover model" << std::endl;
+    } else if (plugin.Name() == "gz::sim::systems::MulticopterMotorModel") {
+      // Extract and print parameters
+      if (plugin.Element()->HasElement("motorNumber"))
+      {
+        int motorNumber = plugin.Element()->Get<int>("motorNumber");
+        if (motorNumber >= n_out_max)
+        {
+          gzerr << "Motor number " << motorNumber << " exceeds maximum number of motors " << n_out_max << std::endl;
+          continue;
+        }
+        int maxRotVelocityInt = 1;
+        if (plugin.Element()->HasElement("motorNumber"))
+        {
+          double maxRotVelocity = plugin.Element()->Get<double>("maxRotVelocity");
+          maxRotVelocityInt = (int)maxRotVelocity;
+          motor_vel_scalings_[motorNumber] = maxRotVelocityInt;
+          gzmsg << "  Motor[" << motorNumber << "] " << maxRotVelocityInt << std::endl;
+        }
+      }
+    }
+  }
+}
+
+
 void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,
       const std::shared_ptr<const sdf::Element> &_sdf,
       gz::sim::EntityComponentManager &_ecm,
@@ -219,6 +271,9 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,
 
   std::default_random_engine rnd_gen_;
 
+  gzmsg << "Try print motor plugins" << std::endl;
+  ParseMulticopterMotorModelPlugins(model_.SourceFilePath(_ecm));
+
   if (hostptr_ || mavlink_hostname_str_.empty()) {
     gzmsg << "--> load mavlink_interface_" << std::endl;
     mavlink_interface_->Load();