Как изменить плагин Gazebo, чтобы он работал не только с мировыми файлами, но и с файлами URDF?

Question

Я сделал простой плагин Gazebo, который вращает объект со скоростью, которую он читает из топи c. Плагин работает при вставке в файл мира. Однако вставка того же плагина в файл URDF не имеет никакого эффекта. Как мне изменить плагин, чтобы он работал и с моим файлом URDF?

Вот мой плагин:

#include <functional>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>
#include <ignition/math/Vector3.hh>
#include <thread>
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>

namespace gazebo
{
  class PropSpin : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
    {
      // Store the pointer to the model
      this->model = _parent;

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
          std::bind(&PropSpin::OnUpdate, this));

      // Create a topic name
      std::string prop_vel_topic = "/prop_vel";

      // Initialize ros, if it has not already bee initialized.
      if (!ros::isInitialized())
      {
        int argc = 0;
        char **argv = NULL;
        ros::init(argc, argv, "earthquake_rosnode",
            ros::init_options::NoSigintHandler);
      }

      // Create our ROS node. This acts in a similar manner to
      // the Gazebo node
      this->rosNode.reset(new ros::NodeHandle("earthquake_rosnode"));

      // Magnitude
      ros::SubscribeOptions options =
        ros::SubscribeOptions::create<std_msgs::Float32>(
            prop_vel_topic,
            1,
            boost::bind(&PropSpin::OnRosMsg_Magn, this, _1),
            ros::VoidPtr(), &this->rosQueue);
      this->rosSub = this->rosNode->subscribe(options);

      // Spin up the queue helper thread.
      this->rosQueueThread =
        std::thread(std::bind(&PropSpin::QueueThread, this));

      ROS_WARN("Loaded PropSpin Plugin with parent...%s", this->model->GetName().c_str());

    }

    // Called by the world update start event
    public: void OnUpdate()
    {
      // Apply a small linear velocity to the model.
      this->model->SetLinearVel(ignition::math::Vector3d(0, 0, 0));
      this->model->SetAngularVel(ignition::math::Vector3d(0, 0, prop_vel));
    }


    public: void OnRosMsg_Magn(const std_msgs::Float32ConstPtr &_msg)
    {
      ROS_INFO("ON ROS MSG");
      this->prop_vel=_msg->data;
    }

    /// \brief ROS helper function that processes messages
    private: void QueueThread()
    {
      static const double timeout = 0.01;
      while (this->rosNode->ok())
      {
        this->rosQueue.callAvailable(ros::WallDuration(timeout));
      }
    }

    // Pointer to the model
    private: physics::ModelPtr model;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    // Magnitude of the Oscilations
    double prop_vel = 1.0;

    private: std::unique_ptr<ros::NodeHandle> rosNode;

    /// \brief A ROS subscriber
    private: ros::Subscriber rosSub;
    /// \brief A ROS callbackqueue that helps process messages
    private: ros::CallbackQueue rosQueue;
    /// \brief A thread the keeps running the rosQueue
    private: std::thread rosQueueThread;

  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(PropSpin)
}

Мировой файл, с которым проверяется работа плагина:

<?xml version="1.0"?>
<sdf version="1.4">
  <world name="vtol_twist">

    <include>
      <uri>model://ground_plane</uri>
    </include>

    <include>
      <uri>model://sun</uri>
    </include>

    <model name="box">
      <pose>0 0 0 0 0 0</pose>
      <link name="base_link">
        <collision name="collision">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </collision>

        <visual name="visual">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </visual>
      </link>

      <plugin name="vtol_twist_plugin" filename="libvtol_twist_plugin.so"/>
    </model>
  </world>
</sdf>

Файл URDF, в котором плагин не работает:

<?xml version="1.0"?>
<robot name="prop_spin">

  <link name="prop">
    <visual>
      <origin xyz="0 0 0.02" rpy="1.57 0 0"/>
      <geometry>
        <mesh filename="package://vtol_description/meshes/chb_prop_ccw.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0.02" rpy="1.57 0 0"/>
      <geometry>
        <mesh filename="package://vtol_description/meshes/chb_prop_ccw.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1"/>
      <inertia ixx="0.1"  ixy="0"  ixz="0" iyy="0.1" iyz="0" izz="0.1" />
    </inertial>
    <gazebo>
      <plugin name="vtol_twist_plugin" filename="/home/raghav/vtol/devel/lib/libvtol_twist_plugin.so"/>
    </gazebo>
  </link>

  <link name="prop_stand">
    <visual>
      <origin xyz="0 0 0"/>
      <geometry>
        <box size="0.025 0.025 0.05"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0"/>
      <geometry>
        <box size="0.025 0.025 0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="5"/>
      <inertia ixx="0.1"  ixy="0"  ixz="0" iyy="0.1" iyz="0" izz="0.1" />
    </inertial>
  </link>

  <joint name="prop_stand_to_prop" type="floating">
    <parent link="prop_stand"/>
    <child link="prop"/>
    <axis xyz="0 0 1"/>
    <origin xyz="0 0 0"/>
  </joint>
</robot>

Любая помощь будет принята с благодарностью. Спасибо!