gazebo_ros_controller_manager.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
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  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <fstream>
31 #include <iostream>
32 #include <math.h>
33 #include <unistd.h>
34 #include <set>
35 
36 #include <map>
37 
38 #include <angles/angles.h>
39 #include <urdf/model.h>
40 
41 //#include <gazebo/XMLConfig.hh>
42 //#include "physics/physics.h"
43 #include <gazebo/physics/physics.hh>
44 #include <gazebo/sensors/sensors.hh>
45 #include <gazebo/common/common.hh>
46 #include <sdf/sdf.hh>
47 #include <sdf/Param.hh>
48 
50 
51 namespace gazebo {
52 
54 {
55 }
56 
58 bool setModelsJointsStates(pr2_gazebo_plugins::SetModelsJointsStates::Request &req,
59  pr2_gazebo_plugins::SetModelsJointsStates::Response &res)
60 {
61 
62  return true;
63 }
64 
65 
67 {
68  ROS_DEBUG("Calling FiniChild in GazeboRosControllerManager");
69 
70  //pr2_hardware_interface::ActuatorMap::const_iterator it;
71  //for (it = hw_.actuators_.begin(); it != hw_.actuators_.end(); ++it)
72  // delete it->second; // why is this causing double free corrpution?
73  this->cm_->~ControllerManager();
74  this->rosnode_->shutdown();
75 #ifdef USE_CBQ
76  this->controller_manager_queue_.clear();
77  this->controller_manager_queue_.disable();
78  this->controller_manager_callback_queue_thread_.join();
79 #endif
80  this->ros_spinner_thread_.join();
81 
82 
83 
84 
85  delete this->cm_;
86  delete this->rosnode_;
87 
88  if (this->fake_state_)
89  {
90  // why does this cause double free corrpution in destruction of RobotState?
91  //this->fake_state_->~RobotState();
92  delete this->fake_state_;
93  }
94 }
95 
96 void GazeboRosControllerManager::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
97 {
98  // Get then name of the parent model
99  std::string modelName = _sdf->GetParent()->Get<std::string>("name");
100 
101  // Get the world name.
102  this->world = _parent->GetWorld();
103 
104  // Get a pointer to the model
105  this->parent_model_ = _parent;
106 
107  // Error message if the model couldn't be found
108  if (!this->parent_model_)
109  gzerr << "Unable to get parent model\n";
110 
111  // Listen to the update event. This event is broadcast every
112  // simulation iteration.
113  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
114  boost::bind(&GazeboRosControllerManager::UpdateChild, this));
115  gzdbg << "plugin model name: " << modelName << "\n";
116 
117 
118 
119 
120  if (getenv("CHECK_SPEEDUP"))
121  {
122 #if GAZEBO_MAJOR_VERSION >= 8
123  wall_start_ = this->world->RealTime().Double();
124  sim_start_ = this->world->SimTime().Double();
125 #else
126  wall_start_ = this->world->GetRealTime().Double();
127  sim_start_ = this->world->GetSimTime().Double();
128 #endif
129  }
130 
131  // check update rate against world physics update rate
132  // should be equal or higher to guarantee the wrench applied is not "diluted"
133  //if (this->updatePeriod > 0 &&
134  // (this->world->GetPhysicsEngine()->GetUpdateRate() > 1.0/this->updatePeriod))
135  // ROS_ERROR("gazebo_ros_force controller update rate is less than physics update rate, wrench applied will be diluted (applied intermittently)");
136 
137 
138 
139 
140 
141  // get parameter name
142  this->robotNamespace = "";
143  if (_sdf->HasElement("robotNamespace"))
144  this->robotNamespace = _sdf->Get<std::string>("robotNamespace");
145 
146  this->robotParam = "robot_description";
147  if (_sdf->HasElement("robotParam"))
148  this->robotParam = _sdf->Get<std::string>("robotParam");
149 
150  this->robotParam = this->robotNamespace+"/" + this->robotParam;
151 
152  if (!ros::isInitialized())
153  {
154  int argc = 0;
155  char** argv = NULL;
157  }
158  this->rosnode_ = new ros::NodeHandle(this->robotNamespace);
159  ROS_INFO("starting gazebo_ros_controller_manager plugin in ns: %s",this->robotNamespace.c_str());
160 
161  // pr2_etherCAT calls ros::spin(), we'll thread out one spinner here to mimic that
162  this->ros_spinner_thread_ = boost::thread( boost::bind( &GazeboRosControllerManager::ControllerManagerROSThread,this ) );
163 
164 
165  // load a controller manager
167 #if GAZEBO_MAJOR_VERSION >= 8
168  this->hw_.current_time_ = ros::Time(this->world->SimTime().Double());
169 #else
170  this->hw_.current_time_ = ros::Time(this->world->GetSimTime().Double());
171 #endif
172  if (this->hw_.current_time_ < ros::Time(0.001)) this->hw_.current_time_ == ros::Time(0.001); // hardcoded to minimum of 1ms on start up
173 
174  this->rosnode_->param("gazebo/start_robot_calibrated",this->fake_calibration_,true);
175 
176  // read pr2 urdf
177  // setup actuators, then setup mechanism control node
178  ReadPr2Xml();
179 
180  // Initializes the fake state (for running the transmissions backwards).
182 
183  // The gazebo joints and mechanism joints should match up.
184  if (this->cm_->state_ != NULL) // could be NULL if ReadPr2Xml is unsuccessful
185  {
186  for (unsigned int i = 0; i < this->cm_->state_->joint_states_.size(); ++i)
187  {
188  std::string joint_name = this->cm_->state_->joint_states_[i].joint_->name;
189 
190  // fill in gazebo joints pointer
191  gazebo::physics::JointPtr joint = this->parent_model_->GetJoint(joint_name);
192  if (joint)
193  {
194  this->joints_.push_back(joint);
195  }
196  else
197  {
198  //ROS_WARN("A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
199  //this->joints_.push_back(NULL); // FIXME: cannot be null, must be an empty boost shared pointer
200  this->joints_.push_back(gazebo::physics::JointPtr()); // FIXME: cannot be null, must be an empty boost shared pointer
201  ROS_ERROR("A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
202  }
203 
204  }
205  }
206 
207 #ifdef USE_CBQ
208  // start custom queue for controller manager
209  this->controller_manager_callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosControllerManager::ControllerManagerQueueThread,this ) );
210 #endif
211 
212 }
213 
214 
216 {
217  if (this->world->IsPaused()) return;
218 
219  if (getenv("CHECK_SPEEDUP"))
220  {
221 #if GAZEBO_MAJOR_VERSION >= 8
222  double wall_elapsed = this->world->RealTime().Double() - wall_start_;
223  double sim_elapsed = this->world->SimTime().Double() - sim_start_;
224 #else
225  double wall_elapsed = this->world->GetRealTime().Double() - wall_start_;
226  double sim_elapsed = this->world->GetSimTime().Double() - sim_start_;
227 #endif
228  std::cout << " real time: " << wall_elapsed
229  << " sim time: " << sim_elapsed
230  << " speed up: " << sim_elapsed / wall_elapsed
231  << std::endl;
232  }
233  assert(this->joints_.size() == this->fake_state_->joint_states_.size());
234 
235  //--------------------------------------------------
236  // Pushes out simulation state
237  //--------------------------------------------------
238 
239  //ROS_ERROR("joints_.size()[%d]",(int)this->joints_.size());
240  // Copies the state from the gazebo joints into the mechanism joints.
241  for (unsigned int i = 0; i < this->joints_.size(); ++i)
242  {
243  if (!this->joints_[i])
244  continue;
245 
246  this->fake_state_->joint_states_[i].measured_effort_ = this->fake_state_->joint_states_[i].commanded_effort_;
247 
248  if (this->joints_[i]->HasType(gazebo::physics::Base::HINGE_JOINT))
249  {
250  gazebo::physics::JointPtr hj = this->joints_[i];
251 #if GAZEBO_MAJOR_VERSION >= 8
252  this->fake_state_->joint_states_[i].position_ = this->fake_state_->joint_states_[i].position_ +
253  angles::shortest_angular_distance(this->fake_state_->joint_states_[i].position_,hj->Position(0));
254 #else
255  this->fake_state_->joint_states_[i].position_ = this->fake_state_->joint_states_[i].position_ +
256  angles::shortest_angular_distance(this->fake_state_->joint_states_[i].position_,hj->GetAngle(0).Radian());
257 #endif
258  this->fake_state_->joint_states_[i].velocity_ = hj->GetVelocity(0);
259  //if (this->joints_[i]->GetName() == "torso_lift_motor_screw_joint")
260  // ROS_WARN("joint[%s] [%f]",this->joints_[i]->GetName().c_str(), this->fake_state_->joint_states_[i].position_);
261  }
262  else if (this->joints_[i]->HasType(gazebo::physics::Base::SLIDER_JOINT))
263  {
264  gazebo::physics::JointPtr sj = this->joints_[i];
265  {
266 #if GAZEBO_MAJOR_VERSION >= 8
267  this->fake_state_->joint_states_[i].position_ = sj->Position(0);
268 #else
269  this->fake_state_->joint_states_[i].position_ = sj->GetAngle(0).Radian();
270 #endif
271  this->fake_state_->joint_states_[i].velocity_ = sj->GetVelocity(0);
272  }
273  //ROS_ERROR("joint[%s] is a slider [%f]",this->joints_[i]->GetName().c_str(),sj->GetAngle(0).Radian());
274  //if (this->joints_[i]->GetName() == "torso_lift_joint")
275  // ROS_WARN("joint[%s] [%f]",this->joints_[i]->GetName().c_str(), this->fake_state_->joint_states_[i].position_);
276  }
277  else
278  {
279  /*
280  ROS_WARN("joint[%s] is not hinge [%d] nor slider",this->joints_[i]->GetName().c_str(),
281  (unsigned int)gazebo::physics::Base::HINGE_JOINT
282  );
283  for (unsigned int j = 0; j < this->joints_[i]->GetTypeCount(); j++)
284  {
285  ROS_WARN(" types: %d hinge[%d] slider[%d]",(unsigned int)this->joints_[i]->GetType(j),(unsigned int)this->joints_[i]->HasType(gazebo::physics::Base::HINGE_JOINT),(unsigned int)this->joints_[i]->HasType(gazebo::physics::Base::SLIDER_JOINT));
286  }
287  */
288  }
289  }
290 
291  // Reverses the transmissions to propagate the joint position into the actuators.
293 
294  //--------------------------------------------------
295  // Runs Mechanism Control
296  //--------------------------------------------------
297 #if GAZEBO_MAJOR_VERSION >= 8
298  this->hw_.current_time_ = ros::Time(this->world->SimTime().Double());
299 #else
300  this->hw_.current_time_ = ros::Time(this->world->GetSimTime().Double());
301 #endif
302  try
303  {
304  if (this->cm_->state_ != NULL) // could be NULL if ReadPr2Xml is unsuccessful
305  this->cm_->update();
306  }
307  catch (const char* c)
308  {
309  if (strcmp(c,"dividebyzero")==0)
310  ROS_WARN("pid controller reports divide by zero error");
311  else
312  ROS_WARN("unknown const char* exception: %s", c);
313  }
314 
315  //--------------------------------------------------
316  // Takes in actuation commands
317  //--------------------------------------------------
318 
319  // Reverses the transmissions to propagate the actuator commands into the joints.
321 
322  // Copies the commands from the mechanism joints into the gazebo joints.
323  for (unsigned int i = 0; i < this->joints_.size(); ++i)
324  {
325  if (!this->joints_[i])
326  continue;
327 
328  double effort = this->fake_state_->joint_states_[i].commanded_effort_;
329 
330  double damping_coef = 0;
331  if (this->cm_->state_ != NULL) // could be NULL if ReadPr2Xml is unsuccessful
332  {
333  if (this->cm_->state_->joint_states_[i].joint_->dynamics)
334  damping_coef = this->cm_->state_->joint_states_[i].joint_->dynamics->damping;
335  }
336 
337  if (this->joints_[i]->HasType(gazebo::physics::Base::HINGE_JOINT))
338  {
339  gazebo::physics::JointPtr hj = this->joints_[i];
340  double current_velocity = hj->GetVelocity(0);
341  double damping_force = damping_coef * current_velocity;
342  double effort_command = effort - damping_force;
343  hj->SetForce(0,effort_command);
344  //if (this->joints_[i]->GetName() == "torso_lift_motor_screw_joint")
345  // ROS_ERROR("gazebo [%s] command [%f] damping [%f]",this->joints_[i]->GetName().c_str(), effort, damping_force);
346  }
347  else if (this->joints_[i]->HasType(gazebo::physics::Base::SLIDER_JOINT))
348  {
349  gazebo::physics::JointPtr sj = this->joints_[i];
350  double current_velocity = sj->GetVelocity(0);
351  double damping_force = damping_coef * current_velocity;
352  double effort_command = effort-damping_force;
353  sj->SetForce(0,effort_command);
354  //if (this->joints_[i]->GetName() == "torso_lift_joint")
355  // ROS_ERROR("gazebo [%s] command [%f] damping [%f]",this->joints_[i]->GetName().c_str(), effort, damping_force);
356  }
357  }
358 }
359 
360 
362 {
363 
364  std::string urdf_param_name;
365  std::string urdf_string;
366  // search and wait for robot_description on param server
367  while(urdf_string.empty())
368  {
369  ROS_INFO("gazebo controller manager plugin is waiting for urdf: %s on the param server. (make sure there is a rosparam by that name in the ros parameter server, otherwise, this plugin blocks simulation forever).", this->robotParam.c_str());
370  if (this->rosnode_->searchParam(this->robotParam,urdf_param_name))
371  {
372  this->rosnode_->getParam(urdf_param_name,urdf_string);
373  ROS_DEBUG("found upstream\n%s\n------\n%s\n------\n%s",this->robotParam.c_str(),urdf_param_name.c_str(),urdf_string.c_str());
374  }
375  else
376  {
377  this->rosnode_->getParam(this->robotParam,urdf_string);
378  ROS_DEBUG("found in node namespace\n%s\n------\n%s\n------\n%s",this->robotParam.c_str(),urdf_param_name.c_str(),urdf_string.c_str());
379  }
380  usleep(100000);
381  }
382  ROS_INFO("gazebo controller manager got pr2.xml from param server, parsing it...");
383 
384  // initialize TiXmlDocument doc with a string
385  TiXmlDocument doc;
386  if (!doc.Parse(urdf_string.c_str()) && doc.Error())
387  {
388  ROS_ERROR("Could not load the gazebo controller manager plugin's configuration file: %s\n",
389  urdf_string.c_str());
390  }
391  else
392  {
393  //doc.Print();
394  //std::cout << *(doc.RootElement()) << std::endl;
395 
396  // Pulls out the list of actuators used in the robot configuration.
397  struct GetActuators : public TiXmlVisitor
398  {
399  std::set<std::string> actuators;
400  virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
401  {
402  if (elt.ValueStr() == std::string("actuator") && elt.Attribute("name"))
403  actuators.insert(elt.Attribute("name"));
404  else if (elt.ValueStr() == std::string("rightActuator") && elt.Attribute("name"))
405  actuators.insert(elt.Attribute("name"));
406  else if (elt.ValueStr() == std::string("leftActuator") && elt.Attribute("name"))
407  actuators.insert(elt.Attribute("name"));
408  return true;
409  }
410  } get_actuators;
411  doc.RootElement()->Accept(&get_actuators);
412 
413  // Places the found actuators into the hardware interface.
414  std::set<std::string>::iterator it;
415  for (it = get_actuators.actuators.begin(); it != get_actuators.actuators.end(); ++it)
416  {
417  //std::cout << " adding actuator " << (*it) << std::endl;
419  pr2_actuator->state_.is_enabled_ = true;
420  this->hw_.addActuator(pr2_actuator);
421  }
422 
423  // Setup mechanism control node
424  this->cm_->initXml(doc.RootElement());
425 
426  for (unsigned int i = 0; i < this->cm_->state_->joint_states_.size(); ++i)
427  this->cm_->state_->joint_states_[i].calibrated_ = fake_calibration_;
428  }
429 }
430 
431 #ifdef USE_CBQ
432 // custom callback queue
434 void GazeboRosControllerManager::ControllerManagerQueueThread()
435 {
436  ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id());
437 
438  static const double timeout = 0.01;
439 
440  while (this->rosnode_->ok())
441  {
442  this->controller_manager_queue_.callAvailable(ros::WallDuration(timeout));
443  }
444 }
445 #endif
446 
448 {
449  ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id());
450 
451  //ros::Rate rate(1000);
452 
453  while (this->rosnode_->ok())
454  {
455  //rate.sleep(); // using rosrate gets stuck on model delete
456  usleep(1000);
457  ros::spinOnce();
458  }
459 }
460  // Register this plugin with the simulator
462 } // namespace gazebo
bool addActuator(Actuator *actuator)
pr2_controller_manager::ControllerManager * cm_
ROSCPP_DECL bool isInitialized()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_WARN(...)
std::string robotParam
set topic name of robot description parameter
std::vector< gazebo::physics::JointPtr > joints_
#define ROS_INFO(...)
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
pr2_hardware_interface::HardwareInterface hw_
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
pr2_mechanism_model::Robot model_
bool initXml(TiXmlElement *config)
#define ROS_INFO_STREAM(args)
std::vector< JointState > joint_states_
bool getParam(const std::string &key, std::string &s) const
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
bool ok() const
pr2_mechanism_model::RobotState * state_
pr2_mechanism_model::RobotState * fake_state_
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
bool setModelsJointsStates(pr2_gazebo_plugins::SetModelsJointsStates::Request &req, pr2_gazebo_plugins::SetModelsJointsStates::Response &res)
ros service callback
def shortest_angular_distance(from_angle, to_angle)
#define ROS_DEBUG(...)


pr2_gazebo_plugins
Author(s): John Hsu
autogenerated on Fri May 3 2019 02:24:24