gazebo_ros_controller_manager.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <sr_gazebo_plugins/gazebo_ros_controller_manager.h>
00031 #include <fstream>
00032 #include <iostream>
00033 #include <math.h>
00034 #include <unistd.h>
00035 #include <set>
00036 
00037 //#include <gazebo/XMLConfig.hh>
00038 //#include "physics/physics.h"
00039 #include <gazebo/physics/World.hh>
00040 #include <gazebo/physics/HingeJoint.hh>
00041 #include <gazebo/sensors/Sensor.hh>
00042 #include <gazebo/common/Exception.hh>
00043 #include <gazebo/physics/PhysicsTypes.hh>
00044 #include <gazebo/physics/Base.hh>
00045 
00046 #include <sdf/sdf.hh>
00047 #include <sdf/Param.hh>
00048 
00049 #include <angles/angles.h>
00050 #include <urdf/model.h>
00051 #include <map>
00052 
00053 #include <sr_hardware_interface/sr_actuator.hpp>
00054 
00055 namespace gazebo {
00056 
00057 GazeboRosControllerManager::GazeboRosControllerManager()
00058 {
00059 }
00060 
00061 
00063 //bool setModelsJointsStates(pr2_gazebo_plugins::SetModelsJointsStates::Request &req,
00064 //                           pr2_gazebo_plugins::SetModelsJointsStates::Response &res)
00065 //{
00066 //
00067 //  return true;
00068 //}
00069 
00070 
00071 GazeboRosControllerManager::~GazeboRosControllerManager()
00072 {
00073   ROS_DEBUG("Calling FiniChild in GazeboRosControllerManager");
00074 
00075   //pr2_hardware_interface::ActuatorMap::const_iterator it;
00076   //for (it = hw_.actuators_.begin(); it != hw_.actuators_.end(); ++it)
00077   //  delete it->second; // why is this causing double free corruption?
00078   this->cm_->~ControllerManager();
00079   this->rosnode_->shutdown();
00080 #ifdef USE_CBQ
00081   this->controller_manager_queue_.clear();
00082   this->controller_manager_queue_.disable();
00083   this->controller_manager_callback_queue_thread_.join();
00084 #endif
00085   this->ros_spinner_thread_.join();
00086 
00087 
00088 
00089 
00090   delete this->cm_;
00091   delete this->rosnode_;
00092 
00093   if (this->fake_state_)
00094   {
00095     // why does this cause double free corruption in destruction of RobotState?
00096     //this->fake_state_->~RobotState();
00097     delete this->fake_state_;
00098   }
00099 }
00100 
00101 
00102 void GazeboRosControllerManager::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
00103 {
00104   // Get then name of the parent model
00105   std::string modelName = _sdf->GetParent()->Get<std::string>("name");
00106 
00107   // Get the world name.
00108   this->world = _parent->GetWorld();
00109 
00110   // Get a pointer to the model
00111   this->parent_model_ = _parent;
00112 
00113   // Error message if the model couldn't be found
00114   if (!this->parent_model_)
00115     gzerr << "Unable to get parent model\n";
00116 
00117   // Listen to the update event. This event is broadcast every
00118   // simulation iteration.
00119   this->updateConnection = event::Events::ConnectWorldUpdateBegin(
00120       boost::bind(&GazeboRosControllerManager::UpdateChild, this));
00121   gzdbg << "plugin model name: " << modelName << "\n";
00122 
00123 
00124 
00125 
00126   if (getenv("CHECK_SPEEDUP"))
00127   {
00128     wall_start_ = this->world->GetRealTime().Double();
00129     sim_start_  = this->world->GetSimTime().Double();
00130   }
00131 
00132   // check update rate against world physics update rate
00133   // should be equal or higher to guarantee the wrench applied is not "diluted"
00134   //if (this->updatePeriod > 0 &&
00135   //    (this->world->GetPhysicsEngine()->GetUpdateRate() > 1.0/this->updatePeriod))
00136   //  ROS_ERROR("gazebo_ros_force controller update rate is less than physics update rate, wrench applied will be diluted (applied intermittently)");
00137 
00138 
00139 
00140 
00141 
00142   // get parameter name
00143   this->robotNamespace = "";
00144   if (_sdf->HasElement("robotNamespace"))
00145     this->robotNamespace = _sdf->GetElement("robotNamespace")->Get<std::string>();
00146 
00147   this->robotParam = "robot_description";
00148   if (_sdf->HasElement("robotParam"))
00149     this->robotParam = _sdf->GetElement("robotParam")->Get<std::string>();
00150 
00151   this->robotParam = this->robotNamespace+"/" + this->robotParam;
00152 
00153   if (!ros::isInitialized())
00154   {
00155     int argc = 0;
00156     char** argv = NULL;
00157     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00158   }
00159   this->rosnode_ = new ros::NodeHandle(this->robotNamespace);
00160   ROS_INFO("starting gazebo_ros_controller_manager plugin in ns: %s",this->robotNamespace.c_str());
00161 
00162   // Use the robots namespace, not gazebos
00163   self_test_.reset(new shadow_robot::SrSelfTest(true, this->robotNamespace));
00164 
00165   // pr2_etherCAT calls ros::spin(), we'll thread out one spinner here to mimic that
00166   this->ros_spinner_thread_ = boost::thread( boost::bind( &GazeboRosControllerManager::ControllerManagerROSThread,this ) );
00167 
00168 
00169   // load a controller manager
00170   this->cm_ = new pr2_controller_manager::ControllerManager(&hw_,*this->rosnode_);
00171   this->hw_.current_time_ = ros::Time(this->world->GetSimTime().Double());
00172   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
00173 
00174   this->rosnode_->param("gazebo/start_robot_calibrated",this->fake_calibration_,true);
00175 
00176   // read pr2 urdf
00177   // setup actuators, then setup mechanism control node
00178   ReadPr2Xml();
00179 
00180   // Initializes the fake state (for running the transmissions backwards).
00181   this->fake_state_ = new pr2_mechanism_model::RobotState(&this->cm_->model_);
00182 
00183   // The gazebo joints and mechanism joints should match up.
00184   if (this->cm_->state_ != NULL) // could be NULL if ReadPr2Xml is unsuccessful
00185   {
00186     for (unsigned int i = 0; i < this->cm_->state_->joint_states_.size(); ++i)
00187     {
00188       std::string joint_name = this->cm_->state_->joint_states_[i].joint_->name;
00189 
00190       // fill in gazebo joints pointer
00191       gazebo::physics::JointPtr joint = this->parent_model_->GetJoint(joint_name);
00192       if (joint)
00193       {
00194         this->joints_.push_back(joint);
00195       }
00196       else
00197       {
00198         //ROS_WARN("A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
00199         //this->joints_.push_back(NULL);  // FIXME: cannot be null, must be an empty boost shared pointer
00200         this->joints_.push_back(gazebo::physics::JointPtr());  // FIXME: cannot be null, must be an empty boost shared pointer
00201         ROS_ERROR("A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
00202       }
00203 
00204     }
00205   }
00206 
00207 #ifdef USE_CBQ
00208   // start custom queue for controller manager
00209   this->controller_manager_callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosControllerManager::ControllerManagerQueueThread,this ) );
00210 #endif
00211 
00212 }
00213 
00214 
00215 void GazeboRosControllerManager::UpdateChild()
00216 {
00217   if (this->world->IsPaused()) return;
00218 
00219   if (getenv("CHECK_SPEEDUP"))
00220   {
00221     double wall_elapsed = this->world->GetRealTime().Double() - wall_start_;
00222     double sim_elapsed  = this->world->GetSimTime().Double()  - sim_start_;
00223     std::cout << " real time: " <<  wall_elapsed
00224               << "  sim time: " <<  sim_elapsed
00225               << "  speed up: " <<  sim_elapsed / wall_elapsed
00226               << std::endl;
00227   }
00228   assert(this->joints_.size() == this->fake_state_->joint_states_.size());
00229 
00230   //--------------------------------------------------
00231   //  Pushes out simulation state
00232   //--------------------------------------------------
00233 
00234   //ROS_ERROR("joints_.size()[%d]",(int)this->joints_.size());
00235   // Copies the state from the gazebo joints into the mechanism joints.
00236   for (unsigned int i = 0; i < this->joints_.size(); ++i)
00237   {
00238     if (!this->joints_[i])
00239       continue;
00240 
00241     this->fake_state_->joint_states_[i].measured_effort_ = this->fake_state_->joint_states_[i].commanded_effort_;
00242 
00243     if (this->joints_[i]->HasType(gazebo::physics::Base::HINGE_JOINT))
00244     {
00245       gazebo::physics::JointPtr hj = this->joints_[i];
00246       this->fake_state_->joint_states_[i].position_ = this->fake_state_->joint_states_[i].position_ +
00247                     angles::shortest_angular_distance(this->fake_state_->joint_states_[i].position_,hj->GetAngle(0).Radian());
00248       this->fake_state_->joint_states_[i].velocity_ = hj->GetVelocity(0);
00249       //if (this->joints_[i]->GetName() == "torso_lift_motor_screw_joint")
00250       //  ROS_WARN("joint[%s] [%f]",this->joints_[i]->GetName().c_str(), this->fake_state_->joint_states_[i].position_);
00251     }
00252     else if (this->joints_[i]->HasType(gazebo::physics::Base::SLIDER_JOINT))
00253     {
00254       gazebo::physics::JointPtr sj = this->joints_[i];
00255       {
00256         this->fake_state_->joint_states_[i].position_ = sj->GetAngle(0).Radian();
00257         this->fake_state_->joint_states_[i].velocity_ = sj->GetVelocity(0);
00258       }
00259       //ROS_ERROR("joint[%s] is a slider [%f]",this->joints_[i]->GetName().c_str(),sj->GetAngle(0).GetAsRadian());
00260       //if (this->joints_[i]->GetName() == "torso_lift_joint")
00261       //  ROS_WARN("joint[%s] [%f]",this->joints_[i]->GetName().c_str(), this->fake_state_->joint_states_[i].position_);
00262     }
00263     else
00264     {
00265       /*
00266       ROS_WARN("joint[%s] is not hinge [%d] nor slider",this->joints_[i]->GetName().c_str(),
00267         (unsigned int)gazebo::physics::Base::HINGE_JOINT
00268         );
00269       for (unsigned int j = 0; j < this->joints_[i]->GetTypeCount(); j++)
00270       {
00271         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));
00272       }
00273       */
00274     }
00275   }
00276 
00277   // Reverses the transmissions to propagate the joint position into the actuators.
00278   this->fake_state_->propagateJointPositionToActuatorPosition();
00279 
00280   //--------------------------------------------------
00281   //  Runs Mechanism Control
00282   //--------------------------------------------------
00283   this->hw_.current_time_ = ros::Time(this->world->GetSimTime().Double());
00284   try
00285   {
00286     if (this->cm_->state_ != NULL) // could be NULL if ReadPr2Xml is unsuccessful
00287       this->cm_->update();
00288   }
00289   catch (const char* c)
00290   {
00291     if (strcmp(c,"dividebyzero")==0)
00292       ROS_WARN("pid controller reports divide by zero error");
00293     else
00294       ROS_WARN("unknown const char* exception: %s", c);
00295   }
00296 
00297   //--------------------------------------------------
00298   //  Takes in actuation commands
00299   //--------------------------------------------------
00300 
00301   // Reverses the transmissions to propagate the actuator commands into the joints.
00302   this->fake_state_->propagateActuatorEffortToJointEffort();
00303 
00304   // Copies the commands from the mechanism joints into the gazebo joints.
00305   for (unsigned int i = 0; i < this->joints_.size(); ++i)
00306   {
00307     if (!this->joints_[i])
00308       continue;
00309 
00310     double effort = this->fake_state_->joint_states_[i].commanded_effort_;
00311 
00312     double damping_coef = 0;
00313     if (this->cm_->state_ != NULL) // could be NULL if ReadPr2Xml is unsuccessful
00314     {
00315       if (this->cm_->state_->joint_states_[i].joint_->dynamics)
00316         damping_coef = this->cm_->state_->joint_states_[i].joint_->dynamics->damping;
00317     }
00318 
00319     if (this->joints_[i]->HasType(gazebo::physics::Base::HINGE_JOINT))
00320     {
00321       gazebo::physics::JointPtr hj = this->joints_[i];
00322       double current_velocity = hj->GetVelocity(0);
00323       double damping_force = damping_coef * current_velocity;
00324       double effort_command = effort - damping_force;
00325       hj->SetForce(0,effort_command);
00326       //if (this->joints_[i]->GetName() == "torso_lift_motor_screw_joint")
00327       //  ROS_ERROR("gazebo [%s] command [%f] damping [%f]",this->joints_[i]->GetName().c_str(), effort, damping_force);
00328     }
00329     else if (this->joints_[i]->HasType(gazebo::physics::Base::SLIDER_JOINT))
00330     {
00331       gazebo::physics::JointPtr sj = this->joints_[i];
00332       double current_velocity = sj->GetVelocity(0);
00333       double damping_force = damping_coef * current_velocity;
00334       double effort_command = effort-damping_force;
00335       sj->SetForce(0,effort_command);
00336       //if (this->joints_[i]->GetName() == "torso_lift_joint")
00337       //  ROS_ERROR("gazebo [%s] command [%f] damping [%f]",this->joints_[i]->GetName().c_str(), effort, damping_force);
00338     }
00339   }
00340 }
00341 
00342 void GazeboRosControllerManager::ReadPr2Xml()
00343 {
00344 
00345   std::string urdf_param_name;
00346   std::string urdf_string;
00347   // search and wait for robot_description on param server
00348   while(urdf_string.empty())
00349   {
00350     ROS_DEBUG("gazebo controller manager plugin is waiting for urdf: %s on the param server.", this->robotParam.c_str());
00351     if (this->rosnode_->searchParam(this->robotParam,urdf_param_name))
00352     {
00353       this->rosnode_->getParam(urdf_param_name,urdf_string);
00354       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());
00355     }
00356     else
00357     {
00358       this->rosnode_->getParam(this->robotParam,urdf_string);
00359       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());
00360     }
00361     usleep(100000);
00362   }
00363   ROS_DEBUG("gazebo controller manager got pr2.xml from param server, parsing it...");
00364 
00365   // initialize TiXmlDocument doc with a string
00366   TiXmlDocument doc;
00367   if (!doc.Parse(urdf_string.c_str()) && doc.Error())
00368   {
00369     ROS_ERROR("Could not load the gazebo controller manager plugin's configuration file: %s\n",
00370             urdf_string.c_str());
00371   }
00372   else
00373   {
00374     //doc.Print();
00375     //std::cout << *(doc.RootElement()) << std::endl;
00376 
00377     // Pulls out the list of actuators used in the robot configuration.
00378     struct GetActuators : public TiXmlVisitor
00379     {
00380       std::set<std::string> actuators;
00381       virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
00382       {
00383         if (elt.ValueStr() == std::string("actuator") && elt.Attribute("name"))
00384           actuators.insert(elt.Attribute("name"));
00385         else if (elt.ValueStr() == std::string("rightActuator") && elt.Attribute("name"))
00386           actuators.insert(elt.Attribute("name"));
00387         else if (elt.ValueStr() == std::string("leftActuator") && elt.Attribute("name"))
00388           actuators.insert(elt.Attribute("name"));
00389         return true;
00390       }
00391     } get_actuators;
00392     doc.RootElement()->Accept(&get_actuators);
00393 
00394     // Places the found actuators into the hardware interface.
00395     std::set<std::string>::iterator it;
00396     for (it = get_actuators.actuators.begin(); it != get_actuators.actuators.end(); ++it)
00397     {
00398       //std::cout << " adding actuator " << (*it) << std::endl;
00399       pr2_hardware_interface::Actuator* pr2_actuator = new sr_actuator::SrActuator(*it);
00400       pr2_actuator->state_.is_enabled_ = true;
00401       this->hw_.addActuator(pr2_actuator);
00402     }
00403 
00404     // Setup mechanism control node
00405     this->cm_->initXml(doc.RootElement());
00406 
00407     for (unsigned int i = 0; i < this->cm_->state_->joint_states_.size(); ++i)
00408       this->cm_->state_->joint_states_[i].calibrated_ = fake_calibration_;
00409   }
00410 
00411 }
00412 
00413 #ifdef USE_CBQ
00414 
00415 // custom callback queue
00416 void GazeboRosControllerManager::ControllerManagerQueueThread()
00417 {
00418   ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id());
00419 
00420   static const double timeout = 0.01;
00421 
00422   while (this->rosnode_->ok())
00423   {
00424     this->controller_manager_queue_.callAvailable(ros::WallDuration(timeout));
00425   }
00426 }
00427 #endif
00428 
00429 void GazeboRosControllerManager::ControllerManagerROSThread()
00430 {
00431   ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id());
00432 
00433   //ros::Rate rate(1000);
00434 
00435   while (this->rosnode_->ok())
00436   {
00437     self_test_->checkTest();
00438     //rate.sleep(); // using rosrate gets stuck on model delete
00439     usleep(1000);
00440     ros::spinOnce();
00441   }
00442 }
00443   // Register this plugin with the simulator
00444   GZ_REGISTER_MODEL_PLUGIN(GazeboRosControllerManager)
00445 } // namespace gazebo


sr_gazebo_plugins
Author(s): adapted by Ugo(software@shadowrobot.com), Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Fri Aug 28 2015 13:10:13