joint_limits_rosparam.h

Go to the documentation of this file.

// Copyright (C) 2013, PAL Robotics S.L.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//   * Redistributions of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//   * Redistributions in binary form must reproduce the above copyright
//     notice, this list of conditions and the following disclaimer in the
//     documentation and/or other materials provided with the distribution.
//   * Neither the name of PAL Robotics S.L. nor the names of its
//     contributors may be used to endorse or promote products derived from
//     this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
 
 
#pragma once
 
 
#include <string>
 
#include <ros/ros.h>
#include <joint_limits_interface/joint_limits.h>
 
namespace joint_limits_interface
{
 
inline bool getJointLimits(const std::string& joint_name, const ros::NodeHandle& nh, JointLimits& limits)
{
  // Node handle scoped where the joint limits are defined
  ros::NodeHandle limits_nh;
  try
  {
    const std::string limits_namespace = "joint_limits/" + joint_name;
    if (!nh.hasParam(limits_namespace))
    {
      ROS_DEBUG_STREAM("No joint limits specification found for joint '" << joint_name <<
                       "' in the parameter server (namespace " << nh.getNamespace() + "/" + limits_namespace << ").");
      return false;
    }
    limits_nh = ros::NodeHandle(nh, limits_namespace);
  }
  catch(const ros::InvalidNameException& ex)
  {
    ROS_ERROR_STREAM(ex.what());
    return false;
  }
 
  // Position limits
  bool has_position_limits = false;
  if(limits_nh.getParam("has_position_limits", has_position_limits))
  {
    if (!has_position_limits) {limits.has_position_limits = false;}
    double min_pos, max_pos;
    if (has_position_limits && limits_nh.getParam("min_position", min_pos) && limits_nh.getParam("max_position", max_pos))
    {
      limits.has_position_limits = true;
      limits.min_position = min_pos;
      limits.max_position = max_pos;
    }
 
    bool angle_wraparound;
    if (!has_position_limits && limits_nh.getParam("angle_wraparound", angle_wraparound))
    {
      limits.angle_wraparound = angle_wraparound;
    }
  }
 
  // Velocity limits
  bool has_velocity_limits = false;
  if(limits_nh.getParam("has_velocity_limits", has_velocity_limits))
  {
    if (!has_velocity_limits) {limits.has_velocity_limits = false;}
    double max_vel;
    if (has_velocity_limits && limits_nh.getParam("max_velocity", max_vel))
    {
      limits.has_velocity_limits = true;
      limits.max_velocity = max_vel;
    }
  }
 
  // Acceleration limits
  bool has_acceleration_limits = false;
  if(limits_nh.getParam("has_acceleration_limits", has_acceleration_limits))
  {
    if (!has_acceleration_limits) {limits.has_acceleration_limits = false;}
    double max_acc;
    if (has_acceleration_limits && limits_nh.getParam("max_acceleration", max_acc))
    {
      limits.has_acceleration_limits = true;
      limits.max_acceleration = max_acc;
    }
  }
 
  // Jerk limits
  bool has_jerk_limits = false;
  if(limits_nh.getParam("has_jerk_limits", has_jerk_limits))
  {
    if (!has_jerk_limits) {limits.has_jerk_limits = false;}
    double max_jerk;
    if (has_jerk_limits && limits_nh.getParam("max_jerk", max_jerk))
    {
      limits.has_jerk_limits = true;
      limits.max_jerk = max_jerk;
    }
  }
 
  // Effort limits
  bool has_effort_limits = false;
  if(limits_nh.getParam("has_effort_limits", has_effort_limits))
  {
    if (!has_effort_limits) {limits.has_effort_limits = false;}
    double max_effort;
    if (has_effort_limits && limits_nh.getParam("max_effort", max_effort))
    {
      limits.has_effort_limits = true;
      limits.max_effort = max_effort;
    }
  }
 
  return true;
}
 
inline bool getSoftJointLimits(const std::string& joint_name, const ros::NodeHandle& nh, SoftJointLimits& soft_limits)
{
  // Node handle scoped where the soft joint limits are defined
  ros::NodeHandle limits_nh;
  try
  {
    const std::string limits_namespace = "joint_limits/" + joint_name;
    if (!nh.hasParam(limits_namespace))
    {
      ROS_DEBUG_STREAM("No soft joint limits specification found for joint '" << joint_name <<
                       "' in the parameter server (namespace " << nh.getNamespace() + "/" + limits_namespace << ").");
      return false;
    }
    limits_nh = ros::NodeHandle(nh, limits_namespace);
  }
  catch(const ros::InvalidNameException& ex)
  {
    ROS_ERROR_STREAM(ex.what());
    return false;
  }
 
  // Override soft limits if complete specification is found
  bool has_soft_limits;
  if(limits_nh.getParam("has_soft_limits", has_soft_limits))
  {
    if(has_soft_limits && limits_nh.hasParam("k_position") && limits_nh.hasParam("k_velocity") && limits_nh.hasParam("soft_lower_limit") && limits_nh.hasParam("soft_upper_limit"))
    {
      limits_nh.getParam("k_position", soft_limits.k_position);
      limits_nh.getParam("k_velocity", soft_limits.k_velocity);
      limits_nh.getParam("soft_lower_limit", soft_limits.min_position);
      limits_nh.getParam("soft_upper_limit", soft_limits.max_position);
      return true;
    }
  }
 
  return false;
}
 
}