Skip to content

Commit

Permalink
use interfacedescription for creation of handle, depricate old way
Browse files Browse the repository at this point in the history
  • Loading branch information
mamueluth committed Aug 14, 2024
1 parent 07fb4f3 commit 7fe9238
Show file tree
Hide file tree
Showing 7 changed files with 285 additions and 47 deletions.
18 changes: 18 additions & 0 deletions hardware_interface/include/hardware_interface/component_parser.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,5 +32,23 @@ namespace hardware_interface
HARDWARE_INTERFACE_PUBLIC
std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string & urdf);

/**
* \param[in] component_info information about a component (gpio, joint, sensor)
* \return vector filled with information about hardware's StateInterfaces for the component
* which are exported
*/
HARDWARE_INTERFACE_PUBLIC
std::vector<InterfaceDescription> parse_state_interface_descriptions_from_hardware_info(
const std::vector<ComponentInfo> & component_info);

/**
* \param[in] component_info information about a component (gpio, joint, sensor)
* \return vector filled with information about hardware's CommandInterfaces for the component
* which are exported
*/
HARDWARE_INTERFACE_PUBLIC
std::vector<InterfaceDescription> parse_command_interface_descriptions_from_hardware_info(
const std::vector<ComponentInfo> & component_info);

} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_
101 changes: 60 additions & 41 deletions hardware_interface/include/hardware_interface/handle.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,42 +15,67 @@
#ifndef HARDWARE_INTERFACE__HANDLE_HPP_
#define HARDWARE_INTERFACE__HANDLE_HPP_

#include <limits>
#include <memory>
#include <string>
#include <utility>
#include <variant>

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

namespace hardware_interface
{

typedef std::variant<double> HANDLE_DATATYPE;

/// A handle used to get and set a value on a given interface.
class ReadOnlyHandle
class Handle
{
public:
ReadOnlyHandle(
[[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), value_ptr_(value_ptr)
{
}

explicit ReadOnlyHandle(const std::string & interface_name)
explicit Handle(const InterfaceDescription & interface_description)
: prefix_name_(interface_description.prefix_name),
interface_name_(interface_description.interface_info.name)
{
// As soon as multiple datatypes are used in HANDLE_DATATYPE
// we need to initialize according the type passed in interface description
value_ = std::numeric_limits<double>::quiet_NaN();
value_ptr_ = std::get_if<double>(&value_);
}

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

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

explicit ReadOnlyHandle(const char * interface_name)
[[deprecated("Use InterfaceDescription for initializing the Interface")]]

explicit Handle(const char * interface_name)
: interface_name_(interface_name), value_ptr_(nullptr)
{
}

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

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;
virtual ~Handle() = default;

/// Returns true if handle references a value.
inline operator bool() const { return value_ptr_ != nullptr; }
Expand All @@ -70,60 +95,54 @@ class ReadOnlyHandle

double get_value() const
{
// 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
}

void set_value(double value)
{
// BEGIN (Handle export change): for backward compatibility
// TODO(Manuel) set value_ directly if old functionality is removed
THROW_ON_NULLPTR(this->value_ptr_);
*this->value_ptr_ = value;
// END
}

protected:
std::string prefix_name_;
std::string interface_name_;
HANDLE_DATATYPE value_;
// BEGIN (Handle export change): for backward compatibility
// TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed
double * value_ptr_;
// END
};

class ReadWriteHandle : public ReadOnlyHandle
class StateInterface : public Handle
{
public:
ReadWriteHandle(
const std::string & prefix_name, const std::string & interface_name,
double * value_ptr = nullptr)
: ReadOnlyHandle(prefix_name, interface_name, value_ptr)
{
}

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;

void set_value(double value)
explicit StateInterface(const InterfaceDescription & interface_description)
: Handle(interface_description)
{
THROW_ON_NULLPTR(this->value_ptr_);
*this->value_ptr_ = value;
}
};

class StateInterface : public ReadOnlyHandle
{
public:
StateInterface(const StateInterface & other) = default;

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)
{
}
/// CommandInterface copy constructor is actively deleted.
/**
* Command interfaces are having a unique ownership and thus
Expand All @@ -134,7 +153,7 @@ class CommandInterface : public ReadWriteHandle

CommandInterface(CommandInterface && other) = default;

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

} // namespace hardware_interface
Expand Down
30 changes: 28 additions & 2 deletions hardware_interface/include/hardware_interface/hardware_info.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,9 +40,9 @@ struct InterfaceInfo
std::string max;
/// (Optional) Initial value of the interface.
std::string initial_value;
/// (Optional) The datatype of the interface, e.g. "bool", "int". Used by GPIOs.
/// (Optional) The datatype of the interface, e.g. "bool", "int".
std::string data_type;
/// (Optional) If the handle is an array, the size of the array. Used by GPIOs.
/// (Optional) If the handle is an array, the size of the array.
int size;
/// (Optional) enable or disable the limits for the command interfaces
bool enable_limits;
Expand Down Expand Up @@ -126,6 +126,32 @@ struct TransmissionInfo
std::unordered_map<std::string, std::string> parameters;
};

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

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

/**
* Information about the Interface type (position, velocity,...) as well as limits and so on.
*/
InterfaceInfo interface_info;

std::string get_name() const { return prefix_name + "/" + interface_info.name; }

std::string get_interface_type() const { return interface_info.name; }
};

/// This structure stores information about hardware defined in a robot's URDF.
struct HardwareInfo
{
Expand Down
34 changes: 34 additions & 0 deletions hardware_interface/src/component_parser.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -905,4 +905,38 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
return hardware_info;
}

std::vector<InterfaceDescription> parse_state_interface_descriptions_from_hardware_info(
const std::vector<ComponentInfo> & component_info)
{
std::vector<InterfaceDescription> component_state_interface_descriptions;
component_state_interface_descriptions.reserve(component_info.size());

for (const auto & component : component_info)
{
for (const auto & state_interface : component.state_interfaces)
{
component_state_interface_descriptions.emplace_back(
InterfaceDescription(component.name, state_interface));
}
}
return component_state_interface_descriptions;
}

std::vector<InterfaceDescription> parse_command_interface_descriptions_from_hardware_info(
const std::vector<ComponentInfo> & component_info)
{
std::vector<InterfaceDescription> component_command_interface_descriptions;
component_command_interface_descriptions.reserve(component_info.size());

for (const auto & component : component_info)
{
for (const auto & command_interface : component.command_interfaces)
{
component_command_interface_descriptions.emplace_back(
InterfaceDescription(component.name, command_interface));
}
}
return component_command_interface_descriptions;
}

} // namespace hardware_interface
110 changes: 110 additions & 0 deletions hardware_interface/test/test_component_parser.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1404,3 +1404,113 @@ TEST_F(TestComponentParser, urdf_incomplete_throws_error)
std::string(ros2_control_test_assets::urdf_tail);
ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
}

