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
27using namespace gazebo;
28
29// Register this plugin to make it available in the simulator
30GZ_REGISTER_MODEL_PLUGIN(Gyro)
31
32Gyro::Gyro() : last_sent_time_(0.0), send_interval_(0.0)
33{
34}
35
36Gyro::~Gyro()
37{
38 printf("Destructing Gyro Plugin!\n");
39}
40
41/** on loading of the plugin
42 * @param _parent Parent Model
43 */
44void
45Gyro::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 */
74void
75Gyro::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 */
87void
89{
90}
91
92void
93Gyro::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