Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[HW Interface] Command-/SystemInterface export via description. Handles store value instead of pointers #952

Closed
wants to merge 1 commit into from
Closed

[HW Interface] Command-/SystemInterface export via description. Handles store value instead of pointers #952

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
141 changes: 87 additions & 54 deletions hardware_interface/include/hardware_interface/handle.hpp
View file
Open in desktop
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
Open in desktop
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
Loading