Fawkes API  Fawkes Development Version
gps.cpp
1 /***************************************************************************
2  * gps.cpp - Provides ground Truth position
3  *
4  * Created: Tue Feb 04 15:06:06 2014
5  * Copyright 2014 Frederik Zwilling
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include <math.h>
22 
23 #include "gps.h"
24 
25 using namespace gazebo;
26 
27 // Register this plugin to make it available in the simulator
28 GZ_REGISTER_MODEL_PLUGIN(Gps)
29 
30 ///Constructor
31 Gps::Gps()
32 {
33 }
34 ///Destructor
36 {
37  printf("Destructing Gps Plugin!\n");
38 }
39 
40 /** on loading of the plugin
41  * @param _parent Parent Model
42  */
43 void Gps::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
44 {
45  // Store the pointer to the model
46  this->model_ = _parent;
47 
48  //get the model-name
49  this->name_ = model_->GetName();
50  printf("Loading Gps Plugin of model %s\n", name_.c_str());
51 
52  // Listen to the update event. This event is broadcast every
53  // simulation iteration.
54  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&Gps::OnUpdate, this, _1));
55 
56  //Create the communication Node for communication with fawkes
57  this->node_ = transport::NodePtr(new transport::Node());
58  //the namespace is set to the model name!
59  this->node_->Init(model_->GetWorld()->GetName()+"/"+name_);
60 
61  //init last sent time
62  last_sent_time_ = model_->GetWorld()->GetSimTime().Double();
63 
64  //create publisher
65  this->gps_pub_ = this->node_->Advertise<msgs::Pose>("~/gazsim/gps/");
66 }
67 
68 
69 /** Called by the world update start event
70  */
71 void Gps::OnUpdate(const common::UpdateInfo & /*_info*/)
72 {
73  //Send position information to Fawkes
74  double time = model_->GetWorld()->GetSimTime().Double();
75  if(time - last_sent_time_ > (1.0 / 10.0))
76  {
77  last_sent_time_ = time;
78  send_position();
79  }
80 }
81 
82 /** on Gazebo reset
83  */
84 void Gps::Reset()
85 {
86 }
87 
88 /** Sending position to Fawkes
89  *
90  */
91 void Gps::send_position()
92 {
93  if(gps_pub_->HasConnections())
94  {
95  //build message
96  msgs::Pose posMsg;
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);
104 
105  //send
106  gps_pub_->Publish(posMsg);
107  }
108 }
Definition: gps.h:30
virtual void OnUpdate(const common::UpdateInfo &)
Called by the world update start event.
Definition: gps.cpp:71
~Gps()
Destructor.
Definition: gps.cpp:35
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr)
on loading of the plugin
Definition: gps.cpp:43
Provides ground Truth position.
Definition: gps.h:36
virtual void Reset()
on Gazebo reset
Definition: gps.cpp:84