joint_limiter_interface.hpp

// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt)
//
// 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_LIMITER_INTERFACE_HPP_
#define JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_
 
#include <cstddef>
#include <string>
#include <vector>
 
#include "joint_limits/joint_limits.hpp"
#include "joint_limits/joint_limits_rosparam.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "realtime_tools/realtime_thread_safe_box.hpp"
 
namespace joint_limits
{
 
template <typename JointLimitsStateDataType>
class JointLimiterInterface
{
public:
  JointLimiterInterface() = default;
 
  virtual ~JointLimiterInterface() = default;
 
  virtual bool init(
    const std::vector<std::string> & joint_names,
    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,
    const std::string & robot_description_topic = "/robot_description")
  {
    number_of_joints_ = joint_names.size();
    joint_names_ = joint_names;
    joint_limits_.resize(number_of_joints_);
    node_param_itf_ = param_itf;
    node_logging_itf_ = logging_itf;
 
    bool result = true;
 
    // TODO(destogl): get limits from URDF
 
    // Initialize and get joint limits from parameter server
    if (has_parameter_interface())
    {
      for (size_t i = 0; i < number_of_joints_; ++i)
      {
        if (!declare_parameters(joint_names[i], node_param_itf_, node_logging_itf_))
        {
          RCLCPP_ERROR(
            node_logging_itf_->get_logger(),
            "JointLimiter: Joint '%s': parameter declaration has failed", joint_names[i].c_str());
          result = false;
          break;
        }
        if (!get_joint_limits(joint_names[i], node_param_itf_, node_logging_itf_, joint_limits_[i]))
        {
          RCLCPP_ERROR(
            node_logging_itf_->get_logger(),
            "JointLimiter: Joint '%s': getting parameters has failed", joint_names[i].c_str());
          result = false;
          break;
        }
        RCLCPP_INFO(
          node_logging_itf_->get_logger(), "Limits for joint %zu (%s) are:\n%s", i,
          joint_names[i].c_str(), joint_limits_[i].to_string().c_str());
      }
      updated_limits_.set(joint_limits_);
 
      auto on_parameter_event_callback = [this](const std::vector<rclcpp::Parameter> & parameters)
      {
        rcl_interfaces::msg::SetParametersResult set_parameters_result;
        set_parameters_result.successful = true;
 
        std::vector<joint_limits::JointLimits> updated_joint_limits = joint_limits_;
        bool changed = false;
 
        for (size_t i = 0; i < number_of_joints_; ++i)
        {
          changed |= joint_limits::check_for_limits_update(
            joint_names_[i], parameters, node_logging_itf_, updated_joint_limits[i]);
        }
 
        if (changed)
        {
          updated_limits_.set(updated_joint_limits);
          RCLCPP_INFO(node_logging_itf_->get_logger(), "Limits are dynamically updated!");
        }
 
        return set_parameters_result;
      };
 
      parameter_callback_ =
        node_param_itf_->add_on_set_parameters_callback(on_parameter_event_callback);
    }
 
    if (result)
    {
      result = on_init();
    }
 
    (void)robot_description_topic;  // avoid linters output
 
    return result;
  }
 
  virtual bool init(
    const std::vector<std::string> & joint_names,
    const std::vector<joint_limits::JointLimits> & joint_limits,
    const std::vector<joint_limits::SoftJointLimits> & soft_joint_limits,
    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf)
  {
    number_of_joints_ = joint_names.size();
    joint_names_ = joint_names;
    joint_limits_ = joint_limits;
    soft_joint_limits_ = soft_joint_limits;
    node_param_itf_ = param_itf;
    node_logging_itf_ = logging_itf;
    updated_limits_.set(joint_limits_);
 
    if ((number_of_joints_ != joint_limits_.size()) && has_logging_interface())
    {
      RCLCPP_ERROR(
        node_logging_itf_->get_logger(),
        "JointLimiter: Number of joint names and limits do not match: %zu != %zu",
        number_of_joints_, joint_limits_.size());
    }
    return (number_of_joints_ == joint_limits_.size()) && on_init();
  }
 
  virtual bool init(
    const std::vector<std::string> & joint_names, const rclcpp::Node::SharedPtr & node,
    const std::string & robot_description_topic = "/robot_description")
  {
    return init(
      joint_names, node->get_node_parameters_interface(), node->get_node_logging_interface(),
      robot_description_topic);
  }
 
  virtual bool init(
    const std::vector<std::string> & joint_names,
    const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node,
    const std::string & robot_description_topic = "/robot_description")
  {
    return init(
      joint_names, lifecycle_node->get_node_parameters_interface(),
      lifecycle_node->get_node_logging_interface(), robot_description_topic);
  }
 
  virtual bool configure(const JointLimitsStateDataType & current_joint_states)
  {
    return on_configure(current_joint_states);
  }
 
  virtual bool enforce(
    const JointLimitsStateDataType & current_joint_states,
    JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt)
  {
    auto joint_limits_op = updated_limits_.try_get();
    if (joint_limits_op.has_value())
    {
      joint_limits_ = joint_limits_op.value();
      return on_enforce(current_joint_states, desired_joint_states, dt);
    }
    else
    {
      return false;
    }
  }
 
  virtual void reset_internals() = 0;
 
protected:
  virtual bool on_init() = 0;
 
  virtual bool on_configure(const JointLimitsStateDataType & current_joint_states) = 0;
 
  virtual bool on_enforce(
    const JointLimitsStateDataType & current_joint_states,
    JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0;
 
  bool has_logging_interface() const { return node_logging_itf_ != nullptr; }
 
  bool has_parameter_interface() const { return node_param_itf_ != nullptr; }
 
  size_t number_of_joints_;
  std::vector<std::string> joint_names_;
  std::vector<joint_limits::JointLimits> joint_limits_;
  std::vector<joint_limits::SoftJointLimits> soft_joint_limits_;
  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_param_itf_;
  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_itf_;
 
private:
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_;
  realtime_tools::RealtimeThreadSafeBox<std::vector<joint_limits::JointLimits>> updated_limits_;
};
 
}  // namespace joint_limits
 
#endif  // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_