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__JOINT_LIMITS_ROSPARAM_HPP_
#define JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_
 
#include <limits>
#include <string>
 
#include "joint_limits/joint_limits.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
namespace  // utilities
{
 
template <typename ParameterT>
auto auto_declare(
  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
  const std::string & name, const ParameterT & default_value)
{
  if (!param_itf->has_parameter(name))
  {
    auto param_default_value = rclcpp::ParameterValue(default_value);
    param_itf->declare_parameter(name, param_default_value);
  }
  return param_itf->get_parameter(name).get_value<ParameterT>();
}
}  // namespace
 
namespace joint_limits
{
inline bool declare_parameters(
  const std::string & joint_name,
  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf)
{
  const std::string param_base_name = "joint_limits." + joint_name;
  try
  {
    auto_declare<bool>(param_itf, param_base_name + ".has_position_limits", false);
    auto_declare<double>(
      param_itf, param_base_name + ".min_position", std::numeric_limits<double>::quiet_NaN());
    auto_declare<double>(
      param_itf, param_base_name + ".max_position", std::numeric_limits<double>::quiet_NaN());
    auto_declare<bool>(param_itf, param_base_name + ".has_velocity_limits", false);
    auto_declare<double>(
      param_itf, param_base_name + ".min_velocity", std::numeric_limits<double>::quiet_NaN());
    auto_declare<double>(
      param_itf, param_base_name + ".max_velocity", std::numeric_limits<double>::quiet_NaN());
    auto_declare<bool>(param_itf, param_base_name + ".has_acceleration_limits", false);
    auto_declare<double>(
      param_itf, param_base_name + ".max_acceleration", std::numeric_limits<double>::quiet_NaN());
    auto_declare<bool>(param_itf, param_base_name + ".has_deceleration_limits", false);
    auto_declare<double>(
      param_itf, param_base_name + ".max_deceleration", std::numeric_limits<double>::quiet_NaN());
    auto_declare<bool>(param_itf, param_base_name + ".has_jerk_limits", false);
    auto_declare<double>(
      param_itf, param_base_name + ".max_jerk", std::numeric_limits<double>::quiet_NaN());
    auto_declare<bool>(param_itf, param_base_name + ".has_effort_limits", false);
    auto_declare<double>(
      param_itf, param_base_name + ".max_effort", std::numeric_limits<double>::quiet_NaN());
    auto_declare<bool>(param_itf, param_base_name + ".angle_wraparound", false);
    auto_declare<bool>(param_itf, param_base_name + ".has_soft_limits", false);
    auto_declare<double>(
      param_itf, param_base_name + ".k_position", std::numeric_limits<double>::quiet_NaN());
    auto_declare<double>(
      param_itf, param_base_name + ".k_velocity", std::numeric_limits<double>::quiet_NaN());
    auto_declare<double>(
      param_itf, param_base_name + ".soft_lower_limit", std::numeric_limits<double>::quiet_NaN());
    auto_declare<double>(
      param_itf, param_base_name + ".soft_upper_limit", std::numeric_limits<double>::quiet_NaN());
  }
  catch (const std::exception & ex)
  {
    RCLCPP_ERROR(logging_itf->get_logger(), "%s", ex.what());
    return false;
  }
  return true;
}
 
inline bool declare_parameters(const std::string & joint_name, const rclcpp::Node::SharedPtr & node)
{
  return declare_parameters(
    joint_name, node->get_node_parameters_interface(), node->get_node_logging_interface());
}
 
inline bool declare_parameters(
  const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node)
{
  return declare_parameters(
    joint_name, lifecycle_node->get_node_parameters_interface(),
    lifecycle_node->get_node_logging_interface());
}
 
 
inline bool get_joint_limits(
  const std::string & joint_name,
  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,
  JointLimits & limits)
{
  const std::string param_base_name = "joint_limits." + joint_name;
  try
  {
    if (
      !param_itf->has_parameter(param_base_name + ".has_position_limits") &&
      !param_itf->has_parameter(param_base_name + ".min_position") &&
      !param_itf->has_parameter(param_base_name + ".max_position") &&
      !param_itf->has_parameter(param_base_name + ".has_velocity_limits") &&
      !param_itf->has_parameter(param_base_name + ".min_velocity") &&
      !param_itf->has_parameter(param_base_name + ".max_velocity") &&
      !param_itf->has_parameter(param_base_name + ".has_acceleration_limits") &&
      !param_itf->has_parameter(param_base_name + ".max_acceleration") &&
      !param_itf->has_parameter(param_base_name + ".has_deceleration_limits") &&
      !param_itf->has_parameter(param_base_name + ".max_deceleration") &&
      !param_itf->has_parameter(param_base_name + ".has_jerk_limits") &&
      !param_itf->has_parameter(param_base_name + ".max_jerk") &&
      !param_itf->has_parameter(param_base_name + ".has_effort_limits") &&
      !param_itf->has_parameter(param_base_name + ".max_effort") &&
      !param_itf->has_parameter(param_base_name + ".angle_wraparound") &&
      !param_itf->has_parameter(param_base_name + ".has_soft_limits") &&
      !param_itf->has_parameter(param_base_name + ".k_position") &&
      !param_itf->has_parameter(param_base_name + ".k_velocity") &&
      !param_itf->has_parameter(param_base_name + ".soft_lower_limit") &&
      !param_itf->has_parameter(param_base_name + ".soft_upper_limit"))
    {
      RCLCPP_ERROR(
        logging_itf->get_logger(),
        "No joint limits specification found for joint '%s' in the parameter server "
        "(param name: %s).",
        joint_name.c_str(), param_base_name.c_str());
      return false;
    }
  }
  catch (const std::exception & ex)
  {
    RCLCPP_ERROR(logging_itf->get_logger(), "%s", ex.what());
    return false;
  }
 
  // Position limits
  if (param_itf->has_parameter(param_base_name + ".has_position_limits"))
  {
    limits.has_position_limits =
      param_itf->get_parameter(param_base_name + ".has_position_limits").as_bool();
    if (
      limits.has_position_limits && param_itf->has_parameter(param_base_name + ".min_position") &&
      param_itf->has_parameter(param_base_name + ".max_position"))
    {
      limits.min_position = param_itf->get_parameter(param_base_name + ".min_position").as_double();
      limits.max_position = param_itf->get_parameter(param_base_name + ".max_position").as_double();
    }
    else
    {
      limits.has_position_limits = false;
    }
 
    if (
      !limits.has_position_limits &&
      param_itf->has_parameter(param_base_name + ".angle_wraparound"))
    {
      limits.angle_wraparound =
        param_itf->get_parameter(param_base_name + ".angle_wraparound").as_bool();
    }
  }
 
  // Velocity limits
  if (param_itf->has_parameter(param_base_name + ".has_velocity_limits"))
  {
    limits.has_velocity_limits =
      param_itf->get_parameter(param_base_name + ".has_velocity_limits").as_bool();
    if (limits.has_velocity_limits && param_itf->has_parameter(param_base_name + ".max_velocity"))
    {
      limits.max_velocity = param_itf->get_parameter(param_base_name + ".max_velocity").as_double();
    }
    else
    {
      limits.has_velocity_limits = false;
    }
  }
 
  // Acceleration limits
  if (param_itf->has_parameter(param_base_name + ".has_acceleration_limits"))
  {
    limits.has_acceleration_limits =
      param_itf->get_parameter(param_base_name + ".has_acceleration_limits").as_bool();
    if (
      limits.has_acceleration_limits &&
      param_itf->has_parameter(param_base_name + ".max_acceleration"))
    {
      limits.max_acceleration =
        param_itf->get_parameter(param_base_name + ".max_acceleration").as_double();
    }
    else
    {
      limits.has_acceleration_limits = false;
    }
  }
 
  // Deceleration limits
  if (param_itf->has_parameter(param_base_name + ".has_deceleration_limits"))
  {
    limits.has_deceleration_limits =
      param_itf->get_parameter(param_base_name + ".has_deceleration_limits").as_bool();
    if (
      limits.has_deceleration_limits &&
      param_itf->has_parameter(param_base_name + ".max_deceleration"))
    {
      limits.max_deceleration =
        param_itf->get_parameter(param_base_name + ".max_deceleration").as_double();
    }
    else
    {
      limits.has_deceleration_limits = false;
    }
  }
 
  // Jerk limits
  if (param_itf->has_parameter(param_base_name + ".has_jerk_limits"))
  {
    limits.has_jerk_limits =
      param_itf->get_parameter(param_base_name + ".has_jerk_limits").as_bool();
    if (limits.has_jerk_limits && param_itf->has_parameter(param_base_name + ".max_jerk"))
    {
      limits.max_jerk = param_itf->get_parameter(param_base_name + ".max_jerk").as_double();
    }
    else
    {
      limits.has_jerk_limits = false;
    }
  }
 
  // Effort limits
  if (param_itf->has_parameter(param_base_name + ".has_effort_limits"))
  {
    limits.has_effort_limits =
      param_itf->get_parameter(param_base_name + ".has_effort_limits").as_bool();
    if (limits.has_effort_limits && param_itf->has_parameter(param_base_name + ".max_effort"))
    {
      limits.has_effort_limits = true;
      limits.max_effort = param_itf->get_parameter(param_base_name + ".max_effort").as_double();
    }
    else
    {
      limits.has_effort_limits = false;
    }
  }
 
  return true;
}
 
inline bool get_joint_limits(
  const std::string & joint_name, const rclcpp::Node::SharedPtr & node, JointLimits & limits)
{
  return get_joint_limits(
    joint_name, node->get_node_parameters_interface(), node->get_node_logging_interface(), limits);
}
 
inline bool get_joint_limits(
  const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node,
  JointLimits & limits)
{
  return get_joint_limits(
    joint_name, lifecycle_node->get_node_parameters_interface(),
    lifecycle_node->get_node_logging_interface(), limits);
}
 
 
inline bool get_joint_limits(
  const std::string & joint_name,
  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,
  SoftJointLimits & soft_limits)
{
  const std::string param_base_name = "joint_limits." + joint_name;
  try
  {
    if (
      !param_itf->has_parameter(param_base_name + ".has_soft_limits") &&
      !param_itf->has_parameter(param_base_name + ".k_velocity") &&
      !param_itf->has_parameter(param_base_name + ".k_position") &&
      !param_itf->has_parameter(param_base_name + ".soft_lower_limit") &&
      !param_itf->has_parameter(param_base_name + ".soft_upper_limit"))
    {
      RCLCPP_DEBUG(
        logging_itf->get_logger(),
        "No soft joint limits specification found for joint '%s' in the parameter server "
        "(param name: %s).",
        joint_name.c_str(), param_base_name.c_str());
      return false;
    }
  }
  catch (const std::exception & ex)
  {
    RCLCPP_ERROR(logging_itf->get_logger(), "%s", ex.what());
    return false;
  }
 
  // Override soft limits if complete specification is found
  if (param_itf->has_parameter(param_base_name + ".has_soft_limits"))
  {
    if (
      param_itf->get_parameter(param_base_name + ".has_soft_limits").as_bool() &&
      param_itf->has_parameter(param_base_name + ".k_position") &&
      param_itf->has_parameter(param_base_name + ".k_velocity") &&
      param_itf->has_parameter(param_base_name + ".soft_lower_limit") &&
      param_itf->has_parameter(param_base_name + ".soft_upper_limit"))
    {
      soft_limits.k_position =
        param_itf->get_parameter(param_base_name + ".k_position").as_double();
      soft_limits.k_velocity =
        param_itf->get_parameter(param_base_name + ".k_velocity").as_double();
      soft_limits.min_position =
        param_itf->get_parameter(param_base_name + ".soft_lower_limit").as_double();
      soft_limits.max_position =
        param_itf->get_parameter(param_base_name + ".soft_upper_limit").as_double();
      return true;
    }
  }
 
  return false;
}
 
inline bool get_joint_limits(
  const std::string & joint_name, const rclcpp::Node::SharedPtr & node,
  SoftJointLimits & soft_limits)
{
  return get_joint_limits(
    joint_name, node->get_node_parameters_interface(), node->get_node_logging_interface(),
    soft_limits);
}
 
inline bool get_joint_limits(
  const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node,
  SoftJointLimits & soft_limits)
{
  return get_joint_limits(
    joint_name, lifecycle_node->get_node_parameters_interface(),
    lifecycle_node->get_node_logging_interface(), soft_limits);
}
 
}  // namespace joint_limits
 
#endif  // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_