hardware_info.hpp

// Copyright 2020 ros2_control Development Team
//
// 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__HARDWARE_INFO_HPP_
#define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_
 
#include <fmt/compile.h>
 
#include <cstdint>
#include <stdexcept>
#include <string>
#include <unordered_map>
#include <variant>
#include <vector>
 
#include "joint_limits/joint_limits.hpp"
 
#ifdef TRUE
#undef TRUE
#endif
 
#ifdef FALSE
#undef FALSE
#endif
 
namespace hardware_interface
{
struct InterfaceInfo
{
  std::string name;
  std::string min = "";
  std::string max = "";
  std::string initial_value = "";
  std::string data_type = "double";
  int size;
  std::unordered_map<std::string, std::string> parameters;
  bool enable_limits;
};
 
struct MimicJoint
{
  std::size_t joint_index;
  std::size_t mimicked_joint_index;
  double multiplier = 1.0;
  double offset = 0.0;
};
 
enum class MimicAttribute
{
  NOT_SET,
  TRUE,
  FALSE
};
 
struct ComponentInfo
{
  std::string name;
  std::string type;
 
  MimicAttribute is_mimic = MimicAttribute::NOT_SET;
 
  bool enable_limits = true;
 
  std::vector<InterfaceInfo> command_interfaces;
  std::vector<InterfaceInfo> state_interfaces;
  std::unordered_map<std::string, std::string> parameters;
};
 
struct JointInfo
{
  std::string name;
  std::vector<std::string> state_interfaces;
  std::vector<std::string> command_interfaces;
  std::string role;
  double mechanical_reduction = 1.0;
  double offset = 0.0;
  bool read_only = false;
};
 
struct ActuatorInfo
{
  std::string name;
  std::vector<std::string> state_interfaces;
  std::vector<std::string> command_interfaces;
  std::string role;
  double mechanical_reduction = 1.0;
  double offset = 0.0;
  bool read_only = false;
};
 
struct TransmissionInfo
{
  std::string name;
  std::string type;
  std::vector<JointInfo> joints;
  std::vector<ActuatorInfo> actuators;
  std::unordered_map<std::string, std::string> parameters;
};
 
using HANDLE_DATATYPE = std::variant<
  std::monostate, double, float, bool, uint8_t, int8_t, uint16_t, int16_t, uint32_t, int32_t>;
class HandleDataType
{
public:
  enum Value : int8_t
  {
    UNKNOWN = -1,
    DOUBLE,
    FLOAT32,
    BOOL,
    UINT8,
    INT8,
    UINT16,
    INT16,
    UINT32,
    INT32,
  };
 
  HandleDataType() = default;
  constexpr HandleDataType(Value value) : value_(value) {}  // NOLINT(runtime/explicit)
  explicit HandleDataType(const std::string & data_type)
  {
    if (data_type == "double")
    {
      value_ = DOUBLE;
    }
    else if (data_type == "bool")
    {
      value_ = BOOL;
    }
    else if (data_type == "float32")
    {
      value_ = FLOAT32;
    }
    else if (data_type == "uint8")
    {
      value_ = UINT8;
    }
    else if (data_type == "int8")
    {
      value_ = INT8;
    }
    else if (data_type == "uint16")
    {
      value_ = UINT16;
    }
    else if (data_type == "int16")
    {
      value_ = INT16;
    }
    else if (data_type == "uint32")
    {
      value_ = UINT32;
    }
    else if (data_type == "int32")
    {
      value_ = INT32;
    }
    else
    {
      value_ = UNKNOWN;
    }
  }
 
  operator Value() const { return value_; }
 
  explicit operator bool() const = delete;
 
  constexpr bool operator==(HandleDataType other) const { return value_ == other.value_; }
  constexpr bool operator!=(HandleDataType other) const { return value_ != other.value_; }
 
  constexpr bool operator==(Value other) const { return value_ == other; }
  constexpr bool operator!=(Value other) const { return value_ != other; }
 
  std::string to_string() const
  {
    switch (value_)
    {
      case DOUBLE:
        return "double";
      case BOOL:
        return "bool";
      case FLOAT32:
        return "float32";
      case UINT8:
        return "uint8";
      case INT8:
        return "int8";
      case UINT16:
        return "uint16";
      case INT16:
        return "int16";
      case UINT32:
        return "uint32";
      case INT32:
        return "int32";
      default:
        return "unknown";
    }
  }
 
  bool is_castable_to_double() const
  {
    switch (value_)
    {
      case DOUBLE:   // fallthrough
      case FLOAT32:  // fallthrough
      case BOOL:     // fallthrough
      case UINT8:    // fallthrough
      case INT8:     // fallthrough
      case UINT16:   // fallthrough
      case INT16:    // fallthrough
      case UINT32:   // fallthrough
      case INT32:
        return true;
      default:
        return false;  // unknown type cannot be converted
    }
  }
 
  double cast_to_double(const HANDLE_DATATYPE & value) const
  {
    switch (value_)
    {
      case DOUBLE:
        return std::get<double>(value);
      case FLOAT32:
        return static_cast<double>(std::get<float>(value));
      case BOOL:
        return static_cast<double>(std::get<bool>(value));
      case UINT8:
        return static_cast<double>(std::get<uint8_t>(value));
      case INT8:
        return static_cast<double>(std::get<int8_t>(value));
      case UINT16:
        return static_cast<double>(std::get<uint16_t>(value));
      case INT16:
        return static_cast<double>(std::get<int16_t>(value));
      case UINT32:
        return static_cast<double>(std::get<uint32_t>(value));
      case INT32:
        return static_cast<double>(std::get<int32_t>(value));
      default:
        throw std::runtime_error(
          fmt::format(FMT_COMPILE("Data type : '{}' cannot be casted to double."), to_string()));
    }
  }
 
  HandleDataType from_string(const std::string & data_type) { return HandleDataType(data_type); }
 
private:
  Value value_ = UNKNOWN;
};
 
struct InterfaceDescription
{
  InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in)
  : prefix_name(prefix_name_in),
    interface_info(interface_info_in),
    interface_name(prefix_name + "/" + interface_info.name)
  {
  }
 
  std::string prefix_name;
 
  InterfaceInfo interface_info;
 
  std::string interface_name;
 
  const std::string & get_prefix_name() const { return prefix_name; }
 
  const std::string & get_interface_name() const { return interface_info.name; }
 
  const std::string & get_name() const { return interface_name; }
 
  const std::string & get_data_type_string() const { return interface_info.data_type; }
 
  HandleDataType get_data_type() const { return HandleDataType(interface_info.data_type); }
};
 
struct HardwareAsyncParams
{
  int thread_priority = 50;
  std::string scheduling_policy = "synchronized";
  std::vector<int> cpu_affinity_cores = {};
  bool print_warnings = true;
};
 
struct HardwareInfo
{
  std::string name;
  std::string type;
  std::string group;
  unsigned int rw_rate;
  bool is_async;
  HardwareAsyncParams async_params;
  std::string hardware_plugin_name;
  std::unordered_map<std::string, std::string> hardware_parameters;
  std::vector<ComponentInfo> joints;
  std::vector<MimicJoint> mimic_joints;
  std::vector<ComponentInfo> sensors;
  std::vector<ComponentInfo> gpios;
  std::vector<TransmissionInfo> transmissions;
  std::string original_xml;
  std::unordered_map<std::string, joint_limits::JointLimits> limits;
 
  std::unordered_map<std::string, joint_limits::SoftJointLimits> soft_limits;
};
 
}  // namespace hardware_interface
#endif  // HARDWARE_INTERFACE__HARDWARE_INFO_HPP_