23 #include <utils/misc/gazebo_api_wrappers.h>
27 using namespace gazebo;
30 GZ_REGISTER_MODEL_PLUGIN(
Motor)
38 printf(
"Destructing Motor Plugin!\n");
48 this->model_ = _parent;
51 this->name_ = model_->GetName();
52 printf(
"Loading Motor Plugin of model %s\n", name_.c_str());
56 this->update_connection_ =
57 event::Events::ConnectWorldUpdateBegin(boost::bind(&
Motor::OnUpdate,
this, _1));
60 this->node_ = transport::NodePtr(
new transport::Node());
62 this->node_->Init(model_->GetWorld()->GZWRAP_NAME() +
"/" + name_);
70 this->motor_move_sub_ = this->node_->Subscribe(std::string(
"~/RobotinoSim/MotorMove/"),
71 &Motor::on_motor_move_msg,
82 float yaw = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_Z;
87 x += cos(yaw + 3.1415926f / 2) * vy_;
88 y += sin(yaw + 3.1415926f / 2) * vy_;
90 this->model_->SetLinearVel(gzwrap::Vector3d(x, y, 0));
91 this->model_->SetAngularVel(gzwrap::Vector3d(0, 0, vomega_));
105 Motor::on_motor_move_msg(ConstVector3dPtr &msg)