28 GZ_REGISTER_MODEL_PLUGIN(
Gps)
37 printf(
"Destructing Gps Plugin!\n");
43 void Gps::Load(physics::ModelPtr _parent, sdf::ElementPtr )
46 this->model_ = _parent;
49 this->name_ = model_->GetName();
50 printf(
"Loading Gps Plugin of model %s\n", name_.c_str());
54 this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&
Gps::OnUpdate,
this, _1));
57 this->node_ = transport::NodePtr(
new transport::Node());
59 this->node_->Init(model_->GetWorld()->GetName()+
"/"+name_);
62 last_sent_time_ = model_->GetWorld()->GetSimTime().Double();
65 this->gps_pub_ = this->node_->Advertise<msgs::Pose>(
"~/gazsim/gps/");
74 double time = model_->GetWorld()->GetSimTime().Double();
75 if(time - last_sent_time_ > (1.0 / 10.0))
77 last_sent_time_ = time;
91 void Gps::send_position()
93 if(gps_pub_->HasConnections())
97 posMsg.mutable_position()->set_x(this->model_->GetWorldPose().pos.x);
98 posMsg.mutable_position()->set_y(this->model_->GetWorldPose().pos.y);
99 posMsg.mutable_position()->set_z(this->model_->GetWorldPose().pos.z);
100 posMsg.mutable_orientation()->set_x(this->model_->GetWorldPose().rot.x);
101 posMsg.mutable_orientation()->set_y(this->model_->GetWorldPose().rot.y);
102 posMsg.mutable_orientation()->set_z(this->model_->GetWorldPose().rot.z);
103 posMsg.mutable_orientation()->set_w(this->model_->GetWorldPose().rot.w);
106 gps_pub_->Publish(posMsg);
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
Provides ground Truth position.
virtual void Reset()
on Gazebo reset