servo_plugin.cpp
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
3 // All rights reserved.
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of the Flight Systems and Automatic Control group,
13 // TU Darmstadt, nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without
15 // specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
28 
30 #include <gazebo/common/Events.hh>
31 #include <gazebo/physics/physics.hh>
32 #include <gazebo/gazebo_config.h>
33 
34 #include <sensor_msgs/JointState.h>
35 
36 #if (GAZEBO_MAJOR_VERSION > 1) || (GAZEBO_MINOR_VERSION >= 2)
37  #define RADIAN Radian
38 #else
39  #define RADIAN GetAsRadian
40 #endif
41 
42 namespace gazebo {
43 
44 GZ_REGISTER_MODEL_PLUGIN(ServoPlugin)
45 
46 enum
47 {
48  FIRST = 0, SECOND = 1, THIRD = 2
49 };
50 
51 enum
52 {
54 };
55 
56 // Constructor
58 {
59  rosnode_ = 0;
61 }
62 
63 // Destructor
65 {
66 #if (GAZEBO_MAJOR_VERSION >= 8)
67  updateConnection.reset();
68 #else
69  event::Events::DisconnectWorldUpdateBegin(updateConnection);
70 #endif
71  delete transform_listener_;
72  rosnode_->shutdown();
73  delete rosnode_;
74 }
75 
76 // Load the controller
77 void ServoPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
78 {
79  // Get the world name.
80  world = _model->GetWorld();
81 
82  // default parameters
83  topicName = "drive";
84  jointStateName = "joint_states";
85  robotNamespace.clear();
86  controlPeriod = 0;
89  maximumVelocity = 0.0;
90  maximumTorque = 0.0;
91 
92  // load parameters
93  if (_sdf->HasElement("robotNamespace")) robotNamespace = _sdf->Get<std::string>("robotNamespace");
94  if (_sdf->HasElement("topicName")) topicName = _sdf->Get<std::string>("topicName");
95  if (_sdf->HasElement("jointStateName")) jointStateName = _sdf->Get<std::string>("jointStateName");
96  if (_sdf->HasElement("firstServoName")) servo[FIRST].name = _sdf->Get<std::string>("firstServoName");
97 #if (GAZEBO_MAJOR_VERSION >= 8)
98  if (_sdf->HasElement("firstServoAxis")) servo[FIRST].axis = _sdf->Get<ignition::math::Vector3d>("firstServoAxis");
99 #else
100  if (_sdf->HasElement("firstServoAxis")) servo[FIRST].axis = _sdf->Get<math::Vector3>("firstServoAxis");
101 #endif
102  if (_sdf->HasElement("secondServoName")) servo[SECOND].name = _sdf->Get<std::string>("secondServoName");
103 #if (GAZEBO_MAJOR_VERSION >= 8)
104  if (_sdf->HasElement("secondServoAxis")) servo[SECOND].axis = _sdf->Get<ignition::math::Vector3d>("secondServoAxis");
105 #else
106  if (_sdf->HasElement("secondServoAxis")) servo[SECOND].axis = _sdf->Get<math::Vector3>("secondServoAxis");
107 #endif
108  if (_sdf->HasElement("thirdServoName")) servo[THIRD].name = _sdf->Get<std::string>("thirdServoName");
109 #if (GAZEBO_MAJOR_VERSION >= 8)
110  if (_sdf->HasElement("thirdServoAxis")) servo[THIRD].axis = _sdf->Get<ignition::math::Vector3d>("thirdServoAxis");
111 #else
112  if (_sdf->HasElement("thirdServoAxis")) servo[THIRD].axis = _sdf->Get<math::Vector3>("thirdServoAxis");
113 #endif
114  if (_sdf->HasElement("proportionalControllerGain")) proportionalControllerGain = _sdf->Get<double>("proportionalControllerGain");
115  if (_sdf->HasElement("derivativeControllerGain")) derivativeControllerGain = _sdf->Get<double>("derivativeControllerGain");
116  if (_sdf->HasElement("maxVelocity")) maximumVelocity = _sdf->Get<double>("maxVelocity");
117  if (_sdf->HasElement("torque")) maximumTorque = _sdf->Get<double>("torque");
118 
119  double controlRate = 0.0;
120  if (_sdf->HasElement("controlRate")) controlRate = _sdf->Get<double>("controlRate");
121  controlPeriod = controlRate > 0.0 ? 1.0/controlRate : 0.0;
122 
123  servo[FIRST].joint = _model->GetJoint(servo[FIRST].name);
124  servo[SECOND].joint = _model->GetJoint(servo[SECOND].name);
125  servo[THIRD].joint = _model->GetJoint(servo[THIRD].name);
126 
127  if (!servo[FIRST].joint)
128  gzthrow("The controller couldn't get first joint");
129 
130  countOfServos = 1;
131  if (servo[SECOND].joint) {
132  countOfServos = 2;
133  if (servo[THIRD].joint) {
134  countOfServos = 3;
135  }
136  }
137  else {
138  if (servo[THIRD].joint) {
139  gzthrow("The controller couldn't get second joint, but third joint was loaded");
140  }
141  }
142 
143  if (!ros::isInitialized()) {
144  int argc = 0;
145  char** argv = NULL;
147  }
148 
150 
153 
154  if (!topicName.empty()) {
155  ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::QuaternionStamped>(topicName, 1,
156  boost::bind(&ServoPlugin::cmdCallback, this, _1),
157  ros::VoidPtr(), &queue_);
158  sub_ = rosnode_->subscribe(so);
159  }
160 
161  if (!jointStateName.empty()) {
162  jointStatePub_ = rosnode_->advertise<sensor_msgs::JointState>(jointStateName, 10);
163  }
164 
165  joint_state.header.frame_id = transform_listener_->resolve(_model->GetLink()->GetName());
166 
167  // New Mechanism for Updating every World Cycle
168  // Listen to the update event. This event is broadcast every
169  // simulation iteration.
170  updateConnection = event::Events::ConnectWorldUpdateBegin(
171  boost::bind(&ServoPlugin::Update, this));
172 }
173 
174 // Initialize the controller
176 {
177  Reset();
178 }
179 
180 // Reset
182 {
183  // Reset orientation
184  current_cmd.reset();
185 
186  enableMotors = true;
187 
188  servo[FIRST].velocity = 0;
189  servo[SECOND].velocity = 0;
190  servo[THIRD].velocity = 0;
191 
192 #if (GAZEBO_MAJOR_VERSION >= 8)
193  prevUpdateTime = world->SimTime();
194 #else
195  prevUpdateTime = world->GetSimTime();
196 #endif
197 }
198 
199 // Update the controller
201 {
202  // handle callbacks
204 
205  common::Time stepTime;
206 #if (GAZEBO_MAJOR_VERSION >= 8)
207  stepTime = world->SimTime() - prevUpdateTime;
208 #else
209  stepTime = world->GetSimTime() - prevUpdateTime;
210 #endif
211 
212  if (controlPeriod == 0.0 || stepTime > controlPeriod) {
215 #if (GAZEBO_MAJOR_VERSION >= 8)
216  prevUpdateTime = world->SimTime();
217 #else
218  prevUpdateTime = world->GetSimTime();
219 #endif
220  }
221 
222  if (enableMotors)
223  {
224  servo[FIRST].joint->SetVelocity(0, servo[FIRST].velocity);
225  if (countOfServos > 1) {
226  servo[SECOND].joint->SetVelocity(0, servo[SECOND].velocity);
227  if (countOfServos > 2) {
228  servo[THIRD].joint->SetVelocity(0, servo[THIRD].velocity);
229  }
230  }
231 
232 #if (GAZEBO_MAJOR_VERSION > 4)
233  servo[FIRST].joint->SetEffortLimit(0, maximumTorque);
234  if (countOfServos > 1) {
235  servo[SECOND].joint->SetEffortLimit(0, maximumTorque);
236  if (countOfServos > 2) {
237  servo[THIRD].joint->SetEffortLimit(0, maximumTorque);
238  }
239  }
240 #else
241  servo[FIRST].joint->SetMaxForce(0, maximumTorque);
242  if (countOfServos > 1) {
243  servo[SECOND].joint->SetMaxForce(0, maximumTorque);
244  if (countOfServos > 2) {
245  servo[THIRD].joint->SetMaxForce(0, maximumTorque);
246  }
247  }
248 #endif
249  } else {
250 #if (GAZEBO_MAJOR_VERSION > 4)
251  servo[FIRST].joint->SetEffortLimit(0, 0.0);
252  if (countOfServos > 1) {
253  servo[SECOND].joint->SetEffortLimit(0, 0.0);
254  if (countOfServos > 2) {
255  servo[THIRD].joint->SetEffortLimit(0, 0.0);
256  }
257  }
258 #else
259  servo[FIRST].joint->SetMaxForce(0, 0.0);
260  if (countOfServos > 1) {
261  servo[SECOND].joint->SetMaxForce(0, 0.0);
262  if (countOfServos > 2) {
263  servo[THIRD].joint->SetMaxForce(0, 0.0);
264  }
265  }
266 #endif
267  }
268 }
269 
271 {
272  tf::StampedTransform transform;
273  boost::mutex::scoped_lock lock(mutex);
274 
275  if(!current_cmd){
276  geometry_msgs::QuaternionStamped *default_cmd = new geometry_msgs::QuaternionStamped;
277  default_cmd->header.frame_id = "base_stabilized";
278  default_cmd->quaternion.w = 1;
279  current_cmd.reset(default_cmd);
280  }
281 
282  try{
283  // ros::Time simTime(world->GetSimTime().sec, world->GetSimTime().nsec);
284  transform_listener_->lookupTransform("base_link", current_cmd->header.frame_id, ros::Time(0), transform);
285  }
286  catch (tf::TransformException ex){
287  ROS_DEBUG("%s",ex.what());
288  servo[FIRST].velocity = 0.0;
289  servo[SECOND].velocity = 0.0;
290  servo[THIRD].velocity = 0.0;
291  return;
292  }
293 
294  rotation_.Set(current_cmd->quaternion.w, current_cmd->quaternion.x, current_cmd->quaternion.y, current_cmd->quaternion.z);
295 
296 #if (GAZEBO_MAJOR_VERSION >= 8)
297  ignition::math::Quaterniond quat(transform.getRotation().getW(),transform.getRotation().getX(),transform.getRotation().getY(),transform.getRotation().getZ());
298 #else
299  math::Quaternion quat(transform.getRotation().getW(),transform.getRotation().getX(),transform.getRotation().getY(),transform.getRotation().getZ());
300 #endif
301 
302  rotation_ = quat * rotation_;
303 
304  double temp[5];
305  double desAngle[3];
306  double actualAngle[3] = {0.0, 0.0, 0.0};
307  double actualVel[3] = {0.0, 0.0, 0.0};
308 
309  //TODO use countOfServos for calculation
310  rotationConv = 99;
311  orderOfAxes[0] = 99;
312  orderOfAxes[1] = 99;
313  orderOfAxes[2] = 99;
314 
315  switch(countOfServos) {
316  case 2:
317 #if (GAZEBO_MAJOR_VERSION >= 8)
318  if ((servo[FIRST].axis.Z() == 1) && (servo[SECOND].axis.Y() == 1)) {
319 #else
320  if ((servo[FIRST].axis.z == 1) && (servo[SECOND].axis.y == 1)) {
321 #endif
322  rotationConv = zyx;
323  orderOfAxes[0] = 0;
324  orderOfAxes[1] = 1;
325  }
326  else {
327 #if (GAZEBO_MAJOR_VERSION >= 8)
328  if ((servo[FIRST].axis.X() == 1) && (servo[SECOND].axis.Y() == 1)) {
329 #else
330  if ((servo[FIRST].axis.x == 1) && (servo[SECOND].axis.y == 1)) {
331 #endif
332  rotationConv = xyz;
333  orderOfAxes[0] = 0;
334  orderOfAxes[1] = 1;
335  }
336  }
337  break;
338 
339  case 3:
340 #if (GAZEBO_MAJOR_VERSION >= 8)
341  if ((servo[FIRST].axis.Z() == 1) && (servo[SECOND].axis.Y() == 1) && (servo[THIRD].axis.X() == 1)) {
342 #else
343  if ((servo[FIRST].axis.z == 1) && (servo[SECOND].axis.y == 1) && (servo[THIRD].axis.x == 1)) {
344 #endif
345  rotationConv = zyx;
346  orderOfAxes[0] = 0;
347  orderOfAxes[1] = 1;
348  orderOfAxes[2] = 2;
349  }
350 #if (GAZEBO_MAJOR_VERSION >= 8)
351  else if ((servo[FIRST].axis.X() == 1) && (servo[SECOND].axis.Y() == 1) && (servo[THIRD].axis.Z() == 1)) {
352 #else
353  else if ((servo[FIRST].axis.x == 1) && (servo[SECOND].axis.y == 1) && (servo[THIRD].axis.z == 1)) {
354 #endif
355  rotationConv = xyz;
356  orderOfAxes[0] = 0;
357  orderOfAxes[1] = 1;
358  orderOfAxes[2] = 2;
359  }
360  break;
361 
362  case 1:
363 #if (GAZEBO_MAJOR_VERSION >= 8)
364  if (servo[FIRST].axis.Y() == 1) {
365 #else
366  if (servo[FIRST].axis.y == 1) {
367 #endif
368  rotationConv = xyz;
369  orderOfAxes[0] = 1;
370  }
371  break;
372 
373  default:
374  gzthrow("Something went wrong. The count of servos is greater than 3");
375  break;
376  }
377 
378  switch(rotationConv) {
379  case zyx:
380 #if (GAZEBO_MAJOR_VERSION >= 8)
381  temp[0] = 2*(rotation_.X()*rotation_.Y() + rotation_.W()*rotation_.Z());
382  temp[1] = rotation_.W()*rotation_.W() + rotation_.X()*rotation_.X() - rotation_.Y()*rotation_.Y() - rotation_.Z()*rotation_.Z();
383  temp[2] = -2*(rotation_.X()*rotation_.Z() - rotation_.W()*rotation_.Y());
384  temp[3] = 2*(rotation_.Y()*rotation_.Z() + rotation_.W()*rotation_.X());
385  temp[4] = rotation_.W()*rotation_.W() - rotation_.X()*rotation_.X() - rotation_.Y()*rotation_.Y() + rotation_.Z()*rotation_.Z();
386 #else
387  temp[0] = 2*(rotation_.x*rotation_.y + rotation_.w*rotation_.z);
388  temp[1] = rotation_.w*rotation_.w + rotation_.x*rotation_.x - rotation_.y*rotation_.y - rotation_.z*rotation_.z;
389  temp[2] = -2*(rotation_.x*rotation_.z - rotation_.w*rotation_.y);
390  temp[3] = 2*(rotation_.y*rotation_.z + rotation_.w*rotation_.x);
391  temp[4] = rotation_.w*rotation_.w - rotation_.x*rotation_.x - rotation_.y*rotation_.y + rotation_.z*rotation_.z;
392 #endif
393  break;
394 
395  case xyz:
396 #if (GAZEBO_MAJOR_VERSION >= 8)
397  temp[0] = -2*(rotation_.Y()*rotation_.Z() - rotation_.W()*rotation_.X());
398  temp[1] = rotation_.W()*rotation_.W() - rotation_.X()*rotation_.X() - rotation_.Y()*rotation_.Y() + rotation_.Z()*rotation_.Z();
399  temp[2] = 2*(rotation_.X()*rotation_.Z() + rotation_.W()*rotation_.Y());
400  temp[3] = -2*(rotation_.X()*rotation_.Y() - rotation_.W()*rotation_.Z());
401  temp[4] = rotation_.W()*rotation_.W() + rotation_.X()*rotation_.X() - rotation_.Y()*rotation_.Y() - rotation_.Z()*rotation_.Z();
402 #else
403  temp[0] = -2*(rotation_.y*rotation_.z - rotation_.w*rotation_.x);
404  temp[1] = rotation_.w*rotation_.w - rotation_.x*rotation_.x - rotation_.y*rotation_.y + rotation_.z*rotation_.z;
405  temp[2] = 2*(rotation_.x*rotation_.z + rotation_.w*rotation_.y);
406  temp[3] = -2*(rotation_.x*rotation_.y - rotation_.w*rotation_.z);
407  temp[4] = rotation_.w*rotation_.w + rotation_.x*rotation_.x - rotation_.y*rotation_.y - rotation_.z*rotation_.z;
408 #endif
409  break;
410 
411  default:
412  gzthrow("joint order " << rotationConv << " not supported");
413  break;
414  }
415 
416  desAngle[0] = atan2(temp[0], temp[1]);
417  desAngle[1] = asin(temp[2]);
418  desAngle[2] = atan2(temp[3], temp[4]);
419 
420 #if (GAZEBO_MAJOR_VERSION >= 8)
421  actualAngle[FIRST] = servo[FIRST].joint->Position(0);
422 #else
423  actualAngle[FIRST] = servo[FIRST].joint->GetAngle(0).RADIAN();
424 #endif
425  actualVel[FIRST] = servo[FIRST].joint->GetVelocity(0);
426  ROS_DEBUG_NAMED("servo_plugin", "%s servo angle: %f - %f", servo[FIRST].name.c_str(), desAngle[orderOfAxes[FIRST]], actualAngle[FIRST]);
427  servo[FIRST].velocity = ( proportionalControllerGain*(desAngle[orderOfAxes[FIRST]] - actualAngle[FIRST]) - derivativeControllerGain*actualVel[FIRST]);
428  if (maximumVelocity > 0.0 && fabs(servo[FIRST].velocity) > maximumVelocity) servo[FIRST].velocity = (servo[FIRST].velocity > 0 ? maximumVelocity : -maximumVelocity);
429 
430  if (countOfServos > 1) {
431 #if (GAZEBO_MAJOR_VERSION >= 8)
432  actualAngle[SECOND] = servo[SECOND].joint->Position(0);
433 #else
434  actualAngle[SECOND] = servo[SECOND].joint->GetAngle(0).RADIAN();
435 #endif
436  actualVel[SECOND] = servo[SECOND].joint->GetVelocity(0);
437  ROS_DEBUG_NAMED("servo_plugin", "%s servo angle: %f - %f", servo[SECOND].name.c_str(), desAngle[orderOfAxes[SECOND]], actualAngle[SECOND]);
438  servo[SECOND].velocity = ( proportionalControllerGain*(desAngle[orderOfAxes[SECOND]] - actualAngle[SECOND]) - derivativeControllerGain*actualVel[SECOND]);
439  if (maximumVelocity > 0.0 && fabs(servo[SECOND].velocity) > maximumVelocity) servo[SECOND].velocity = (servo[SECOND].velocity > 0 ? maximumVelocity : -maximumVelocity);
440 
441  if (countOfServos == 3) {
442 #if (GAZEBO_MAJOR_VERSION >= 8)
443  actualAngle[THIRD] = servo[THIRD].joint->Position(0);
444 #else
445  actualAngle[THIRD] = servo[THIRD].joint->GetAngle(0).RADIAN();
446 #endif
447  actualVel[THIRD] = servo[THIRD].joint->GetVelocity(0);
448  ROS_DEBUG_NAMED("servo_plugin", "%s servo angle: %f - %f", servo[THIRD].name.c_str(), desAngle[orderOfAxes[THIRD]], actualAngle[THIRD]);
449  servo[THIRD].velocity = ( proportionalControllerGain*(desAngle[orderOfAxes[THIRD]] - actualAngle[THIRD]) - derivativeControllerGain*actualVel[THIRD]);
450  if (maximumVelocity > 0.0 && fabs(servo[THIRD].velocity) > maximumVelocity) servo[THIRD].velocity = (servo[THIRD].velocity > 0 ? maximumVelocity : -maximumVelocity);
451  }
452  }
453 
454  // Changed motors to be always on, which is probably what we want anyway
455  enableMotors = true; //myIface->data->cmdEnableMotors > 0;
456 }
457 
458 // NEW: Store the velocities from the ROS message
459 void ServoPlugin::cmdCallback(const geometry_msgs::QuaternionStamped::ConstPtr& cmd_msg)
460 {
461  boost::mutex::scoped_lock lock(mutex);
462  current_cmd = cmd_msg;
463 }
464 
466 {
467  if (!jointStatePub_) return;
468 
469 #if (GAZEBO_MAJOR_VERSION >= 8)
470  joint_state.header.stamp.sec = (world->SimTime()).sec;
471  joint_state.header.stamp.nsec = (world->SimTime()).nsec;
472 #else
473  joint_state.header.stamp.sec = (world->GetSimTime()).sec;
474  joint_state.header.stamp.nsec = (world->GetSimTime()).nsec;
475 #endif
476 
477  joint_state.name.resize(countOfServos);
478  joint_state.position.resize(countOfServos);
479  joint_state.velocity.resize(countOfServos);
480  joint_state.effort.resize(countOfServos);
481 
482  for (unsigned int i = 0; i < countOfServos; i++) {
483  joint_state.name[i] = servo[i].joint->GetName();
484 #if (GAZEBO_MAJOR_VERSION >= 8)
485  joint_state.position[i] = servo[i].joint->Position(0);
486 #else
487  joint_state.position[i] = servo[i].joint->GetAngle(0).RADIAN();
488 #endif
489  joint_state.velocity[i] = servo[i].joint->GetVelocity(0);
490  joint_state.effort[i] = servo[i].joint->GetForce(0u);
491  }
492 
494 }
495 
496 } // namespace gazebo
std::string topicName
Definition: servo_plugin.h:97
void publish(const boost::shared_ptr< M > &message) const
common::Time prevUpdateTime
Definition: servo_plugin.h:74
unsigned int countOfServos
Definition: servo_plugin.h:90
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL bool isInitialized()
event::ConnectionPtr updateConnection
Definition: servo_plugin.h:129
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::Publisher jointStatePub_
Definition: servo_plugin.h:108
TFSIMD_FORCE_INLINE const tfScalar & getW() const
tf::TransformListener * transform_listener_
Definition: servo_plugin.h:110
std::string resolve(const std::string &frame_name)
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
#define ROS_DEBUG_NAMED(name,...)
math::Quaternion rotation_
Definition: servo_plugin.h:125
float proportionalControllerGain
Definition: servo_plugin.h:101
unsigned int orderOfAxes[3]
Definition: servo_plugin.h:91
ros::CallbackQueue queue_
Definition: servo_plugin.h:113
void cmdCallback(const geometry_msgs::QuaternionStamped::ConstPtr &cmd_msg)
ros::Subscriber sub_
Definition: servo_plugin.h:109
physics::WorldPtr world
The parent World.
Definition: servo_plugin.h:71
virtual void Reset()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
unsigned int rotationConv
Definition: servo_plugin.h:92
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
common::Time controlPeriod
Definition: servo_plugin.h:99
std::string jointStateName
Definition: servo_plugin.h:98
geometry_msgs::QuaternionStamped::ConstPtr current_cmd
Definition: servo_plugin.h:121
std::string robotNamespace
Definition: servo_plugin.h:96
Quaternion getRotation() const
boost::mutex mutex
Definition: servo_plugin.h:120
physics::JointPtr joint
Definition: servo_plugin.h:85
float derivativeControllerGain
Definition: servo_plugin.h:102
ros::NodeHandle * rosnode_
Definition: servo_plugin.h:107
boost::shared_ptr< void > VoidPtr
struct gazebo::ServoPlugin::Servo servo[3]
void setExtrapolationLimit(const ros::Duration &distance)
virtual void Update()
virtual void Init()
sensor_msgs::JointState joint_state
Definition: servo_plugin.h:93
#define ROS_DEBUG(...)


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Fri Feb 5 2021 03:48:30