28 GZ_REGISTER_MODEL_PLUGIN(
Gyro)
36 printf(
"Destructing Gyro Plugin!\n");
42 void Gyro::Load(physics::ModelPtr _parent, sdf::ElementPtr )
45 this->model_ = _parent;
48 this->name_ = model_->GetName();
49 printf(
"Loading Gyro Plugin of model %s\n", name_.c_str());
53 this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&
Gyro::OnUpdate,
this, _1));
56 this->node_ = transport::NodePtr(
new transport::Node());
58 this->node_->Init(model_->GetWorld()->GetName()+
"/"+name_);
62 this->gyro_pub_ = this->node_->Advertise<msgs::Vector3d>(
"~/RobotinoSim/Gyro/");
65 last_sent_time_ = model_->GetWorld()->GetSimTime().Double();
66 this->send_interval_ = 0.05;
74 double time = model_->GetWorld()->GetSimTime().Double();
75 if(time - last_sent_time_ > send_interval_)
77 last_sent_time_ = time;
88 void Gyro::send_gyro()
90 if(gyro_pub_->HasConnections())
93 float roll = this->model_->GetWorldPose().rot.GetAsEuler().x;
94 float pitch = this->model_->GetWorldPose().rot.GetAsEuler().y;
95 float yaw = this->model_->GetWorldPose().rot.GetAsEuler().z;
98 msgs::Vector3d gyroMsg;
100 gyroMsg.set_y(pitch);
104 gyro_pub_->Publish(gyroMsg);
Plugin for a gyro sensor on a model.
virtual void Reset()
on Gazebo reset
virtual void OnUpdate(const common::UpdateInfo &)
Called by the world update start event.
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr)
on loading of the plugin