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


pr2_gazebo_plugins
Author(s): John Hsu
autogenerated on Sat Sep 5 2015 11:22:50