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
27using namespace gazebo;
28
29// Register this plugin to make it available in the simulator
30GZ_REGISTER_MODEL_PLUGIN(Gps)
31
32///Constructor
34{
35}
36///Destructor
37Gps::~Gps()
38{
39 printf("Destructing Gps Plugin!\n");
40}
41
42/** on loading of the plugin
43 * @param _parent Parent Model
44 */
45void
46Gps::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 */
73void
74Gps::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 */
87void
89{
90}
91
92/** Sending position to Fawkes
93 *
94 */
95void
96Gps::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}
Provides ground Truth position.
Definition: gps.h:36
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr)
on loading of the plugin
Definition: gps.cpp:46
virtual void OnUpdate(const common::UpdateInfo &)
Called by the world update start event.
Definition: gps.cpp:74
virtual void Reset()
on Gazebo reset
Definition: gps.cpp:88