Program Listing for File handle.hpp

↰ Return to documentation for file (include/hardware_interface/handle.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 HARDWARE_INTERFACE__HANDLE_HPP_
#define HARDWARE_INTERFACE__HANDLE_HPP_

#include <algorithm>
#include <limits>
#include <memory>
#include <mutex>
#include <optional>
#include <shared_mutex>
#include <string>
#include <utility>
#include <variant>

#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/introspection.hpp"
#include "hardware_interface/macros.hpp"

namespace
{
template <typename T>
std::string get_type_name()
{
  int status = 0;
  std::unique_ptr<char[], void (*)(void *)> res{
    abi::__cxa_demangle(typeid(T).name(), nullptr, nullptr, &status), std::free};
  return (status == 0) ? res.get() : typeid(T).name();
}
}  // namespace

namespace hardware_interface
{

using HANDLE_DATATYPE = std::variant<std::monostate, double, bool>;

class Handle
{
public:
  [[deprecated("Use InterfaceDescription for initializing the Interface")]]

  Handle(
    const std::string & prefix_name, const std::string & interface_name,
    double * value_ptr = nullptr)
  : prefix_name_(prefix_name),
    interface_name_(interface_name),
    handle_name_(prefix_name_ + "/" + interface_name_),
    value_ptr_(value_ptr)
  {
  }

  explicit Handle(const InterfaceDescription & interface_description)
  : prefix_name_(interface_description.get_prefix_name()),
    interface_name_(interface_description.get_interface_name()),
    handle_name_(interface_description.get_name())
  {
    data_type_ = interface_description.get_data_type();
    // As soon as multiple datatypes are used in HANDLE_DATATYPE
    // we need to initialize according the type passed in interface description
    if (data_type_ == hardware_interface::HandleDataType::DOUBLE)
    {
      value_ = std::numeric_limits<double>::quiet_NaN();
      value_ptr_ = std::get_if<double>(&value_);
    }
    else if (data_type_ == hardware_interface::HandleDataType::BOOL)
    {
      value_ptr_ = nullptr;
      value_ = false;
    }
    else
    {
      throw std::runtime_error(
        "Invalid data type : '" + interface_description.interface_info.data_type +
        "' for interface : " + interface_description.get_name());
    }
  }

  [[deprecated("Use InterfaceDescription for initializing the Interface")]]

  explicit Handle(const std::string & interface_name)
  : interface_name_(interface_name), handle_name_("/" + interface_name_), value_ptr_(nullptr)
  {
  }

  [[deprecated("Use InterfaceDescription for initializing the Interface")]]

  explicit Handle(const char * interface_name)
  : interface_name_(interface_name), handle_name_("/" + interface_name_), value_ptr_(nullptr)
  {
  }

  Handle(const Handle & other) noexcept { copy(other); }

  Handle & operator=(const Handle & other)
  {
    if (this != &other)
    {
      copy(other);
    }
    return *this;
  }

  Handle(Handle && other) noexcept { swap(*this, other); }

  Handle & operator=(Handle && other)
  {
    swap(*this, other);
    return *this;
  }

  virtual ~Handle() = default;

  inline operator bool() const { return value_ptr_ != nullptr; }

  const std::string & get_name() const { return handle_name_; }

  const std::string & get_interface_name() const { return interface_name_; }

  [[deprecated(
    "Replaced by get_name method, which is semantically more correct")]] const std::string &
  get_full_name() const
  {
    return get_name();
  }

  const std::string & get_prefix_name() const { return prefix_name_; }

  [[deprecated(
    "Use std::optional<T> get_optional() instead to retrieve the value. This method will be "
    "removed by the ROS 2 Kilted Kaiju release.")]]
  double get_value() const
  {
    std::shared_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
    if (!lock.owns_lock())
    {
      return std::numeric_limits<double>::quiet_NaN();
    }
    // BEGIN (Handle export change): for backward compatibility
    // TODO(Manuel) return value_ if old functionality is removed
    THROW_ON_NULLPTR(value_ptr_);
    return *value_ptr_;
    // END
  }

  template <typename T = double>
  [[nodiscard]] std::optional<T> get_optional() const
  {
    std::shared_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
    if (!lock.owns_lock())
    {
      return std::nullopt;
    }
    // BEGIN (Handle export change): for backward compatibility
    // TODO(saikishor) return value_ if old functionality is removed
    if constexpr (std::is_same_v<T, double>)
    {
      // If the template is of type double, check if the value_ptr_ is not nullptr
      THROW_ON_NULLPTR(value_ptr_);
      return *value_ptr_;
    }
    try
    {
      return std::get<T>(value_);
    }
    catch (const std::bad_variant_access & err)
    {
      throw std::runtime_error(
        "Invalid data type : '" + get_type_name<T>() + "' access for interface : " + get_name() +
        " expected : '" + data_type_.to_string() + "'");
    }
    // END
  }

  template <typename T>
  [[deprecated(
    "Use std::optional<T> get_optional() instead to retrieve the value. This method will be "
    "removed by the ROS 2 Kilted Kaiju release.")]] [[nodiscard]] bool
  get_value(T & value) const
  {
    std::shared_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
    if (!lock.owns_lock())
    {
      return false;
    }
    // BEGIN (Handle export change): for backward compatibility
    // TODO(Manuel) return value_ if old functionality is removed
    if constexpr (std::is_same_v<T, double>)
    {
      // If the template is of type double, check if the value_ptr_ is not nullptr
      THROW_ON_NULLPTR(value_ptr_);
      value = *value_ptr_;
    }
    else
    {
      try
      {
        value = std::get<T>(value_);
      }
      catch (const std::bad_variant_access & err)
      {
        throw std::runtime_error(
          "Invalid data type : '" + get_type_name<T>() + "' access for interface : " + get_name() +
          " expected : '" + data_type_.to_string() + "'");
      }
    }
    return true;
    // END
  }

  template <typename T>
  [[nodiscard]] bool set_value(const T & value)
  {
    std::unique_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
    if (!lock.owns_lock())
    {
      return false;
    }
    // BEGIN (Handle export change): for backward compatibility
    // TODO(Manuel) set value_ directly if old functionality is removed
    if constexpr (std::is_same_v<T, double>)
    {
      // If the template is of type double, check if the value_ptr_ is not nullptr
      THROW_ON_NULLPTR(value_ptr_);
      *value_ptr_ = value;
    }
    else
    {
      if (!std::holds_alternative<T>(value_))
      {
        throw std::runtime_error(
          "Invalid data type : '" + get_type_name<T>() + "' access for interface : " + get_name() +
          " expected : '" + data_type_.to_string() + "'");
      }
      value_ = value;
    }
    return true;
    // END
  }

  HandleDataType get_data_type() const { return data_type_; }

private:
  void copy(const Handle & other) noexcept
  {
    std::scoped_lock lock(other.handle_mutex_, handle_mutex_);
    prefix_name_ = other.prefix_name_;
    interface_name_ = other.interface_name_;
    handle_name_ = other.handle_name_;
    value_ = other.value_;
    if (std::holds_alternative<std::monostate>(value_))
    {
      value_ptr_ = other.value_ptr_;
    }
    else
    {
      value_ptr_ = std::get_if<double>(&value_);
    }
  }

  void swap(Handle & first, Handle & second) noexcept
  {
    std::scoped_lock lock(first.handle_mutex_, second.handle_mutex_);
    std::swap(first.prefix_name_, second.prefix_name_);
    std::swap(first.interface_name_, second.interface_name_);
    std::swap(first.handle_name_, second.handle_name_);
    std::swap(first.value_, second.value_);
    std::swap(first.value_ptr_, second.value_ptr_);
  }

protected:
  std::string prefix_name_;
  std::string interface_name_;
  std::string handle_name_;
  HANDLE_DATATYPE value_ = std::monostate{};
  HandleDataType data_type_ = HandleDataType::DOUBLE;
  // BEGIN (Handle export change): for backward compatibility
  // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed
  double * value_ptr_;
  // END
  mutable std::shared_mutex handle_mutex_;
};

class StateInterface : public Handle
{
public:
  explicit StateInterface(const InterfaceDescription & interface_description)
  : Handle(interface_description)
  {
  }

  void registerIntrospection() const
  {
    if (value_ptr_ || std::holds_alternative<double>(value_))
    {
      std::function<double()> f = [this]()
      { return value_ptr_ ? *value_ptr_ : std::get<double>(value_); };
      DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name(), f);
    }
  }

  void unregisterIntrospection() const
  {
    if (value_ptr_ || std::holds_alternative<double>(value_))
    {
      DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name());
    }
  }

  StateInterface(const StateInterface & other) = default;

  StateInterface(StateInterface && other) = default;

  using Handle::Handle;

  using SharedPtr = std::shared_ptr<StateInterface>;
  using ConstSharedPtr = std::shared_ptr<const StateInterface>;
};

class CommandInterface : public Handle
{
public:
  explicit CommandInterface(const InterfaceDescription & interface_description)
  : Handle(interface_description)
  {
  }

  CommandInterface(const CommandInterface & other) = delete;

  CommandInterface(CommandInterface && other) = default;

  void registerIntrospection() const
  {
    if (value_ptr_ || std::holds_alternative<double>(value_))
    {
      std::function<double()> f = [this]()
      { return value_ptr_ ? *value_ptr_ : std::get<double>(value_); };
      DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION("command_interface." + get_name(), f);
    }
  }

  void unregisterIntrospection() const
  {
    if (value_ptr_ || std::holds_alternative<double>(value_))
    {
      DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("command_interface." + get_name());
    }
  }

  using Handle::Handle;

  using SharedPtr = std::shared_ptr<CommandInterface>;
};

}  // namespace hardware_interface

#endif  // HARDWARE_INTERFACE__HANDLE_HPP_