23 #include <utils/misc/gazebo_api_wrappers.h>
27 using namespace gazebo;
30 GZ_REGISTER_MODEL_PLUGIN(
Gps)
39 printf(
"Destructing Gps Plugin!\n");
46 Gps::Load(physics::ModelPtr _parent, sdf::ElementPtr )
49 this->model_ = _parent;
52 this->name_ = model_->GetName();
53 printf(
"Loading Gps Plugin of model %s\n", name_.c_str());
57 this->update_connection_ =
58 event::Events::ConnectWorldUpdateBegin(boost::bind(&
Gps::OnUpdate,
this, _1));
61 this->node_ = transport::NodePtr(
new transport::Node());
64 this->node_->Init(model_->GetWorld()->GZWRAP_NAME() +
"/" + name_);
65 last_sent_time_ = model_->GetWorld()->GZWRAP_SIM_TIME().Double();
68 this->gps_pub_ = this->node_->Advertise<msgs::Pose>(
"~/gazsim/gps/");
77 double time = model_->GetWorld()->GZWRAP_SIM_TIME().Double();
79 if (time - last_sent_time_ > (1.0 / 10.0)) {
80 last_sent_time_ = time;
98 if (gps_pub_->HasConnections()) {
101 posMsg.mutable_position()->set_x(this->model_->GZWRAP_WORLD_POSE().GZWRAP_POS_X);
102 posMsg.mutable_position()->set_y(this->model_->GZWRAP_WORLD_POSE().GZWRAP_POS_Y);
103 posMsg.mutable_position()->set_z(this->model_->GZWRAP_WORLD_POSE().GZWRAP_POS_Z);
104 posMsg.mutable_orientation()->set_x(this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_X);
105 posMsg.mutable_orientation()->set_y(this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_Y);
106 posMsg.mutable_orientation()->set_z(this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_Z);
107 posMsg.mutable_orientation()->set_w(this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_W);
110 gps_pub_->Publish(posMsg);