Fawkes API  Fawkes Development Version
gyro.cpp
1 /***************************************************************************
2  * gyro.cpp - Plugin for a gyro sensor on a model
3  *
4  * Created: Tue Feb 04 14:43:59 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 "gyro.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(Gyro)
31 
32 Gyro::Gyro() : last_sent_time_(0.0), send_interval_(0.0)
33 {
34 }
35 
37 {
38  printf("Destructing Gyro Plugin!\n");
39 }
40 
41 /** on loading of the plugin
42  * @param _parent Parent Model
43  */
44 void
45 Gyro::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
46 {
47  // Store the pointer to the model
48  this->model_ = _parent;
49 
50  //get the model-name
51  this->name_ = model_->GetName();
52  printf("Loading Gyro Plugin of model %s\n", name_.c_str());
53 
54  // Listen to the update event. This event is broadcast every
55  // simulation iteration.
56  this->update_connection_ =
57  event::Events::ConnectWorldUpdateBegin(boost::bind(&Gyro::OnUpdate, this, _1));
58 
59  //Create the communication Node for communication with fawkes
60  this->node_ = transport::NodePtr(new transport::Node());
61  //the namespace is set to the model name!
62  this->node_->Init(model_->GetWorld()->GZWRAP_NAME() + "/" + name_);
63 
64  //create publisher
65  this->gyro_pub_ = this->node_->Advertise<msgs::Vector3d>("~/RobotinoSim/Gyro/");
66 
67  //init last sent time
68  last_sent_time_ = model_->GetWorld()->GZWRAP_SIM_TIME().Double();
69  this->send_interval_ = 0.05;
70 }
71 
72 /** Called by the world update start event
73  */
74 void
75 Gyro::OnUpdate(const common::UpdateInfo & /*_info*/)
76 {
77  //Send gyro information to Fawkes
78  double time = model_->GetWorld()->GZWRAP_SIM_TIME().Double();
79  if (time - last_sent_time_ > send_interval_) {
80  last_sent_time_ = time;
81  send_gyro();
82  }
83 }
84 
85 /** on Gazebo reset
86  */
87 void
89 {
90 }
91 
92 void
93 Gyro::send_gyro()
94 {
95  if (gyro_pub_->HasConnections()) {
96  //Read gyro from simulation
97  float roll = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_X;
98  float pitch = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_Y;
99  float yaw = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_Z;
100 
101  //build message
102  msgs::Vector3d gyroMsg;
103  gyroMsg.set_x(roll);
104  gyroMsg.set_y(pitch);
105  gyroMsg.set_z(yaw);
106 
107  //send
108  gyro_pub_->Publish(gyroMsg);
109  }
110 }
Plugin for a gyro sensor on a model.
Definition: gyro.h:36
virtual void OnUpdate(const common::UpdateInfo &)
Called by the world update start event.
Definition: gyro.cpp:75
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr)
on loading of the plugin
Definition: gyro.cpp:45
virtual void Reset()
on Gazebo reset
Definition: gyro.cpp:88
~Gyro()
Destructor.
Definition: gyro.cpp:36