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
27using namespace gazebo;
28
29// Register this plugin to make it available in the simulator
30GZ_REGISTER_MODEL_PLUGIN(Motor)
31
32Motor::Motor() : vx_(0), vy_(0), vomega_(0)
33{
34}
35
36Motor::~Motor()
37{
38 printf("Destructing Motor Plugin!\n");
39}
40
41/** on loading of the plugin
42 * @param _parent Parent Model
43 */
44void
45Motor::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 */
77void
78Motor::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 */
96void
98{
99}
100
101/** Functions for recieving Messages (registerd via suscribers)
102 * @param msg message
103 */
104void
105Motor::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
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