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