TEST_F(TestComponentParser, parse_joint_state_interface_descriptions_from_hardware_info)
{
const std::string urdf_to_test =
std::string(ros2_control_test_assets::urdf_head) +
ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);

const auto joint_state_descriptions =
parse_state_interface_descriptions_from_hardware_info(control_hardware[0].joints);
EXPECT_EQ(joint_state_descriptions[0].prefix_name, "joint1");
EXPECT_EQ(joint_state_descriptions[0].get_interface_type(), "position");
EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position");

EXPECT_EQ(joint_state_descriptions[1].prefix_name, "joint2");
EXPECT_EQ(joint_state_descriptions[1].get_interface_type(), "position");
EXPECT_EQ(joint_state_descriptions[1].get_name(), "joint2/position");
}

TEST_F(TestComponentParser, parse_joint_command_interface_descriptions_from_hardware_info)
{
const std::string urdf_to_test =
std::string(ros2_control_test_assets::urdf_head) +
ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);

const auto joint_command_descriptions =
parse_command_interface_descriptions_from_hardware_info(control_hardware[0].joints);
EXPECT_EQ(joint_command_descriptions[0].prefix_name, "joint1");
EXPECT_EQ(joint_command_descriptions[0].get_interface_type(), "position");
EXPECT_EQ(joint_command_descriptions[0].get_name(), "joint1/position");
EXPECT_EQ(joint_command_descriptions[0].interface_info.min, "-1");
EXPECT_EQ(joint_command_descriptions[0].interface_info.max, "1");

EXPECT_EQ(joint_command_descriptions[1].prefix_name, "joint2");
EXPECT_EQ(joint_command_descriptions[1].get_interface_type(), "position");
EXPECT_EQ(joint_command_descriptions[1].get_name(), "joint2/position");
EXPECT_EQ(joint_command_descriptions[1].interface_info.min, "-1");
EXPECT_EQ(joint_command_descriptions[1].interface_info.max, "1");
}

TEST_F(TestComponentParser, parse_sensor_state_interface_descriptions_from_hardware_info)
{
const std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) +
ros2_control_test_assets::valid_urdf_ros2_control_sensor_only +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);

const auto sensor_state_descriptions =
parse_state_interface_descriptions_from_hardware_info(control_hardware[0].sensors);
EXPECT_EQ(sensor_state_descriptions[0].prefix_name, "sensor1");
EXPECT_EQ(sensor_state_descriptions[0].get_interface_type(), "roll");
EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll");
EXPECT_EQ(sensor_state_descriptions[1].prefix_name, "sensor1");
EXPECT_EQ(sensor_state_descriptions[1].get_interface_type(), "pitch");
EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch");
EXPECT_EQ(sensor_state_descriptions[2].prefix_name, "sensor1");
EXPECT_EQ(sensor_state_descriptions[2].get_interface_type(), "yaw");
EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw");

EXPECT_EQ(sensor_state_descriptions[3].prefix_name, "sensor2");
EXPECT_EQ(sensor_state_descriptions[3].get_interface_type(), "image");
EXPECT_EQ(sensor_state_descriptions[3].get_name(), "sensor2/image");
}

TEST_F(TestComponentParser, parse_gpio_state_interface_descriptions_from_hardware_info)
{
const std::string urdf_to_test =
std::string(ros2_control_test_assets::urdf_head) +
ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);

const auto gpio_state_descriptions =
parse_state_interface_descriptions_from_hardware_info(control_hardware[0].gpios);
EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs");
EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1");
EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1");
EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_analog_IOs");
EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "analog_input1");
EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1");
EXPECT_EQ(gpio_state_descriptions[2].prefix_name, "flange_analog_IOs");
EXPECT_EQ(gpio_state_descriptions[2].get_interface_type(), "analog_input2");
EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2");

EXPECT_EQ(gpio_state_descriptions[3].prefix_name, "flange_vacuum");
EXPECT_EQ(gpio_state_descriptions[3].get_interface_type(), "vacuum");
EXPECT_EQ(gpio_state_descriptions[3].get_name(), "flange_vacuum/vacuum");
}

TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardware_info)
{
const std::string urdf_to_test =
std::string(ros2_control_test_assets::urdf_head) +
ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);

const auto gpio_state_descriptions =
parse_command_interface_descriptions_from_hardware_info(control_hardware[0].gpios);
EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs");
EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1");
EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1");

EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_vacuum");
EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "vacuum");
EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum");
}
Loading

0 comments on commit 7fe9238

Please sign in to comment.