Fawkes API  Fawkes Development Version
motor.cpp
1 /***************************************************************************
2  * motor.cpp - Plugin for controling a model through a simulated motor
3  *
4  * Created: Wed Jan 29 16:10:17 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 "motor.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(Motor)
31 
32 Motor::Motor() : vx_(0), vy_(0), vomega_(0)
33 {
34 }
35 
37 {
38  printf("Destructing Motor Plugin!\n");
39 }
40 
41 /** on loading of the plugin
42  * @param _parent Parent Model
43  */
44 void
45 Motor::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 Motor 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(&Motor::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  //initialize movement commands:
65  vx_ = 0.0;
66  vy_ = 0.0;
67  vomega_ = 0.0;
68 
69  //create subscriber
70  this->motor_move_sub_ = this->node_->Subscribe(std::string("~/RobotinoSim/MotorMove/"),
71  &Motor::on_motor_move_msg,
72  this);
73 }
74 
75 /** Called by the world update start event
76  */
77 void
78 Motor::OnUpdate(const common::UpdateInfo & /*_info*/)
79 {
80  //Apply movement command
81  float x, y;
82  float yaw = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_Z;
83  //foward part
84  x = cos(yaw) * vx_;
85  y = sin(yaw) * vx_;
86  //sideways part
87  x += cos(yaw + 3.1415926f / 2) * vy_;
88  y += sin(yaw + 3.1415926f / 2) * vy_;
89  // Apply velocity to the model.
90  this->model_->SetLinearVel(gzwrap::Vector3d(x, y, 0));
91  this->model_->SetAngularVel(gzwrap::Vector3d(0, 0, vomega_));
92 }
93 
94 /** on Gazebo reset
95  */
96 void
98 {
99 }
100 
101 /** Functions for recieving Messages (registerd via suscribers)
102  * @param msg message
103  */
104 void
105 Motor::on_motor_move_msg(ConstVector3dPtr &msg)
106 {
107  //printf("Got MotorMove Msg!!! %f %f %f\n", msg->x(), msg->y(), msg->z());
108  //Transform relative motion into ablosulte motion
109  vx_ = msg->x();
110  vy_ = msg->y();
111  vomega_ = msg->z();
112 }
Motor plugin for Gazebo.
Definition: motor.h:36
~Motor()
Destructor.
Definition: motor.cpp:36
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr)
on loading of the plugin
Definition: motor.cpp:45
virtual void Reset()
on Gazebo reset
Definition: motor.cpp:97
virtual void OnUpdate(const common::UpdateInfo &)
Called by the world update start event.
Definition: motor.cpp:78