Skip to content

Commit

Permalink
first poc for export of inferface description
Browse files Browse the repository at this point in the history
  • Loading branch information
@mamueluth
mamueluth committed Nov 15, 2023
1 parent cf405bc commit 3526deb
Show file tree
Hide file tree
Showing 14 changed files with 742 additions and 148 deletions.
141 changes: 87 additions & 54 deletions hardware_interface/include/hardware_interface/handle.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,47 +15,55 @@
#ifndef HARDWARE_INTERFACE__HANDLE_HPP_
#define HARDWARE_INTERFACE__HANDLE_HPP_

#include <limits>
#include <string>
#include <utility>

#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/macros.hpp"
#include "hardware_interface/visibility_control.h"

namespace hardware_interface
{
/// A handle used to get and set a value on a given interface.
class ReadOnlyHandle
class Handle
{
public:
ReadOnlyHandle(
const std::string & prefix_name, const std::string & interface_name,
double * value_ptr = nullptr)
: prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr)
Handle(const std::string & prefix_name, const std::string & interface_name)
: prefix_name_(prefix_name),
interface_name_(interface_name),
has_new_value_(false),
is_valid_(false),
value_(std::numeric_limits<double>::quiet_NaN())
{
}

explicit ReadOnlyHandle(const std::string & interface_name)
: interface_name_(interface_name), value_ptr_(nullptr)
explicit Handle(const std::string & interface_name)
: interface_name_(interface_name),
has_new_value_(false),
is_valid_(false),
value_(std::numeric_limits<double>::quiet_NaN())
{
}

explicit ReadOnlyHandle(const char * interface_name)
: interface_name_(interface_name), value_ptr_(nullptr)
explicit Handle(const char * interface_name)
: interface_name_(interface_name),
has_new_value_(false),
is_valid_(false),
value_(std::numeric_limits<double>::quiet_NaN())
{
}

ReadOnlyHandle(const ReadOnlyHandle & other) = default;
// Handle should be unique
Handle(const Handle & other) = delete;

ReadOnlyHandle(ReadOnlyHandle && other) = default;
Handle(Handle && other) = default;

ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default;
Handle & operator=(const Handle & other) = default;

ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default;
Handle & operator=(Handle && other) = default;

virtual ~ReadOnlyHandle() = default;

/// Returns true if handle references a value.
inline operator bool() const { return value_ptr_ != nullptr; }
virtual ~Handle() = default;

const std::string get_name() const { return prefix_name_ + "/" + interface_name_; }

Expand All @@ -70,62 +78,87 @@ class ReadOnlyHandle

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

double get_value() const
/**
* @brief Set the new value of the handle and mark the Handle as "has_new_value_ = true".
* This indicates that new data has been set since last read access.
*
* @param value current stored value in the handle.
*/
virtual void set_value(const double & value)
{
THROW_ON_NULLPTR(value_ptr_);
return *value_ptr_;
value_ = value;
has_new_value_ = true;
}

protected:
std::string prefix_name_;
std::string interface_name_;
double * value_ptr_;
};

class ReadWriteHandle : public ReadOnlyHandle
{
public:
ReadWriteHandle(
const std::string & prefix_name, const std::string & interface_name,
double * value_ptr = nullptr)
: ReadOnlyHandle(prefix_name, interface_name, value_ptr)
/**
* @brief Get the value of the handle an mark the handle as "has_new_value_ = false"
* since the value has been read and not be changed since last read access.
*
* @return HandleValue is the current stored value of the handle.
*/
virtual double get_value()
{
has_new_value_ = false;
return value_;
}

explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {}

explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {}

ReadWriteHandle(const ReadWriteHandle & other) = default;

ReadWriteHandle(ReadWriteHandle && other) = default;

ReadWriteHandle & operator=(const ReadWriteHandle & other) = default;

ReadWriteHandle & operator=(ReadWriteHandle && other) = default;

virtual ~ReadWriteHandle() = default;
/**
* @brief Indicates if new value has been stored in the handle since the last
* read access.
*
* @return true => new value has been stored since last read access to the handle.
* @return false => no new value has been stored since last read access to the handle.
*/
virtual bool has_new_value() const { return has_new_value_; }

void set_value(double value)
/**
* @brief Indicates if the value stored inside the handle is valid
*
* @return true => stored value is valid and can be used.
* @return false => false stored value is not valid and should not be used.
*/
virtual bool value_is_valid() const
{
THROW_ON_NULLPTR(this->value_ptr_);
*this->value_ptr_ = value;
if (value_ == std::numeric_limits<double>::quiet_NaN())
{
return false;
}
return true;
}

protected:
std::string prefix_name_;
std::string interface_name_;
// marks if value is new or has already been read
bool has_new_value_;
// stores if the stored value is valid
bool is_valid_;
// the current stored value of the handle
double value_;
};

