23 #include <utils/misc/gazebo_api_wrappers.h>
27 using namespace gazebo;
30 GZ_REGISTER_MODEL_PLUGIN(
Gyro)
32 Gyro::
Gyro() : last_sent_time_(0.0), send_interval_(0.0)
38 printf(
"Destructing Gyro Plugin!\n");
48 this->model_ = _parent;
51 this->name_ = model_->GetName();
52 printf(
"Loading Gyro Plugin of model %s\n", name_.c_str());
56 this->update_connection_ =
57 event::Events::ConnectWorldUpdateBegin(boost::bind(&
Gyro::OnUpdate,
this, _1));
60 this->node_ = transport::NodePtr(
new transport::Node());
62 this->node_->Init(model_->GetWorld()->GZWRAP_NAME() +
"/" + name_);
65 this->gyro_pub_ = this->node_->Advertise<msgs::Vector3d>(
"~/RobotinoSim/Gyro/");
68 last_sent_time_ = model_->GetWorld()->GZWRAP_SIM_TIME().Double();
69 this->send_interval_ = 0.05;
78 double time = model_->GetWorld()->GZWRAP_SIM_TIME().Double();
79 if (time - last_sent_time_ > send_interval_) {
80 last_sent_time_ = time;
95 if (gyro_pub_->HasConnections()) {
97 float roll = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_X;
98 float pitch = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_Y;
99 float yaw = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_Z;
102 msgs::Vector3d gyroMsg;
104 gyroMsg.set_y(pitch);
108 gyro_pub_->Publish(gyroMsg);