joint_limits_rosparam.hpp

// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
 
#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_
#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_
 
#include <joint_limits_interface/joint_limits.hpp>
 
#include <rclcpp/rclcpp.hpp>
 
#include <string>
 
namespace joint_limits_interface
{
 
inline bool getJointLimits(
  const std::string & joint_name, const rclcpp::Node::SharedPtr & node, JointLimits & limits)
{
  const std::string param_base_name = "joint_limits." + joint_name;
  try
  {
    if (
      !node->has_parameter(param_base_name + ".has_position_limits") &&
      !node->has_parameter(param_base_name + ".min_position") &&
      !node->has_parameter(param_base_name + ".max_position") &&
      !node->has_parameter(param_base_name + ".has_velocity_limits") &&
      !node->has_parameter(param_base_name + ".min_velocity") &&
      !node->has_parameter(param_base_name + ".max_velocity") &&
      !node->has_parameter(param_base_name + ".has_acceleration_limits") &&
      !node->has_parameter(param_base_name + ".max_acceleration") &&
      !node->has_parameter(param_base_name + ".has_jerk_limits") &&
      !node->has_parameter(param_base_name + ".max_jerk") &&
      !node->has_parameter(param_base_name + ".has_effort_limits") &&
      !node->has_parameter(param_base_name + ".max_effort") &&
      !node->has_parameter(param_base_name + ".angle_wraparound") &&
      !node->has_parameter(param_base_name + ".has_soft_limits") &&
      !node->has_parameter(param_base_name + ".k_position") &&
      !node->has_parameter(param_base_name + ".k_velocity") &&
      !node->has_parameter(param_base_name + ".soft_lower_limit") &&
      !node->has_parameter(param_base_name + ".soft_upper_limit"))
    {
      RCLCPP_ERROR(
        node->get_logger(),
        "No joint limits specification found for joint '%s' in the parameter server "
        "(node: %s param name: %s).",
        joint_name.c_str(), node->get_name(), param_base_name.c_str());
      return false;
    }
  }
  catch (const std::exception & ex)
  {
    RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
    return false;
  }
 
  // Position limits
  bool has_position_limits = false;
  if (node->get_parameter(param_base_name + ".has_position_limits", has_position_limits))
  {
    if (!has_position_limits)
    {
      limits.has_position_limits = false;
    }
    double min_pos, max_pos;
    if (
      has_position_limits && node->get_parameter(param_base_name + ".min_position", min_pos) &&
      node->get_parameter(param_base_name + ".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 &&
      node->get_parameter(param_base_name + ".angle_wraparound", angle_wraparound))
    {
      limits.angle_wraparound = angle_wraparound;
    }
  }
 
  // Velocity limits
  bool has_velocity_limits = false;
  if (node->get_parameter(param_base_name + ".has_velocity_limits", has_velocity_limits))
  {
    if (!has_velocity_limits)
    {
      limits.has_velocity_limits = false;
    }
    double max_vel;
    if (has_velocity_limits && node->get_parameter(param_base_name + ".max_velocity", max_vel))
    {
      limits.has_velocity_limits = true;
      limits.max_velocity = max_vel;
    }
  }
 
  // Acceleration limits
  bool has_acceleration_limits = false;
  if (node->get_parameter(param_base_name + ".has_acceleration_limits", has_acceleration_limits))
  {
    if (!has_acceleration_limits)
    {
      limits.has_acceleration_limits = false;
    }
    double max_acc;
    if (
      has_acceleration_limits &&
      node->get_parameter(param_base_name + ".max_acceleration", max_acc))
    {
      limits.has_acceleration_limits = true;
      limits.max_acceleration = max_acc;
    }
  }
 
  // Jerk limits
  bool has_jerk_limits = false;
  if (node->get_parameter(param_base_name + ".has_jerk_limits", has_jerk_limits))
  {
    if (!has_jerk_limits)
    {
      limits.has_jerk_limits = false;
    }
    double max_jerk;
    if (has_jerk_limits && node->get_parameter(param_base_name + ".max_jerk", max_jerk))
    {
      limits.has_jerk_limits = true;
      limits.max_jerk = max_jerk;
    }
  }
 
  // Effort limits
  bool has_effort_limits = false;
  if (node->get_parameter(param_base_name + ".has_effort_limits", has_effort_limits))
  {
    if (!has_effort_limits)
    {
      limits.has_effort_limits = false;
    }
    double max_effort;
    if (has_effort_limits && node->get_parameter(param_base_name + ".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 rclcpp::Node::SharedPtr & node,
  SoftJointLimits & soft_limits)
{
  const std::string param_base_name = "joint_limits." + joint_name;
  try
  {
    if (
      !node->has_parameter(param_base_name + ".has_soft_limits") &&
      !node->has_parameter(param_base_name + ".k_velocity") &&
      !node->has_parameter(param_base_name + ".k_position") &&
      !node->has_parameter(param_base_name + ".soft_lower_limit") &&
      !node->has_parameter(param_base_name + ".soft_upper_limit"))
    {
      RCLCPP_DEBUG(
        node->get_logger(),
        "No soft joint limits specification found for joint '%s' in the parameter server "
        "(node: %s param name: %s).",
        joint_name.c_str(), node->get_name(), param_base_name.c_str());
      return false;
    }
  }
  catch (const std::exception & ex)
  {
    RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
    return false;
  }
 
  // Override soft limits if complete specification is found
  bool has_soft_limits;
  if (node->get_parameter(param_base_name + ".has_soft_limits", has_soft_limits))
  {
    if (
      has_soft_limits && node->has_parameter(param_base_name + ".k_position") &&
      node->has_parameter(param_base_name + ".k_velocity") &&
      node->has_parameter(param_base_name + ".soft_lower_limit") &&
      node->has_parameter(param_base_name + ".soft_upper_limit"))
    {
      node->get_parameter(param_base_name + ".k_position", soft_limits.k_position);
      node->get_parameter(param_base_name + ".k_velocity", soft_limits.k_velocity);
      node->get_parameter(param_base_name + ".soft_lower_limit", soft_limits.min_position);
      node->get_parameter(param_base_name + ".soft_upper_limit", soft_limits.max_position);
      return true;
    }
  }
 
  return false;
}
 
}  // namespace joint_limits_interface
 
#endif  // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_