generic_hw_interface.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, PickNik LLC
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of PickNik LLC nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman
00036    Desc:   Helper ros_control hardware interface that loads configurations
00037 */
00038 
00039 #include <ros_control_boilerplate/generic_hw_interface.h>
00040 #include <limits>
00041 
00042 // ROS parameter loading
00043 #include <rosparam_shortcuts/rosparam_shortcuts.h>
00044 
00045 namespace ros_control_boilerplate
00046 {
00047 GenericHWInterface::GenericHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model)
00048   : name_("generic_hw_interface")
00049   , nh_(nh)
00050   , use_rosparam_joint_limits_(false)
00051   , use_soft_limits_if_available_(false)
00052 {
00053   // Check if the URDF model needs to be loaded
00054   if (urdf_model == NULL)
00055     loadURDF(nh, "robot_description");
00056   else
00057     urdf_model_ = urdf_model;
00058 
00059   // Load rosparams
00060   ros::NodeHandle rpnh(nh_, "hardware_interface"); // TODO(davetcoleman): change the namespace to "generic_hw_interface" aka name_
00061   std::size_t error = 0;
00062   error += !rosparam_shortcuts::get(name_, rpnh, "joints", joint_names_);
00063   rosparam_shortcuts::shutdownIfError(name_, error);
00064 }
00065 
00066 void GenericHWInterface::init()
00067 {
00068   num_joints_ = joint_names_.size();
00069 
00070   // Status
00071   joint_position_.resize(num_joints_, 0.0);
00072   joint_velocity_.resize(num_joints_, 0.0);
00073   joint_effort_.resize(num_joints_, 0.0);
00074 
00075   // Command
00076   joint_position_command_.resize(num_joints_, 0.0);
00077   joint_velocity_command_.resize(num_joints_, 0.0);
00078   joint_effort_command_.resize(num_joints_, 0.0);
00079 
00080   // Limits
00081   joint_position_lower_limits_.resize(num_joints_, 0.0);
00082   joint_position_upper_limits_.resize(num_joints_, 0.0);
00083   joint_velocity_limits_.resize(num_joints_, 0.0);
00084   joint_effort_limits_.resize(num_joints_, 0.0);
00085 
00086   // Initialize interfaces for each joint
00087   for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
00088   {
00089     ROS_DEBUG_STREAM_NAMED(name_, "Loading joint name: " << joint_names_[joint_id]);
00090 
00091     // Create joint state interface
00092     joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(
00093         joint_names_[joint_id], &joint_position_[joint_id], &joint_velocity_[joint_id], &joint_effort_[joint_id]));
00094 
00095     // Add command interfaces to joints
00096     // TODO: decide based on transmissions?
00097     hardware_interface::JointHandle joint_handle_position = hardware_interface::JointHandle(
00098         joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_position_command_[joint_id]);
00099     position_joint_interface_.registerHandle(joint_handle_position);
00100 
00101     hardware_interface::JointHandle joint_handle_velocity = hardware_interface::JointHandle(
00102         joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_velocity_command_[joint_id]);
00103     velocity_joint_interface_.registerHandle(joint_handle_velocity);
00104 
00105     hardware_interface::JointHandle joint_handle_effort = hardware_interface::JointHandle(
00106         joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_effort_command_[joint_id]);
00107     effort_joint_interface_.registerHandle(joint_handle_effort);
00108 
00109     // Load the joint limits
00110     registerJointLimits(joint_handle_position, joint_handle_velocity, joint_handle_effort, joint_id);
00111   }  // end for each joint
00112 
00113   registerInterface(&joint_state_interface_);     // From RobotHW base class.
00114   registerInterface(&position_joint_interface_);  // From RobotHW base class.
00115   registerInterface(&velocity_joint_interface_);  // From RobotHW base class.
00116   registerInterface(&effort_joint_interface_);    // From RobotHW base class.
00117 
00118   ROS_INFO_STREAM_NAMED(name_, "GenericHWInterface Ready.");
00119 }
00120 
00121 void GenericHWInterface::registerJointLimits(const hardware_interface::JointHandle &joint_handle_position,
00122                                              const hardware_interface::JointHandle &joint_handle_velocity,
00123                                              const hardware_interface::JointHandle &joint_handle_effort,
00124                                              std::size_t joint_id)
00125 {
00126   // Default values
00127   joint_position_lower_limits_[joint_id] = -std::numeric_limits<double>::max();
00128   joint_position_upper_limits_[joint_id] = std::numeric_limits<double>::max();
00129   joint_velocity_limits_[joint_id] = std::numeric_limits<double>::max();
00130   joint_effort_limits_[joint_id] = std::numeric_limits<double>::max();
00131 
00132   // Limits datastructures
00133   joint_limits_interface::JointLimits joint_limits;     // Position
00134   joint_limits_interface::SoftJointLimits soft_limits;  // Soft Position
00135   bool has_joint_limits = false;
00136   bool has_soft_limits = false;
00137 
00138   // Get limits from URDF
00139   if (urdf_model_ == NULL)
00140   {
00141     ROS_WARN_STREAM_NAMED(name_, "No URDF model loaded, unable to get joint limits");
00142     return;
00143   }
00144 
00145   // Get limits from URDF
00146   const boost::shared_ptr<const urdf::Joint> urdf_joint = urdf_model_->getJoint(joint_names_[joint_id]);
00147 
00148   // Get main joint limits
00149   if (urdf_joint == NULL)
00150   {
00151     ROS_ERROR_STREAM_NAMED(name_, "URDF joint not found " << joint_names_[joint_id]);
00152     return;
00153   }
00154 
00155   // Get limits from URDF
00156   if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits))
00157   {
00158     has_joint_limits = true;
00159     ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has URDF position limits ["
00160                                                             << joint_limits.min_position << ", "
00161                                                             << joint_limits.max_position << "]");
00162     if (joint_limits.has_velocity_limits)
00163       ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has URDF velocity limit ["
00164                                                               << joint_limits.max_velocity << "]");
00165   }
00166   else
00167   {
00168     if (urdf_joint->type != urdf::Joint::CONTINUOUS)
00169       ROS_WARN_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " does not have a URDF "
00170                             "position limit");
00171   }
00172 
00173   // Get limits from ROS param
00174   if (use_rosparam_joint_limits_)
00175   {
00176     if (joint_limits_interface::getJointLimits(joint_names_[joint_id], nh_, joint_limits))
00177     {
00178       has_joint_limits = true;
00179       ROS_DEBUG_STREAM_NAMED(name_,
00180                              "Joint " << joint_names_[joint_id] << " has rosparam position limits ["
00181                                       << joint_limits.min_position << ", " << joint_limits.max_position << "]");
00182       if (joint_limits.has_velocity_limits)
00183         ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id]
00184                                                                 << " has rosparam velocity limit ["
00185                                                                 << joint_limits.max_velocity << "]");
00186     }  // the else debug message provided internally by joint_limits_interface
00187   }
00188 
00189   // Get soft limits from URDF
00190   if (use_soft_limits_if_available_)
00191   {
00192     if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits))
00193     {
00194       has_soft_limits = true;
00195       ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has soft joint limits.");
00196     }
00197     else
00198     {
00199       ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " does not have soft joint "
00200                              "limits");
00201     }
00202   }
00203 
00204   // Quit we we haven't found any limits in URDF or rosparam server
00205   if (!has_joint_limits)
00206   {
00207     return;
00208   }
00209 
00210   // Copy position limits if available
00211   if (joint_limits.has_position_limits)
00212   {
00213     // Slighly reduce the joint limits to prevent floating point errors
00214     joint_limits.min_position += std::numeric_limits<double>::epsilon();
00215     joint_limits.max_position -= std::numeric_limits<double>::epsilon();
00216 
00217     joint_position_lower_limits_[joint_id] = joint_limits.min_position;
00218     joint_position_upper_limits_[joint_id] = joint_limits.max_position;
00219   }
00220 
00221   // Copy velocity limits if available
00222   if (joint_limits.has_velocity_limits)
00223   {
00224     joint_velocity_limits_[joint_id] = joint_limits.max_velocity;
00225   }
00226 
00227   // Copy effort limits if available
00228   if (joint_limits.has_effort_limits)
00229   {
00230     joint_effort_limits_[joint_id] = joint_limits.max_effort;
00231   }
00232 
00233   if (has_soft_limits)  // Use soft limits
00234   {
00235     ROS_DEBUG_STREAM_NAMED(name_, "Using soft saturation limits");
00236     const joint_limits_interface::PositionJointSoftLimitsHandle soft_handle_position(joint_handle_position,
00237                                                                                        joint_limits, soft_limits);
00238     pos_jnt_soft_limits_.registerHandle(soft_handle_position);
00239     const joint_limits_interface::VelocityJointSoftLimitsHandle soft_handle_velocity(joint_handle_velocity,
00240                                                                                        joint_limits, soft_limits);
00241     vel_jnt_soft_limits_.registerHandle(soft_handle_velocity);
00242     const joint_limits_interface::EffortJointSoftLimitsHandle soft_handle_effort(joint_handle_effort, joint_limits,
00243                                                                                    soft_limits);
00244     eff_jnt_soft_limits_.registerHandle(soft_handle_effort);
00245   }
00246   else  // Use saturation limits
00247   {
00248     ROS_DEBUG_STREAM_NAMED(name_, "Using saturation limits (not soft limits)");
00249 
00250     const joint_limits_interface::PositionJointSaturationHandle sat_handle_position(joint_handle_position, joint_limits);
00251     pos_jnt_sat_interface_.registerHandle(sat_handle_position);
00252 
00253     const joint_limits_interface::VelocityJointSaturationHandle sat_handle_velocity(joint_handle_velocity, joint_limits);
00254     vel_jnt_sat_interface_.registerHandle(sat_handle_velocity);
00255 
00256     const joint_limits_interface::EffortJointSaturationHandle sat_handle_effort(joint_handle_effort, joint_limits);
00257     eff_jnt_sat_interface_.registerHandle(sat_handle_effort);
00258   }
00259 }
00260 
00261 void GenericHWInterface::reset()
00262 {
00263   // Reset joint limits state, in case of mode switch or e-stop
00264   pos_jnt_sat_interface_.reset();
00265   pos_jnt_soft_limits_.reset();
00266 }
00267 
00268 void GenericHWInterface::printState()
00269 {
00270   // WARNING: THIS IS NOT REALTIME SAFE
00271   // FOR DEBUGGING ONLY, USE AT YOUR OWN ROBOT's RISK!
00272   ROS_INFO_STREAM_THROTTLE(1, std::endl
00273                                   << printStateHelper());
00274 }
00275 
00276 std::string GenericHWInterface::printStateHelper()
00277 {
00278   std::stringstream ss;
00279   std::cout.precision(15);
00280 
00281   for (std::size_t i = 0; i < num_joints_; ++i)
00282   {
00283     ss << "j" << i << ": " << std::fixed << joint_position_[i] << "\t ";
00284     ss << std::fixed << joint_velocity_[i] << "\t ";
00285     ss << std::fixed << joint_effort_[i] << std::endl;
00286   }
00287   return ss.str();
00288 }
00289 
00290 std::string GenericHWInterface::printCommandHelper()
00291 {
00292   std::stringstream ss;
00293   std::cout.precision(15);
00294   ss << "    position     velocity         effort  \n";
00295   for (std::size_t i = 0; i < num_joints_; ++i)
00296   {
00297     ss << "j" << i << ": " << std::fixed << joint_position_command_[i] << "\t ";
00298     ss << std::fixed << joint_velocity_command_[i] << "\t ";
00299     ss << std::fixed << joint_effort_command_[i] << std::endl;
00300   }
00301   return ss.str();
00302 }
00303 
00304 void GenericHWInterface::loadURDF(ros::NodeHandle &nh, std::string param_name)
00305 {
00306   std::string urdf_string;
00307   urdf_model_ = new urdf::Model();
00308 
00309   // search and wait for robot_description on param server
00310   while (urdf_string.empty() && ros::ok())
00311   {
00312     std::string search_param_name;
00313     if (nh.searchParam(param_name, search_param_name))
00314     {
00315       ROS_INFO_STREAM_NAMED(name_, "Waiting for model URDF on the ROS param server at location: " <<
00316                             nh.getNamespace() << search_param_name);
00317       nh.getParam(search_param_name, urdf_string);
00318     }
00319     else
00320     {
00321       ROS_INFO_STREAM_NAMED(name_, "Waiting for model URDF on the ROS param server at location: " <<
00322                             nh.getNamespace() << param_name);
00323       nh.getParam(param_name, urdf_string);
00324     }
00325 
00326     usleep(100000);
00327   }
00328 
00329   if (!urdf_model_->initString(urdf_string))
00330     ROS_ERROR_STREAM_NAMED(name_, "Unable to load URDF model");
00331   else
00332     ROS_DEBUG_STREAM_NAMED(name_, "Received URDF from param server");
00333 }
00334 
00335 }  // namespace


ros_control_boilerplate
Author(s): Dave Coleman
autogenerated on Sun Apr 24 2016 04:39:29