class StateInterface : public ReadOnlyHandle
class StateInterface : public Handle
{
public:
StateInterface(const StateInterface & other) = default;
explicit StateInterface(const InterfaceDescription & interface_description)
: Handle(interface_description.prefix_name, interface_description.interface_info.name)
{
}

StateInterface(const StateInterface & other) = delete;

StateInterface(StateInterface && other) = default;

using ReadOnlyHandle::ReadOnlyHandle;
using Handle::Handle;
};

class CommandInterface : public ReadWriteHandle
class CommandInterface : public Handle
{
public:
explicit CommandInterface(const InterfaceDescription & interface_description)
: Handle(interface_description.prefix_name, interface_description.interface_info.name)
{
}
/// CommandInterface copy constructor is actively deleted.
/**
* Command interfaces are having a unique ownership and thus
Expand All @@ -136,7 +169,7 @@ class CommandInterface : public ReadWriteHandle

CommandInterface(CommandInterface && other) = default;

using ReadWriteHandle::ReadWriteHandle;
using Handle::Handle;
};

} // namespace hardware_interface
Expand Down
115 changes: 115 additions & 0 deletions hardware_interface/include/hardware_interface/hardware_info.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,12 +15,38 @@
#ifndef HARDWARE_INTERFACE__HARDWARE_INFO_HPP_
#define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_

#include <iostream>
#include <string>
#include <unordered_map>
#include <vector>

namespace hardware_interface
{

template <typename T>
std::ostream & operator<<(std::ostream & os, const std::vector<T> & input)
{
os << "<vector>[";
for (auto const & i : input)
{
os << i << ", ";
}
os << "]";
return os;
}

template <typename K, typename V>
std::ostream & operator<<(std::ostream & os, const std::unordered_map<K, V> & input)
{
os << "<map>[";
for (auto const & [key, value] : input)
{
os << "{" << key << ", " << value << "}, ";
}
os << "]";
return os;
}

/**
* This structure stores information about components defined for a specific hardware
* in robot's URDF.
Expand All @@ -42,6 +68,42 @@ struct InterfaceInfo
std::string data_type;
/// (Optional) If the handle is an array, the size of the array. Used by GPIOs.
int size;

friend std::ostream & operator<<(std::ostream & os, const InterfaceInfo & interface_info)
{
os << "<InterfaceInfo{{name:" << interface_info.name << "}, {min: " << interface_info.min
<< "}, {max: " << interface_info.max << "}, {initial_value: " << interface_info.initial_value
<< "}, {size: " << interface_info.size << "}}>";
return os;
}
};

/**
* This structure stores information about an interface for a specific hardware which should be
* instantiated internally.
*/
struct InterfaceDescription
{
InterfaceDescription(const std::string & prefix_name, const InterfaceInfo & interface_info)
: prefix_name(prefix_name), interface_info(interface_info)
{
}

/**
* Name of the interface defined by the user.
*/
std::string prefix_name;

InterfaceInfo interface_info;

friend std::ostream & operator<<(std::ostream & os, const InterfaceDescription & interface_descr)
{
os << "<InterfaceDescription{{prefix_name:" << interface_descr.prefix_name
<< "}, {interface_type: " << interface_descr.interface_info << "}}>";
return os;
}

// InterfaceInfo interface_info;
};

/**
Expand All @@ -67,6 +129,15 @@ struct ComponentInfo
std::vector<InterfaceInfo> state_interfaces;
/// (Optional) Key-value pairs of component parameters, e.g. min/max values or serial number.
std::unordered_map<std::string, std::string> parameters;

friend std::ostream & operator<<(std::ostream & os, const ComponentInfo & component_info)
{
os << "<ComponentInfo{{name:" << component_info.name << "}, {type: " << component_info.type
<< "}, {command_interfaces: " << component_info.command_interfaces
<< "}, {state_interfaces: " << component_info.state_interfaces
<< "}, {parameters: " << component_info.parameters << "}}>";
return os;
}
};

/// Contains semantic info about a given joint loaded from URDF for a transmission
Expand All @@ -78,6 +149,17 @@ struct JointInfo
std::string role;
double mechanical_reduction = 1.0;
double offset = 0.0;

friend std::ostream & operator<<(std::ostream & os, const JointInfo & joint_info)
{
os << "<JointInfo{{name:" << joint_info.name
<< "}, {state_interfaces: " << joint_info.state_interfaces
<< "}, {command_interfaces: " << joint_info.command_interfaces
<< "}, {role: " << joint_info.role
<< "}, {mechanical_reduction: " << joint_info.mechanical_reduction
<< "}, {offset: " << joint_info.offset << "}}>";
return os;
}
};

/// Contains semantic info about a given actuator loaded from URDF for a transmission
Expand All @@ -89,6 +171,17 @@ struct ActuatorInfo
std::string role;
double mechanical_reduction = 1.0;
double offset = 0.0;

friend std::ostream & operator<<(std::ostream & os, const ActuatorInfo & actuator_info)
{
os << "<ActuatorInfo{{name:" << actuator_info.name
<< "}, {state_interfaces: " << actuator_info.state_interfaces
<< "}, {command_interfaces: " << actuator_info.command_interfaces
<< "}, {role: " << actuator_info.role
<< "}, {mechanical_reduction: " << actuator_info.mechanical_reduction
<< "}, {offset: " << actuator_info.offset << "}}>";
return os;
}
};

/// Contains semantic info about a given transmission loaded from URDF
Expand All @@ -100,6 +193,15 @@ struct TransmissionInfo
std::vector<ActuatorInfo> actuators;
/// (Optional) Key-value pairs of custom parameters
std::unordered_map<std::string, std::string> parameters;

friend std::ostream & operator<<(std::ostream & os, const TransmissionInfo & transmission_info)
{
os << "<TransmissionInfo{{name:" << transmission_info.name
<< "}, {type: " << transmission_info.type << "}, {joints: " << transmission_info.joints
<< "}, {actuators: " << transmission_info.actuators
<< "}, {parameters: " << transmission_info.parameters << "}}>";
return os;
}
};

/// This structure stores information about hardware defined in a robot's URDF.
Expand Down Expand Up @@ -139,6 +241,19 @@ struct HardwareInfo
* The XML contents prior to parsing
*/
std::string original_xml;

friend std::ostream & operator<<(std::ostream & os, const HardwareInfo & hardware_info)
{
os << "<HardwareInfo{{name:" << hardware_info.name << "}, {type: " << hardware_info.type
<< "}, {is_async: " << hardware_info.is_async
<< "}, {hardware_plugin_name: " << hardware_info.hardware_plugin_name
<< "}, {hardware_parameters: " << hardware_info.hardware_parameters
<< "}, {joints: " << hardware_info.joints << "}, {sensors: " << hardware_info.sensors
<< "}, {gpios: " << hardware_info.gpios
<< "}, {transmissions: " << hardware_info.transmissions
<< "}, {original_xml: " << hardware_info.original_xml << "}}}";
return os;
}
};

} // namespace hardware_interface
Expand Down
Loading

0 comments on commit 3526deb

Please sign in to comment.