gazebo_ros_control_plugin.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Open Source Robotics Foundation
00005  *  Copyright (c) 2013, The Johns Hopkins University
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Open Source Robotics Foundation
00019  *     nor the names of its contributors may be
00020  *     used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *********************************************************************/
00036 
00037 /* Author: Dave Coleman, Jonathan Bohren
00038    Desc:   Gazebo plugin for ros_control that allows 'hardware_interfaces' to be plugged in
00039    using pluginlib
00040 */
00041 
00042 // Boost
00043 #include <boost/bind.hpp>
00044 
00045 #include <gazebo_ros_control/gazebo_ros_control_plugin.h>
00046 #include <urdf/model.h>
00047 
00048 namespace gazebo_ros_control
00049 {
00050 
00051 GazeboRosControlPlugin::~GazeboRosControlPlugin()
00052 {
00053   // Disconnect from gazebo events
00054   gazebo::event::Events::DisconnectWorldUpdateBegin(update_connection_);
00055 }
00056 
00057 // Overloaded Gazebo entry point
00058 void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
00059 {
00060   ROS_INFO_STREAM_NAMED("gazebo_ros_control","Loading gazebo_ros_control plugin");
00061 
00062 
00063   // Save pointers to the model
00064   parent_model_ = parent;
00065   sdf_ = sdf;
00066 
00067   // Error message if the model couldn't be found
00068   if (!parent_model_)
00069   {
00070     ROS_ERROR_STREAM_NAMED("loadThread","parent model is NULL");
00071     return;
00072   }
00073 
00074   // Check that ROS has been initialized
00075   if(!ros::isInitialized())
00076   {
00077     ROS_FATAL_STREAM_NAMED("gazebo_ros_control","A ROS node for Gazebo has not been initialized, unable to load plugin. "
00078       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00079     return;
00080   }
00081 
00082   // Get namespace for nodehandle
00083   if(sdf_->HasElement("robotNamespace"))
00084   {
00085     robot_namespace_ = sdf_->GetElement("robotNamespace")->Get<std::string>();
00086   }
00087   else
00088   {
00089     robot_namespace_ = parent_model_->GetName(); // default
00090   }
00091 
00092   // Get robot_description ROS param name
00093   if (sdf_->HasElement("robotParam"))
00094   {
00095     robot_description_ = sdf_->GetElement("robotParam")->Get<std::string>();
00096   }
00097   else
00098   {
00099     robot_description_ = "robot_description"; // default
00100   }
00101 
00102   // Get the robot simulation interface type
00103   if(sdf_->HasElement("robotSimType"))
00104   {
00105     robot_hw_sim_type_str_ = sdf_->Get<std::string>("robotSimType");
00106   }
00107   else
00108   {
00109     robot_hw_sim_type_str_ = "gazebo_ros_control/DefaultRobotHWSim";
00110     ROS_DEBUG_STREAM_NAMED("loadThread","Using default plugin for RobotHWSim (none specified in URDF/SDF)\""<<robot_hw_sim_type_str_<<"\"");
00111   }
00112 
00113   // Get the Gazebo simulation period
00114   ros::Duration gazebo_period(parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize());
00115 
00116   // Decide the plugin control period
00117   if(sdf_->HasElement("controlPeriod"))
00118   {
00119     control_period_ = ros::Duration(sdf_->Get<double>("controlPeriod"));
00120 
00121     // Check the period against the simulation period
00122     if( control_period_ < gazebo_period )
00123     {
00124       ROS_ERROR_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<<control_period_
00125         <<" s) is faster than the gazebo simulation period ("<<gazebo_period<<" s).");
00126     }
00127     else if( control_period_ > gazebo_period )
00128     {
00129       ROS_WARN_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<<control_period_
00130         <<" s) is slower than the gazebo simulation period ("<<gazebo_period<<" s).");
00131     }
00132   }
00133   else
00134   {
00135     control_period_ = gazebo_period;
00136     ROS_DEBUG_STREAM_NAMED("gazebo_ros_control","Control period not found in URDF/SDF, defaulting to Gazebo period of "
00137       << control_period_);
00138   }
00139 
00140   // Get parameters/settings for controllers from ROS param server
00141   model_nh_ = ros::NodeHandle(robot_namespace_);
00142 
00143   // Initialize the emergency stop code.
00144   e_stop_active_ = false;
00145   last_e_stop_active_ = false;
00146   if (sdf_->HasElement("eStopTopic"))
00147   {
00148     const std::string e_stop_topic = sdf_->GetElement("eStopTopic")->Get<std::string>();
00149     e_stop_sub_ = model_nh_.subscribe(e_stop_topic, 1, &GazeboRosControlPlugin::eStopCB, this);
00150   }
00151 
00152   ROS_INFO_NAMED("gazebo_ros_control", "Starting gazebo_ros_control plugin in namespace: %s", robot_namespace_.c_str());
00153 
00154   // Read urdf from ros parameter server then
00155   // setup actuators and mechanism control node.
00156   // This call will block if ROS is not properly initialized.
00157   const std::string urdf_string = getURDF(robot_description_);
00158   if (!parseTransmissionsFromURDF(urdf_string))
00159   {
00160     ROS_ERROR_NAMED("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n");
00161     return;
00162   }
00163 
00164   // Load the RobotHWSim abstraction to interface the controllers with the gazebo model
00165   try
00166   {
00167     robot_hw_sim_loader_.reset
00168       (new pluginlib::ClassLoader<gazebo_ros_control::RobotHWSim>
00169         ("gazebo_ros_control",
00170           "gazebo_ros_control::RobotHWSim"));
00171 
00172     robot_hw_sim_ = robot_hw_sim_loader_->createInstance(robot_hw_sim_type_str_);
00173     urdf::Model urdf_model;
00174     const urdf::Model *const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL;
00175 
00176     if(!robot_hw_sim_->initSim(robot_namespace_, model_nh_, parent_model_, urdf_model_ptr, transmissions_))
00177     {
00178       ROS_FATAL_NAMED("gazebo_ros_control","Could not initialize robot simulation interface");
00179       return;
00180     }
00181 
00182     // Create the controller manager
00183     ROS_DEBUG_STREAM_NAMED("ros_control_plugin","Loading controller_manager");
00184     controller_manager_.reset
00185       (new controller_manager::ControllerManager(robot_hw_sim_.get(), model_nh_));
00186 
00187     // Listen to the update event. This event is broadcast every simulation iteration.
00188     update_connection_ =
00189       gazebo::event::Events::ConnectWorldUpdateBegin
00190       (boost::bind(&GazeboRosControlPlugin::Update, this));
00191 
00192   }
00193   catch(pluginlib::LibraryLoadException &ex)
00194   {
00195     ROS_FATAL_STREAM_NAMED("gazebo_ros_control","Failed to create robot simulation interface loader: "<<ex.what());
00196   }
00197 
00198   ROS_INFO_NAMED("gazebo_ros_control", "Loaded gazebo_ros_control.");
00199 }
00200 
00201 // Called by the world update start event
00202 void GazeboRosControlPlugin::Update()
00203 {
00204   // Get the simulation time and period
00205   gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime();
00206   ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
00207   ros::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;
00208 
00209   robot_hw_sim_->eStopActive(e_stop_active_);
00210 
00211   // Check if we should update the controllers
00212   if(sim_period >= control_period_) {
00213     // Store this simulation time
00214     last_update_sim_time_ros_ = sim_time_ros;
00215 
00216     // Update the robot simulation with the state of the gazebo model
00217     robot_hw_sim_->readSim(sim_time_ros, sim_period);
00218 
00219     // Compute the controller commands
00220     bool reset_ctrlrs;
00221     if (e_stop_active_)
00222     {
00223       reset_ctrlrs = false;
00224       last_e_stop_active_ = true;
00225     }
00226     else
00227     {
00228       if (last_e_stop_active_)
00229       {
00230         reset_ctrlrs = true;
00231         last_e_stop_active_ = false;
00232       }
00233       else
00234       {
00235         reset_ctrlrs = false;
00236       }
00237     }
00238     controller_manager_->update(sim_time_ros, sim_period, reset_ctrlrs);
00239   }
00240 
00241   // Update the gazebo model with the result of the controller
00242   // computation
00243   robot_hw_sim_->writeSim(sim_time_ros, sim_time_ros - last_write_sim_time_ros_);
00244   last_write_sim_time_ros_ = sim_time_ros;
00245 }
00246 
00247 // Called on world reset
00248 void GazeboRosControlPlugin::Reset()
00249 {
00250   // Reset timing variables to not pass negative update periods to controllers on world reset
00251   last_update_sim_time_ros_ = ros::Time();
00252   last_write_sim_time_ros_ = ros::Time();
00253 }
00254 
00255 // Get the URDF XML from the parameter server
00256 std::string GazeboRosControlPlugin::getURDF(std::string param_name) const
00257 {
00258   std::string urdf_string;
00259 
00260   // search and wait for robot_description on param server
00261   while (urdf_string.empty())
00262   {
00263     std::string search_param_name;
00264     if (model_nh_.searchParam(param_name, search_param_name))
00265     {
00266       ROS_INFO_ONCE_NAMED("gazebo_ros_control", "gazebo_ros_control plugin is waiting for model"
00267         " URDF in parameter [%s] on the ROS param server.", search_param_name.c_str());
00268 
00269       model_nh_.getParam(search_param_name, urdf_string);
00270     }
00271     else
00272     {
00273       ROS_INFO_ONCE_NAMED("gazebo_ros_control", "gazebo_ros_control plugin is waiting for model"
00274         " URDF in parameter [%s] on the ROS param server.", robot_description_.c_str());
00275 
00276       model_nh_.getParam(param_name, urdf_string);
00277     }
00278 
00279     usleep(100000);
00280   }
00281   ROS_DEBUG_STREAM_NAMED("gazebo_ros_control", "Recieved urdf from param server, parsing...");
00282 
00283   return urdf_string;
00284 }
00285 
00286 // Get Transmissions from the URDF
00287 bool GazeboRosControlPlugin::parseTransmissionsFromURDF(const std::string& urdf_string)
00288 {
00289   transmission_interface::TransmissionParser::parse(urdf_string, transmissions_);
00290   return true;
00291 }
00292 
00293 // Emergency stop callback
00294 void GazeboRosControlPlugin::eStopCB(const std_msgs::BoolConstPtr& e_stop_active)
00295 {
00296   e_stop_active_ = e_stop_active->data;
00297 }
00298 
00299 
00300 // Register this plugin with the simulator
00301 GZ_REGISTER_MODEL_PLUGIN(GazeboRosControlPlugin);
00302 } // namespace


gazebo_ros_control
Author(s): Jonathan Bohren, Dave Coleman
autogenerated on Thu Feb 23 2017 03:43:51