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
tiiuae · Sep 5, 2024 · f3ce874 · f3ce874
1 parent 7ae6aea
commit f3ce874
Show file tree

Hide file tree

Showing 2 changed files with 59 additions and 0 deletions.
diff --git 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
@@ -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] = plugin.Element()->Get<int>("motorVelocityScaling");
+        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();