gazebo_ros_control_plugin.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Open Source Robotics Foundation
5  * Copyright (c) 2013, The Johns Hopkins University
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Open Source Robotics Foundation
19  * nor the names of its contributors may be
20  * used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *********************************************************************/
36 
37 /* Author: Dave Coleman, Jonathan Bohren
38  Desc: Gazebo plugin for ros_control that allows 'hardware_interfaces' to be plugged in
39  using pluginlib
40 */
41 
42 // Boost
43 #include <boost/bind.hpp>
44 
46 #include <urdf/model.h>
47 
48 namespace gazebo_ros_control
49 {
50 
52 {
53  // Disconnect from gazebo events
54  update_connection_.reset();
55 }
56 
57 // Overloaded Gazebo entry point
58 void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
59 {
60  ROS_INFO_STREAM_NAMED("gazebo_ros_control","Loading gazebo_ros_control plugin");
61 
62 
63  // Save pointers to the model
64  parent_model_ = parent;
65  sdf_ = sdf;
66 
67  // Error message if the model couldn't be found
68  if (!parent_model_)
69  {
70  ROS_ERROR_STREAM_NAMED("loadThread","parent model is NULL");
71  return;
72  }
73 
74  // Check that ROS has been initialized
75  if(!ros::isInitialized())
76  {
77  ROS_FATAL_STREAM_NAMED("gazebo_ros_control","A ROS node for Gazebo has not been initialized, unable to load plugin. "
78  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
79  return;
80  }
81 
82  // Get namespace for nodehandle
83  if(sdf_->HasElement("robotNamespace"))
84  {
85  robot_namespace_ = sdf_->GetElement("robotNamespace")->Get<std::string>();
86  }
87  else
88  {
89  robot_namespace_ = parent_model_->GetName(); // default
90  }
91 
92  // Get robot_description ROS param name
93  if (sdf_->HasElement("robotParam"))
94  {
95  robot_description_ = sdf_->GetElement("robotParam")->Get<std::string>();
96  }
97  else
98  {
99  robot_description_ = "robot_description"; // default
100  }
101 
102  // Get the robot simulation interface type
103  if(sdf_->HasElement("robotSimType"))
104  {
105  robot_hw_sim_type_str_ = sdf_->Get<std::string>("robotSimType");
106  }
107  else
108  {
109  robot_hw_sim_type_str_ = "gazebo_ros_control/DefaultRobotHWSim";
110  ROS_DEBUG_STREAM_NAMED("loadThread","Using default plugin for RobotHWSim (none specified in URDF/SDF)\""<<robot_hw_sim_type_str_<<"\"");
111  }
112 
113  // temporary fix to bug regarding the robotNamespace in default_robot_hw_sim.cpp (see #637)
114  std::string robot_ns = robot_namespace_;
115  if(robot_hw_sim_type_str_ == "gazebo_ros_control/DefaultRobotHWSim"){
116  if (sdf_->HasElement("legacyModeNS")) {
117  if( sdf_->GetElement("legacyModeNS")->Get<bool>() ){
118  robot_ns = "";
119  }
120  }else{
121  robot_ns = "";
122  ROS_ERROR("GazeboRosControlPlugin missing <legacyModeNS> while using DefaultRobotHWSim, defaults to true.\n"
123  "This setting assumes you have an old package with an old implementation of DefaultRobotHWSim, "
124  "where the robotNamespace is disregarded and absolute paths are used instead.\n"
125  "If you do not want to fix this issue in an old package just set <legacyModeNS> to true.\n"
126  );
127  }
128  }
129 
130  // Get the Gazebo simulation period
131 #if GAZEBO_MAJOR_VERSION >= 8
132  ros::Duration gazebo_period(parent_model_->GetWorld()->Physics()->GetMaxStepSize());
133 #else
134  ros::Duration gazebo_period(parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize());
135 #endif
136 
137  // Decide the plugin control period
138  if(sdf_->HasElement("controlPeriod"))
139  {
140  control_period_ = ros::Duration(sdf_->Get<double>("controlPeriod"));
141 
142  // Check the period against the simulation period
143  if( control_period_ < gazebo_period )
144  {
145  ROS_ERROR_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<<control_period_
146  <<" s) is faster than the gazebo simulation period ("<<gazebo_period<<" s).");
147  }
148  else if( control_period_ > gazebo_period )
149  {
150  ROS_WARN_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<<control_period_
151  <<" s) is slower than the gazebo simulation period ("<<gazebo_period<<" s).");
152  }
153  }
154  else
155  {
156  control_period_ = gazebo_period;
157  ROS_DEBUG_STREAM_NAMED("gazebo_ros_control","Control period not found in URDF/SDF, defaulting to Gazebo period of "
158  << control_period_);
159  }
160 
161  // Get parameters/settings for controllers from ROS param server
163 
164  // Initialize the emergency stop code.
165  e_stop_active_ = false;
166  last_e_stop_active_ = false;
167  if (sdf_->HasElement("eStopTopic"))
168  {
169  const std::string e_stop_topic = sdf_->GetElement("eStopTopic")->Get<std::string>();
171  }
172 
173  ROS_INFO_NAMED("gazebo_ros_control", "Starting gazebo_ros_control plugin in namespace: %s", robot_namespace_.c_str());
174 
175  // Read urdf from ros parameter server then
176  // setup actuators and mechanism control node.
177  // This call will block if ROS is not properly initialized.
178  const std::string urdf_string = getURDF(robot_description_);
179  if (!parseTransmissionsFromURDF(urdf_string))
180  {
181  ROS_ERROR_NAMED("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n");
182  return;
183  }
184 
185  // Load the RobotHWSim abstraction to interface the controllers with the gazebo model
186  try
187  {
190  ("gazebo_ros_control",
191  "gazebo_ros_control::RobotHWSim"));
192 
194  urdf::Model urdf_model;
195  const urdf::Model *const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL;
196 
197  if(!robot_hw_sim_->initSim(robot_ns, model_nh_, parent_model_, urdf_model_ptr, transmissions_))
198  {
199  ROS_FATAL_NAMED("gazebo_ros_control","Could not initialize robot simulation interface");
200  return;
201  }
202 
203  // Create the controller manager
204  ROS_DEBUG_STREAM_NAMED("ros_control_plugin","Loading controller_manager");
205  controller_manager_.reset
207 
208  // Listen to the update event. This event is broadcast every simulation iteration.
210  gazebo::event::Events::ConnectWorldUpdateBegin
211  (boost::bind(&GazeboRosControlPlugin::Update, this));
212 
213  }
215  {
216  ROS_FATAL_STREAM_NAMED("gazebo_ros_control","Failed to create robot simulation interface loader: "<<ex.what());
217  }
218 
219  ROS_INFO_NAMED("gazebo_ros_control", "Loaded gazebo_ros_control.");
220 }
221 
222 // Called by the world update start event
224 {
225  // Get the simulation time and period
226 #if GAZEBO_MAJOR_VERSION >= 8
227  gazebo::common::Time gz_time_now = parent_model_->GetWorld()->SimTime();
228 #else
229  gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime();
230 #endif
231  ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
232  ros::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;
233 
234  robot_hw_sim_->eStopActive(e_stop_active_);
235 
236  // Check if we should update the controllers
237  if(sim_period >= control_period_) {
238  // Store this simulation time
239  last_update_sim_time_ros_ = sim_time_ros;
240 
241  // Update the robot simulation with the state of the gazebo model
242  robot_hw_sim_->readSim(sim_time_ros, sim_period);
243 
244  // Compute the controller commands
245  bool reset_ctrlrs;
246  if (e_stop_active_)
247  {
248  reset_ctrlrs = false;
249  last_e_stop_active_ = true;
250  }
251  else
252  {
254  {
255  reset_ctrlrs = true;
256  last_e_stop_active_ = false;
257  }
258  else
259  {
260  reset_ctrlrs = false;
261  }
262  }
263  controller_manager_->update(sim_time_ros, sim_period, reset_ctrlrs);
264  }
265 
266  // Update the gazebo model with the result of the controller
267  // computation
268  robot_hw_sim_->writeSim(sim_time_ros, sim_time_ros - last_write_sim_time_ros_);
269  last_write_sim_time_ros_ = sim_time_ros;
270 }
271 
272 // Called on world reset
274 {
275  // Reset timing variables to not pass negative update periods to controllers on world reset
278 }
279 
280 // Get the URDF XML from the parameter server
281 std::string GazeboRosControlPlugin::getURDF(std::string param_name) const
282 {
283  std::string urdf_string;
284 
285  // search and wait for robot_description on param server
286  while (urdf_string.empty())
287  {
288  std::string search_param_name;
289  if (model_nh_.searchParam(param_name, search_param_name))
290  {
291  ROS_INFO_ONCE_NAMED("gazebo_ros_control", "gazebo_ros_control plugin is waiting for model"
292  " URDF in parameter [%s] on the ROS param server.", search_param_name.c_str());
293 
294  model_nh_.getParam(search_param_name, urdf_string);
295  }
296  else
297  {
298  ROS_INFO_ONCE_NAMED("gazebo_ros_control", "gazebo_ros_control plugin is waiting for model"
299  " URDF in parameter [%s] on the ROS param server.", robot_description_.c_str());
300 
301  model_nh_.getParam(param_name, urdf_string);
302  }
303 
304  usleep(100000);
305  }
306  ROS_DEBUG_STREAM_NAMED("gazebo_ros_control", "Recieved urdf from param server, parsing...");
307 
308  return urdf_string;
309 }
310 
311 // Get Transmissions from the URDF
312 bool GazeboRosControlPlugin::parseTransmissionsFromURDF(const std::string& urdf_string)
313 {
315  return true;
316 }
317 
318 // Emergency stop callback
319 void GazeboRosControlPlugin::eStopCB(const std_msgs::BoolConstPtr& e_stop_active)
320 {
321  e_stop_active_ = e_stop_active->data;
322 }
323 
324 
325 // Register this plugin with the simulator
327 } // namespace
static bool parse(const std::string &urdf_string, std::vector< TransmissionInfo > &transmissions)
#define ROS_INFO_NAMED(name,...)
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
boost::shared_ptr< controller_manager::ControllerManager > controller_manager_
URDF_EXPORT bool initString(const std::string &xmlstring)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL bool isInitialized()
#define ROS_INFO_STREAM_NAMED(name, args)
boost::shared_ptr< gazebo_ros_control::RobotHWSim > robot_hw_sim_
bool parseTransmissionsFromURDF(const std::string &urdf_string)
#define ROS_FATAL_STREAM_NAMED(name, args)
bool searchParam(const std::string &key, std::string &result) const
std::vector< transmission_interface::TransmissionInfo > transmissions_
GZ_REGISTER_MODEL_PLUGIN(GazeboRosControlPlugin)
#define ROS_INFO_ONCE_NAMED(name,...)
void eStopCB(const std_msgs::BoolConstPtr &e_stop_active)
bool getParam(const std::string &key, std::string &s) const
boost::shared_ptr< pluginlib::ClassLoader< gazebo_ros_control::RobotHWSim > > robot_hw_sim_loader_
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR_NAMED(name,...)
Plugin template for hardware interfaces for ros_control and Gazebo.
#define ROS_ERROR(...)
virtual void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
std::string getURDF(std::string param_name) const
#define ROS_WARN_STREAM_NAMED(name, args)


gazebo_ros_control
Author(s): Jonathan Bohren, Dave Coleman
autogenerated on Tue Apr 6 2021 02:19:50