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 "gps.h"
22 
23 #include <utils/misc/gazebo_api_wrappers.h>
24 
25 #include <math.h>
26 
27 using namespace gazebo;
28 
29 // Register this plugin to make it available in the simulator
30 GZ_REGISTER_MODEL_PLUGIN(Gps)
31 
32 ///Constructor
33 Gps::Gps()
34 {
35 }
36 ///Destructor
38 {
39  printf("Destructing Gps Plugin!\n");
40 }
41 
42 /** on loading of the plugin
43  * @param _parent Parent Model
44  */
45 void
46 Gps::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
47 {
48  // Store the pointer to the model
49  this->model_ = _parent;
50 
51  //get the model-name
52  this->name_ = model_->GetName();
53  printf("Loading Gps Plugin of model %s\n", name_.c_str());
54 
55  // Listen to the update event. This event is broadcast every
56  // simulation iteration.
57  this->update_connection_ =
58  event::Events::ConnectWorldUpdateBegin(boost::bind(&Gps::OnUpdate, this, _1));
59 
60  //Create the communication Node for communication with fawkes
61  this->node_ = transport::NodePtr(new transport::Node());
62 
63  //set namespace to the model name & init last sent time
64  this->node_->Init(model_->GetWorld()->GZWRAP_NAME() + "/" + name_);
65  last_sent_time_ = model_->GetWorld()->GZWRAP_SIM_TIME().Double();
66 
67  //create publisher
68  this->gps_pub_ = this->node_->Advertise<msgs::Pose>("~/gazsim/gps/");
69 }
70 
71 /** Called by the world update start event
72  */
73 void
74 Gps::OnUpdate(const common::UpdateInfo & /*_info*/)
75 {
76  //Send position information to Fawkes
77  double time = model_->GetWorld()->GZWRAP_SIM_TIME().Double();
78 
79  if (time - last_sent_time_ > (1.0 / 10.0)) {
80  last_sent_time_ = time;
81  send_position();
82  }
83 }
84 
85 /** on Gazebo reset
86  */
87 void
89 {
90 }
91 
92 /** Sending position to Fawkes
93  *
94  */
95 void
96 Gps::send_position()
97 {
98  if (gps_pub_->HasConnections()) {
99  //build message
100  msgs::Pose posMsg;
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);
108 
109  //send
110  gps_pub_->Publish(posMsg);
111  }
112 }
virtual void OnUpdate(const common::UpdateInfo &)
Called by the world update start event.
Definition: gps.cpp:74
~Gps()
Destructor.
Definition: gps.cpp:37
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr)
on loading of the plugin
Definition: gps.cpp:46
Provides ground Truth position.
Definition: gps.h:35
virtual void Reset()
on Gazebo reset
Definition: gps.cpp:88