Skip to content

Commit

Permalink
Introduce Creation of Handles with InterfaceDescription (variant supp…
Browse files Browse the repository at this point in the history
  • Loading branch information
mamueluth authored Aug 21, 2024
1 parent fb1d254 commit 40ea191
Show file tree
Hide file tree
Showing 6 changed files with 252 additions and 10 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(
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(
const std::vector<ComponentInfo> & component_info);

} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_
30 changes: 22 additions & 8 deletions hardware_interface/include/hardware_interface/handle.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,9 +15,11 @@
#ifndef HARDWARE_INTERFACE__HANDLE_HPP_
#define HARDWARE_INTERFACE__HANDLE_HPP_

#include <limits>
#include <string>
#include <variant>

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

namespace hardware_interface
Expand All @@ -29,18 +31,34 @@ using HANDLE_DATATYPE = std::variant<double>;
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), value_ptr_(value_ptr)
{
}

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)
{
}

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

explicit Handle(const char * interface_name)
: interface_name_(interface_name), value_ptr_(nullptr)
{
Expand Down Expand Up @@ -103,10 +121,8 @@ class Handle
class StateInterface : public Handle
{
public:
explicit StateInterface(
const std::string & prefix_name, const std::string & interface_name,
double * value_ptr = nullptr)
: Handle(prefix_name, interface_name, value_ptr)
explicit StateInterface(const InterfaceDescription & interface_description)
: Handle(interface_description)
{
}

Expand All @@ -120,10 +136,8 @@ class StateInterface : public Handle
class CommandInterface : public Handle
{
public:
explicit CommandInterface(
const std::string & prefix_name, const std::string & interface_name,
double * value_ptr = nullptr)
: Handle(prefix_name, interface_name, value_ptr)
explicit CommandInterface(const InterfaceDescription & interface_description)
: Handle(interface_description)
{
}
/// CommandInterface copy constructor is actively deleted.
Expand Down
39 changes: 37 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,41 @@ 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),
interface_name(prefix_name + "/" + interface_info.name)
{
}

/**
* 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;

/**
* Name of the interface
*/
std::string interface_name;

std::string get_prefix_name() const { return prefix_name; }

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

std::string get_name() const { return interface_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(
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(
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(control_hardware[0].joints);
EXPECT_EQ(joint_state_descriptions[0].get_prefix_name(), "joint1");
EXPECT_EQ(joint_state_descriptions[0].get_interface_name(), "position");
EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position");

EXPECT_EQ(joint_state_descriptions[1].get_prefix_name(), "joint2");
EXPECT_EQ(joint_state_descriptions[1].get_interface_name(), "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(control_hardware[0].joints);
EXPECT_EQ(joint_command_descriptions[0].get_prefix_name(), "joint1");
EXPECT_EQ(joint_command_descriptions[0].get_interface_name(), "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].get_prefix_name(), "joint2");
EXPECT_EQ(joint_command_descriptions[1].get_interface_name(), "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(control_hardware[0].sensors);
EXPECT_EQ(sensor_state_descriptions[0].get_prefix_name(), "sensor1");
EXPECT_EQ(sensor_state_descriptions[0].get_interface_name(), "roll");
EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll");
EXPECT_EQ(sensor_state_descriptions[1].get_prefix_name(), "sensor1");
EXPECT_EQ(sensor_state_descriptions[1].get_interface_name(), "pitch");
EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch");
EXPECT_EQ(sensor_state_descriptions[2].get_prefix_name(), "sensor1");
EXPECT_EQ(sensor_state_descriptions[2].get_interface_name(), "yaw");
EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw");

EXPECT_EQ(sensor_state_descriptions[3].get_prefix_name(), "sensor2");
EXPECT_EQ(sensor_state_descriptions[3].get_interface_name(), "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(control_hardware[0].gpios);
EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs");
EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1");
EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1");
EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_analog_IOs");
EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "analog_input1");
EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1");
EXPECT_EQ(gpio_state_descriptions[2].get_prefix_name(), "flange_analog_IOs");
EXPECT_EQ(gpio_state_descriptions[2].get_interface_name(), "analog_input2");
EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2");

EXPECT_EQ(gpio_state_descriptions[3].get_prefix_name(), "flange_vacuum");
EXPECT_EQ(gpio_state_descriptions[3].get_interface_name(), "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(control_hardware[0].gpios);
EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs");
EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1");
EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1");

EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_vacuum");
EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "vacuum");
EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum");
}
31 changes: 31 additions & 0 deletions hardware_interface/test/test_handle.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,11 @@

#include <gmock/gmock.h>
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"

using hardware_interface::CommandInterface;
using hardware_interface::InterfaceDescription;
using hardware_interface::InterfaceInfo;
using hardware_interface::StateInterface;

namespace
Expand Down Expand Up @@ -64,3 +67,31 @@ TEST(TestHandle, value_methods_work_on_non_nullptr)
EXPECT_NO_THROW(handle.set_value(0.0));
EXPECT_DOUBLE_EQ(handle.get_value(), 0.0);
}

TEST(TestHandle, interface_description_state_interface_name_getters_work)
{
const std::string POSITION_INTERFACE = "position";
const std::string JOINT_NAME_1 = "joint1";
InterfaceInfo info;
info.name = POSITION_INTERFACE;
InterfaceDescription interface_descr(JOINT_NAME_1, info);
StateInterface handle{interface_descr};

EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE);
EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE);
EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1);
}

TEST(TestHandle, interface_description_command_interface_name_getters_work)
{
const std::string POSITION_INTERFACE = "position";
const std::string JOINT_NAME_1 = "joint1";
InterfaceInfo info;
info.name = POSITION_INTERFACE;
InterfaceDescription interface_descr(JOINT_NAME_1, info);
CommandInterface handle{interface_descr};

EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE);
EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE);
EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1);
}

0 comments on commit 40ea191

Please sign in to comment.