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 <fmt/compile.h>
 
#include <limits>
#include <string>
#include <vector>
 
#include "joint_limits/joint_limits.hpp"
#include "rclcpp/node.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 = fmt::format(FMT_COMPILE("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 + ".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 = fmt::format(FMT_COMPILE("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 + ".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"))
    {
      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 check_for_limits_update(
  const std::string & joint_name, const std::vector<rclcpp::Parameter> & parameters,
  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,
  JointLimits & updated_limits)
{
  const std::string param_base_name = fmt::format(FMT_COMPILE("joint_limits.{}"), joint_name);
  bool changed = false;
 
  // update first numerical values to make later checks for "has" limits members
  for (auto & parameter : parameters)
  {
    const std::string param_name = parameter.get_name();
    try
    {
      if (param_name == param_base_name + ".min_position")
      {
        changed = updated_limits.min_position != parameter.get_value<double>();
        updated_limits.min_position = parameter.get_value<double>();
      }
      else if (param_name == param_base_name + ".max_position")
      {
        changed = updated_limits.max_position != parameter.get_value<double>();
        updated_limits.max_position = parameter.get_value<double>();
      }
      else if (param_name == param_base_name + ".max_velocity")
      {
        changed = updated_limits.max_velocity != parameter.get_value<double>();
        updated_limits.max_velocity = parameter.get_value<double>();
      }
      else if (param_name == param_base_name + ".max_acceleration")
      {
        changed = updated_limits.max_acceleration != parameter.get_value<double>();
        updated_limits.max_acceleration = parameter.get_value<double>();
      }
      else if (param_name == param_base_name + ".max_deceleration")
      {
        changed = updated_limits.max_deceleration != parameter.get_value<double>();
        updated_limits.max_deceleration = parameter.get_value<double>();
      }
      else if (param_name == param_base_name + ".max_jerk")
      {
        changed = updated_limits.max_jerk != parameter.get_value<double>();
        updated_limits.max_jerk = parameter.get_value<double>();
      }
      else if (param_name == param_base_name + ".max_effort")
      {
        changed = updated_limits.max_effort != parameter.get_value<double>();
        updated_limits.max_effort = parameter.get_value<double>();
      }
    }
    catch (const rclcpp::exceptions::InvalidParameterTypeException & e)
    {
      RCLCPP_WARN(logging_itf->get_logger(), "Please use the right type: %s", e.what());
    }
  }
 
  for (auto & parameter : parameters)
  {
    const std::string param_name = parameter.get_name();
    try
    {
      if (param_name == param_base_name + ".has_position_limits")
      {
        updated_limits.has_position_limits = parameter.get_value<bool>();
        if (updated_limits.has_position_limits)
        {
          if (std::isnan(updated_limits.min_position) || std::isnan(updated_limits.max_position))
          {
            RCLCPP_WARN(
              logging_itf->get_logger(),
              "PARAMETER NOT UPDATED: Position limits can not be used, i.e., "
              "'has_position_limits' flag can not be set, if 'min_position' "
              "and 'max_position' are not set or not have valid double values.");
            updated_limits.has_position_limits = false;
          }
          else if (updated_limits.min_position >= updated_limits.max_position)
          {
            RCLCPP_WARN(
              logging_itf->get_logger(),
              "PARAMETER NOT UPDATED: Position limits can not be used, i.e., "
              "'has_position_limits' flag can not be set, if not "
              "'min_position' < 'max_position'");
            updated_limits.has_position_limits = false;
          }
          else
          {
            changed = true;
          }
        }
      }
      else if (param_name == param_base_name + ".has_velocity_limits")
      {
        updated_limits.has_velocity_limits = parameter.get_value<bool>();
        if (updated_limits.has_velocity_limits && std::isnan(updated_limits.max_velocity))
        {
          RCLCPP_WARN(
            logging_itf->get_logger(),
            "PARAMETER NOT UPDATED: 'has_velocity_limits' flag can not be set if 'min_velocity' "
            "and 'max_velocity' are not set or not have valid double values.");
          updated_limits.has_velocity_limits = false;
        }
        else
        {
          changed = true;
        }
      }
      else if (param_name == param_base_name + ".has_acceleration_limits")
      {
        updated_limits.has_acceleration_limits = parameter.get_value<bool>();
        if (updated_limits.has_acceleration_limits && std::isnan(updated_limits.max_acceleration))
        {
          RCLCPP_WARN(
            logging_itf->get_logger(),
            "PARAMETER NOT UPDATED: 'has_acceleration_limits' flag can not be set if "
            "'max_acceleration' is not set or not have valid double values.");
          updated_limits.has_acceleration_limits = false;
        }
        else
        {
          changed = true;
        }
      }
      else if (param_name == param_base_name + ".has_deceleration_limits")
      {
        updated_limits.has_deceleration_limits = parameter.get_value<bool>();
        if (updated_limits.has_deceleration_limits && std::isnan(updated_limits.max_deceleration))
        {
          RCLCPP_WARN(
            logging_itf->get_logger(),
            "PARAMETER NOT UPDATED: 'has_deceleration_limits' flag can not be set if "
            "'max_deceleration' is not set or not have valid double values.");
          updated_limits.has_deceleration_limits = false;
        }
        else
        {
          changed = true;
        }
      }
      else if (param_name == param_base_name + ".has_jerk_limits")
      {
        updated_limits.has_jerk_limits = parameter.get_value<bool>();
        if (updated_limits.has_jerk_limits && std::isnan(updated_limits.max_jerk))
        {
          RCLCPP_WARN(
            logging_itf->get_logger(),
            "PARAMETER NOT UPDATED: 'has_jerk_limits' flag can not be set if 'max_jerk' is not set "
            "or not have valid double values.");
          updated_limits.has_jerk_limits = false;
        }
        else
        {
          changed = true;
        }
      }
      else if (param_name == param_base_name + ".has_effort_limits")
      {
        updated_limits.has_effort_limits = parameter.get_value<bool>();
        if (updated_limits.has_effort_limits && std::isnan(updated_limits.max_effort))
        {
          RCLCPP_WARN(
            logging_itf->get_logger(),
            "PARAMETER NOT UPDATED: 'has_effort_limits' flag can not be set if 'max_effort' is not "
            "set or not have valid double values.");
          updated_limits.has_effort_limits = false;
        }
        else
        {
          changed = true;
        }
      }
      else if (param_name == param_base_name + ".angle_wraparound")
      {
        updated_limits.angle_wraparound = parameter.get_value<bool>();
        if (updated_limits.angle_wraparound && updated_limits.has_position_limits)
        {
          RCLCPP_WARN(
            logging_itf->get_logger(),
            "PARAMETER NOT UPDATED: 'angle_wraparound' flag can not be set if "
            "'has_position_limits' flag is set.");
          updated_limits.angle_wraparound = false;
        }
        else
        {
          changed = true;
        }
      }
    }
    catch (const rclcpp::exceptions::InvalidParameterTypeException & e)
    {
      RCLCPP_WARN(
        logging_itf->get_logger(), "PARAMETER NOT UPDATED: Please use the right type: %s",
        e.what());
    }
  }
 
  return changed;
}
 
 
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 = fmt::format(FMT_COMPILE("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);
}
 
inline bool check_for_limits_update(
  const std::string & joint_name, const std::vector<rclcpp::Parameter> & parameters,
  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,
  SoftJointLimits & updated_limits)
{
  const std::string param_base_name = fmt::format(FMT_COMPILE("joint_limits.{}"), joint_name);
  bool changed = false;
 
  for (auto & parameter : parameters)
  {
    const std::string param_name = parameter.get_name();
    try
    {
      if (param_name == param_base_name + ".has_soft_limits")
      {
        if (!parameter.get_value<bool>())
        {
          RCLCPP_WARN(
            logging_itf->get_logger(),
            "Parameter 'has_soft_limits' is not set, therefore the limits will not be updated!");
          return false;
        }
      }
    }
    catch (const rclcpp::exceptions::InvalidParameterTypeException & e)
    {
      RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what());
    }
  }
 
  for (auto & parameter : parameters)
  {
    const std::string param_name = parameter.get_name();
    try
    {
      if (param_name == param_base_name + ".k_position")
      {
        changed = updated_limits.k_position != parameter.get_value<double>();
        updated_limits.k_position = parameter.get_value<double>();
      }
      else if (param_name == param_base_name + ".k_velocity")
      {
        changed = updated_limits.k_velocity != parameter.get_value<double>();
        updated_limits.k_velocity = parameter.get_value<double>();
      }
      else if (param_name == param_base_name + ".soft_lower_limit")
      {
        changed = updated_limits.min_position != parameter.get_value<double>();
        updated_limits.min_position = parameter.get_value<double>();
      }
      else if (param_name == param_base_name + ".soft_upper_limit")
      {
        changed = updated_limits.max_position != parameter.get_value<double>();
        updated_limits.max_position = parameter.get_value<double>();
      }
    }
    catch (const rclcpp::exceptions::InvalidParameterTypeException & e)
    {
      RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what());
    }
  }
 
  return changed;
}
 
}  // namespace joint_limits
 
#endif  // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_