generic_hw_interface.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, PickNik LLC
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of PickNik LLC nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman
36  Desc: Helper ros_control hardware interface that loads configurations
37 */
38 
40 #include <limits>
41 
42 // ROS parameter loading
44 
46 {
48  : name_("generic_hw_interface")
49  , nh_(nh)
50  , use_rosparam_joint_limits_(false)
51  , use_soft_limits_if_available_(false)
52 {
53  // Check if the URDF model needs to be loaded
54  if (urdf_model == NULL)
55  loadURDF(nh, "robot_description");
56  else
57  urdf_model_ = urdf_model;
58 
59  // Load rosparams
60  ros::NodeHandle rpnh(nh_, "hardware_interface"); // TODO(davetcoleman): change the namespace to "generic_hw_interface" aka name_
61  std::size_t error = 0;
62  error += !rosparam_shortcuts::get(name_, rpnh, "joints", joint_names_);
64 }
65 
67 {
68  num_joints_ = joint_names_.size();
69 
70  // Status
71  joint_position_.resize(num_joints_, 0.0);
72  joint_velocity_.resize(num_joints_, 0.0);
73  joint_effort_.resize(num_joints_, 0.0);
74 
75  // Command
79 
80  // Limits
84  joint_effort_limits_.resize(num_joints_, 0.0);
85 
86  // Initialize interfaces for each joint
87  for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
88  {
89  ROS_DEBUG_STREAM_NAMED(name_, "Loading joint name: " << joint_names_[joint_id]);
90 
91  // Create joint state interface
93  joint_names_[joint_id], &joint_position_[joint_id], &joint_velocity_[joint_id], &joint_effort_[joint_id]));
94 
95  // Add command interfaces to joints
96  // TODO: decide based on transmissions?
99  position_joint_interface_.registerHandle(joint_handle_position);
100 
103  velocity_joint_interface_.registerHandle(joint_handle_velocity);
104 
107  effort_joint_interface_.registerHandle(joint_handle_effort);
108 
109  // Load the joint limits
110  registerJointLimits(joint_handle_position, joint_handle_velocity, joint_handle_effort, joint_id);
111  } // end for each joint
112 
113  registerInterface(&joint_state_interface_); // From RobotHW base class.
114  registerInterface(&position_joint_interface_); // From RobotHW base class.
115  registerInterface(&velocity_joint_interface_); // From RobotHW base class.
116  registerInterface(&effort_joint_interface_); // From RobotHW base class.
117 
118  ROS_INFO_STREAM_NAMED(name_, "GenericHWInterface Ready.");
119 }
120 
122  const hardware_interface::JointHandle &joint_handle_velocity,
123  const hardware_interface::JointHandle &joint_handle_effort,
124  std::size_t joint_id)
125 {
126  // Default values
127  joint_position_lower_limits_[joint_id] = -std::numeric_limits<double>::max();
128  joint_position_upper_limits_[joint_id] = std::numeric_limits<double>::max();
129  joint_velocity_limits_[joint_id] = std::numeric_limits<double>::max();
130  joint_effort_limits_[joint_id] = std::numeric_limits<double>::max();
131 
132  // Limits datastructures
133  joint_limits_interface::JointLimits joint_limits; // Position
134  joint_limits_interface::SoftJointLimits soft_limits; // Soft Position
135  bool has_joint_limits = false;
136  bool has_soft_limits = false;
137 
138  // Get limits from URDF
139  if (urdf_model_ == NULL)
140  {
141  ROS_WARN_STREAM_NAMED(name_, "No URDF model loaded, unable to get joint limits");
142  return;
143  }
144 
145  // Get limits from URDF
146  urdf::JointConstSharedPtr urdf_joint = urdf_model_->getJoint(joint_names_[joint_id]);
147 
148  // Get main joint limits
149  if (urdf_joint == NULL)
150  {
151  ROS_ERROR_STREAM_NAMED(name_, "URDF joint not found " << joint_names_[joint_id]);
152  return;
153  }
154 
155  // Get limits from URDF
156  if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits))
157  {
158  has_joint_limits = true;
159  ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has URDF position limits ["
160  << joint_limits.min_position << ", "
161  << joint_limits.max_position << "]");
162  if (joint_limits.has_velocity_limits)
163  ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has URDF velocity limit ["
164  << joint_limits.max_velocity << "]");
165  }
166  else
167  {
168  if (urdf_joint->type != urdf::Joint::CONTINUOUS)
169  ROS_WARN_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " does not have a URDF "
170  "position limit");
171  }
172 
173  // Get limits from ROS param
175  {
176  if (joint_limits_interface::getJointLimits(joint_names_[joint_id], nh_, joint_limits))
177  {
178  has_joint_limits = true;
180  "Joint " << joint_names_[joint_id] << " has rosparam position limits ["
181  << joint_limits.min_position << ", " << joint_limits.max_position << "]");
182  if (joint_limits.has_velocity_limits)
183  ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id]
184  << " has rosparam velocity limit ["
185  << joint_limits.max_velocity << "]");
186  } // the else debug message provided internally by joint_limits_interface
187  }
188 
189  // Get soft limits from URDF
191  {
192  if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits))
193  {
194  has_soft_limits = true;
195  ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has soft joint limits.");
196  }
197  else
198  {
199  ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " does not have soft joint "
200  "limits");
201  }
202  }
203 
204  // Quit we we haven't found any limits in URDF or rosparam server
205  if (!has_joint_limits)
206  {
207  return;
208  }
209 
210  // Copy position limits if available
211  if (joint_limits.has_position_limits)
212  {
213  // Slighly reduce the joint limits to prevent floating point errors
214  joint_limits.min_position += std::numeric_limits<double>::epsilon();
215  joint_limits.max_position -= std::numeric_limits<double>::epsilon();
216 
217  joint_position_lower_limits_[joint_id] = joint_limits.min_position;
218  joint_position_upper_limits_[joint_id] = joint_limits.max_position;
219  }
220 
221  // Copy velocity limits if available
222  if (joint_limits.has_velocity_limits)
223  {
224  joint_velocity_limits_[joint_id] = joint_limits.max_velocity;
225  }
226 
227  // Copy effort limits if available
228  if (joint_limits.has_effort_limits)
229  {
230  joint_effort_limits_[joint_id] = joint_limits.max_effort;
231  }
232 
233  if (has_soft_limits) // Use soft limits
234  {
235  ROS_DEBUG_STREAM_NAMED(name_, "Using soft saturation limits");
236  const joint_limits_interface::PositionJointSoftLimitsHandle soft_handle_position(joint_handle_position,
237  joint_limits, soft_limits);
238  pos_jnt_soft_limits_.registerHandle(soft_handle_position);
239  const joint_limits_interface::VelocityJointSoftLimitsHandle soft_handle_velocity(joint_handle_velocity,
240  joint_limits, soft_limits);
241  vel_jnt_soft_limits_.registerHandle(soft_handle_velocity);
242  const joint_limits_interface::EffortJointSoftLimitsHandle soft_handle_effort(joint_handle_effort, joint_limits,
243  soft_limits);
244  eff_jnt_soft_limits_.registerHandle(soft_handle_effort);
245  }
246  else // Use saturation limits
247  {
248  ROS_DEBUG_STREAM_NAMED(name_, "Using saturation limits (not soft limits)");
249 
250  const joint_limits_interface::PositionJointSaturationHandle sat_handle_position(joint_handle_position, joint_limits);
251  pos_jnt_sat_interface_.registerHandle(sat_handle_position);
252 
253  const joint_limits_interface::VelocityJointSaturationHandle sat_handle_velocity(joint_handle_velocity, joint_limits);
254  vel_jnt_sat_interface_.registerHandle(sat_handle_velocity);
255 
256  const joint_limits_interface::EffortJointSaturationHandle sat_handle_effort(joint_handle_effort, joint_limits);
257  eff_jnt_sat_interface_.registerHandle(sat_handle_effort);
258  }
259 }
260 
262 {
263  // Reset joint limits state, in case of mode switch or e-stop
266 }
267 
269 {
270  // WARNING: THIS IS NOT REALTIME SAFE
271  // FOR DEBUGGING ONLY, USE AT YOUR OWN ROBOT's RISK!
272  ROS_INFO_STREAM_THROTTLE(1, std::endl
273  << printStateHelper());
274 }
275 
277 {
278  std::stringstream ss;
279  std::cout.precision(15);
280 
281  for (std::size_t i = 0; i < num_joints_; ++i)
282  {
283  ss << "j" << i << ": " << std::fixed << joint_position_[i] << "\t ";
284  ss << std::fixed << joint_velocity_[i] << "\t ";
285  ss << std::fixed << joint_effort_[i] << std::endl;
286  }
287  return ss.str();
288 }
289 
291 {
292  std::stringstream ss;
293  std::cout.precision(15);
294  ss << " position velocity effort \n";
295  for (std::size_t i = 0; i < num_joints_; ++i)
296  {
297  ss << "j" << i << ": " << std::fixed << joint_position_command_[i] << "\t ";
298  ss << std::fixed << joint_velocity_command_[i] << "\t ";
299  ss << std::fixed << joint_effort_command_[i] << std::endl;
300  }
301  return ss.str();
302 }
303 
304 void GenericHWInterface::loadURDF(ros::NodeHandle &nh, std::string param_name)
305 {
306  std::string urdf_string;
307  urdf_model_ = new urdf::Model();
308 
309  // search and wait for robot_description on param server
310  while (urdf_string.empty() && ros::ok())
311  {
312  std::string search_param_name;
313  if (nh.searchParam(param_name, search_param_name))
314  {
315  ROS_INFO_STREAM_NAMED(name_, "Waiting for model URDF on the ROS param server at location: " <<
316  nh.getNamespace() << search_param_name);
317  nh.getParam(search_param_name, urdf_string);
318  }
319  else
320  {
321  ROS_INFO_STREAM_NAMED(name_, "Waiting for model URDF on the ROS param server at location: " <<
322  nh.getNamespace() << param_name);
323  nh.getParam(param_name, urdf_string);
324  }
325 
326  usleep(100000);
327  }
328 
329  if (!urdf_model_->initString(urdf_string))
330  ROS_ERROR_STREAM_NAMED(name_, "Unable to load URDF model");
331  else
332  ROS_DEBUG_STREAM_NAMED(name_, "Received URDF from param server");
333 }
334 
335 } // namespace
hardware_interface::EffortJointInterface effort_joint_interface_
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
URDF_EXPORT bool initString(const std::string &xmlstring)
hardware_interface::JointStateInterface joint_state_interface_
#define ROS_INFO_STREAM_THROTTLE(period, args)
virtual void loadURDF(ros::NodeHandle &nh, std::string param_name)
Get the URDF XML from the parameter server.
#define ROS_INFO_STREAM_NAMED(name, args)
joint_limits_interface::EffortJointSaturationInterface eff_jnt_sat_interface_
virtual void init()
Initialize the hardware interface.
virtual void registerJointLimits(const hardware_interface::JointHandle &joint_handle_position, const hardware_interface::JointHandle &joint_handle_velocity, const hardware_interface::JointHandle &joint_handle_effort, std::size_t joint_id)
Register the limits of the joint specified by joint_id and joint_handle. The limits are retrieved fro...
joint_limits_interface::VelocityJointSaturationInterface vel_jnt_sat_interface_
bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits &soft_limits)
bool searchParam(const std::string &key, std::string &result) const
virtual void printState()
Helper for debugging a joint&#39;s state.
hardware_interface::VelocityJointInterface velocity_joint_interface_
joint_limits_interface::EffortJointSoftLimitsInterface eff_jnt_soft_limits_
ROSCPP_DECL bool ok()
joint_limits_interface::PositionJointSoftLimitsInterface pos_jnt_soft_limits_
const std::string & getNamespace() const
virtual void reset()
Set all members to default values.
std::string printCommandHelper()
Helper for debugging a joint&#39;s command.
void registerHandle(const JointStateHandle &handle)
JointStateHandle getHandle(const std::string &name)
void shutdownIfError(const std::string &parent_name, std::size_t error_count)
bool getParam(const std::string &key, std::string &s) const
bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits &limits)
joint_limits_interface::VelocityJointSoftLimitsInterface vel_jnt_soft_limits_
GenericHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model=NULL)
Constructor.
bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, bool &value)
hardware_interface::PositionJointInterface position_joint_interface_
joint_limits_interface::PositionJointSaturationInterface pos_jnt_sat_interface_
#define ROS_WARN_STREAM_NAMED(name, args)


ros_control_boilerplate
Author(s): Dave Coleman
autogenerated on Sat Jun 8 2019 18:06:49