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.


#ifndef JOINT_LIMITS_INTERFACE_JOINT_LIMITS_ROSPARAM_H
#define JOINT_LIMITS_INTERFACE_JOINT_LIMITS_ROSPARAM_H

#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;
}

}

#endif