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


pr2_gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Thu Jan 2 2014 11:45:00