Skip to content

Commit 40ea191

Browse files
mamueluthmamueluth
authored
Introduce Creation of Handles with InterfaceDescription (variant support) (#1679)
1 parent fb1d254 commit 40ea191

File tree

6 files changed

+252
-10
lines changed

6 files changed

+252
-10
lines changed

‎hardware_interface/include/hardware_interface/component_parser.hpp

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,5 +32,23 @@ namespace hardware_interface
3232
HARDWARE_INTERFACE_PUBLIC
3333
std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string & urdf);
3434

35+
/**
36+
* \param[in] component_info information about a component (gpio, joint, sensor)
37+
* \return vector filled with information about hardware's StateInterfaces for the component
38+
* which are exported
39+
*/
40+
HARDWARE_INTERFACE_PUBLIC
41+
std::vector<InterfaceDescription> parse_state_interface_descriptions(
42+
const std::vector<ComponentInfo> & component_info);
43+
44+
/**
45+
* \param[in] component_info information about a component (gpio, joint, sensor)
46+
* \return vector filled with information about hardware's CommandInterfaces for the component
47+
* which are exported
48+
*/
49+
HARDWARE_INTERFACE_PUBLIC
50+
std::vector<InterfaceDescription> parse_command_interface_descriptions(
51+
const std::vector<ComponentInfo> & component_info);
52+
3553
} // namespace hardware_interface
3654
#endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_

‎hardware_interface/include/hardware_interface/handle.hpp

Lines changed: 22 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -15,9 +15,11 @@
1515
#ifndef HARDWARE_INTERFACE__HANDLE_HPP_
1616
#define HARDWARE_INTERFACE__HANDLE_HPP_
1717

18+
#include <limits>
1819
#include <string>
1920
#include <variant>
2021

22+
#include "hardware_interface/hardware_info.hpp"
2123
#include "hardware_interface/macros.hpp"
2224

2325
namespace hardware_interface
@@ -29,18 +31,34 @@ using HANDLE_DATATYPE = std::variant<double>;
2931
class Handle
3032
{
3133
public:
34+
[[deprecated("Use InterfaceDescription for initializing the Interface")]]
35+
3236
Handle(
3337
const std::string & prefix_name, const std::string & interface_name,
3438
double * value_ptr = nullptr)
3539
: prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr)
3640
{
3741
}
3842

43+
explicit Handle(const InterfaceDescription & interface_description)
44+
: prefix_name_(interface_description.prefix_name),
45+
interface_name_(interface_description.interface_info.name)
46+
{
47+
// As soon as multiple datatypes are used in HANDLE_DATATYPE
48+
// we need to initialize according the type passed in interface description
49+
value_ = std::numeric_limits<double>::quiet_NaN();
50+
value_ptr_ = std::get_if<double>(&value_);
51+
}
52+
53+
[[deprecated("Use InterfaceDescription for initializing the Interface")]]
54+
3955
explicit Handle(const std::string & interface_name)
4056
: interface_name_(interface_name), value_ptr_(nullptr)
4157
{
4258
}
4359

60+
[[deprecated("Use InterfaceDescription for initializing the Interface")]]
61+
4462
explicit Handle(const char * interface_name)
4563
: interface_name_(interface_name), value_ptr_(nullptr)
4664
{
@@ -103,10 +121,8 @@ class Handle
103121
class StateInterface : public Handle
104122
{
105123
public:
106-
explicit StateInterface(
107-
const std::string & prefix_name, const std::string & interface_name,
108-
double * value_ptr = nullptr)
109-
: Handle(prefix_name, interface_name, value_ptr)
124+
explicit StateInterface(const InterfaceDescription & interface_description)
125+
: Handle(interface_description)
110126
{
111127
}
112128

@@ -120,10 +136,8 @@ class StateInterface : public Handle
120136
class CommandInterface : public Handle
121137
{
122138
public:
123-
explicit CommandInterface(
124-
const std::string & prefix_name, const std::string & interface_name,
125-
double * value_ptr = nullptr)
126-
: Handle(prefix_name, interface_name, value_ptr)
139+
explicit CommandInterface(const InterfaceDescription & interface_description)
140+
: Handle(interface_description)
127141
{
128142
}
129143
/// CommandInterface copy constructor is actively deleted.

‎hardware_interface/include/hardware_interface/hardware_info.hpp

Lines changed: 37 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -40,9 +40,9 @@ struct InterfaceInfo
4040
std::string max;
4141
/// (Optional) Initial value of the interface.
4242
std::string initial_value;
43-
/// (Optional) The datatype of the interface, e.g. "bool", "int". Used by GPIOs.
43+
/// (Optional) The datatype of the interface, e.g. "bool", "int".
4444
std::string data_type;
45-
/// (Optional) If the handle is an array, the size of the array. Used by GPIOs.
45+
/// (Optional) If the handle is an array, the size of the array.
4646
int size;
4747
/// (Optional) enable or disable the limits for the command interfaces
4848
bool enable_limits;
@@ -126,6 +126,41 @@ struct TransmissionInfo
126126
std::unordered_map<std::string, std::string> parameters;
127127
};
128128

129+
/**
130+
* This structure stores information about an interface for a specific hardware which should be
131+
* instantiated internally.
132+
*/
133+
struct InterfaceDescription
134+
{
135+
InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in)
136+
: prefix_name(prefix_name_in),
137+
interface_info(interface_info_in),
138+
interface_name(prefix_name + "/" + interface_info.name)
139+
{
140+
}
141+
142+
/**
143+
* Name of the interface defined by the user.
144+
*/
145+
std::string prefix_name;
146+
147+
/**
148+
* Information about the Interface type (position, velocity,...) as well as limits and so on.
149+
*/
150+
InterfaceInfo interface_info;
151+
152+
/**
153+
* Name of the interface
154+
*/
155+
std::string interface_name;
156+
157+
std::string get_prefix_name() const { return prefix_name; }
158+
159+
std::string get_interface_name() const { return interface_info.name; }
160+
161+
std::string get_name() const { return interface_name; }
162+
};
163+
129164
/// This structure stores information about hardware defined in a robot's URDF.
130165
struct HardwareInfo
131166
{

‎hardware_interface/src/component_parser.cpp

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -905,4 +905,38 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
905905
return hardware_info;
906906
}
907907

908+
std::vector<InterfaceDescription> parse_state_interface_descriptions(
909+
const std::vector<ComponentInfo> & component_info)
910+
{
911+
std::vector<InterfaceDescription> component_state_interface_descriptions;
912+
component_state_interface_descriptions.reserve(component_info.size());
913+
914+
for (const auto & component : component_info)
915+
{
916+
for (const auto & state_interface : component.state_interfaces)
917+
{
918+
component_state_interface_descriptions.emplace_back(
919+
InterfaceDescription(component.name, state_interface));
920+
}
921+
}
922+
return component_state_interface_descriptions;
923+
}
924+
925+
std::vector<InterfaceDescription> parse_command_interface_descriptions(
926+
const std::vector<ComponentInfo> & component_info)
927+
{
928+
std::vector<InterfaceDescription> component_command_interface_descriptions;
929+
component_command_interface_descriptions.reserve(component_info.size());
930+
931+
for (const auto & component : component_info)
932+
{
933+
for (const auto & command_interface : component.command_interfaces)
934+
{
935+
component_command_interface_descriptions.emplace_back(
936+
InterfaceDescription(component.name, command_interface));
937+
}
938+
}
939+
return component_command_interface_descriptions;
940+
}
941+
908942
} // namespace hardware_interface

‎hardware_interface/test/test_component_parser.cpp

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1404,3 +1404,113 @@ TEST_F(TestComponentParser, urdf_incomplete_throws_error)
14041404
std::string(ros2_control_test_assets::urdf_tail);
14051405
ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
14061406
}
1407+
1408+
TEST_F(TestComponentParser, parse_joint_state_interface_descriptions_from_hardware_info)
1409+
{
1410+
const std::string urdf_to_test =
1411+
std::string(ros2_control_test_assets::urdf_head) +
1412+
ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission +
1413+
ros2_control_test_assets::urdf_tail;
1414+
const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
1415+
1416+
const auto joint_state_descriptions =
1417+
parse_state_interface_descriptions(control_hardware[0].joints);
1418+
EXPECT_EQ(joint_state_descriptions[0].get_prefix_name(), "joint1");
1419+
EXPECT_EQ(joint_state_descriptions[0].get_interface_name(), "position");
1420+
EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position");
1421+
1422+
EXPECT_EQ(joint_state_descriptions[1].get_prefix_name(), "joint2");
1423+
EXPECT_EQ(joint_state_descriptions[1].get_interface_name(), "position");
1424+
EXPECT_EQ(joint_state_descriptions[1].get_name(), "joint2/position");
1425+
}
1426+
1427+
TEST_F(TestComponentParser, parse_joint_command_interface_descriptions_from_hardware_info)
1428+
{
1429+
const std::string urdf_to_test =
1430+
std::string(ros2_control_test_assets::urdf_head) +
1431+
ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission +
1432+
ros2_control_test_assets::urdf_tail;
1433+
const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
1434+
1435+
const auto joint_command_descriptions =
1436+
parse_command_interface_descriptions(control_hardware[0].joints);
1437+
EXPECT_EQ(joint_command_descriptions[0].get_prefix_name(), "joint1");
1438+
EXPECT_EQ(joint_command_descriptions[0].get_interface_name(), "position");
1439+
EXPECT_EQ(joint_command_descriptions[0].get_name(), "joint1/position");
1440+
EXPECT_EQ(joint_command_descriptions[0].interface_info.min, "-1");
1441+
EXPECT_EQ(joint_command_descriptions[0].interface_info.max, "1");
1442+
1443+
EXPECT_EQ(joint_command_descriptions[1].get_prefix_name(), "joint2");
1444+
EXPECT_EQ(joint_command_descriptions[1].get_interface_name(), "position");
1445+
EXPECT_EQ(joint_command_descriptions[1].get_name(), "joint2/position");
1446+
EXPECT_EQ(joint_command_descriptions[1].interface_info.min, "-1");
1447+
EXPECT_EQ(joint_command_descriptions[1].interface_info.max, "1");
1448+
}
1449+
1450+
TEST_F(TestComponentParser, parse_sensor_state_interface_descriptions_from_hardware_info)
1451+
{
1452+
const std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) +
1453+
ros2_control_test_assets::valid_urdf_ros2_control_sensor_only +
1454+
ros2_control_test_assets::urdf_tail;
1455+
const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
1456+
1457+
const auto sensor_state_descriptions =
1458+
parse_state_interface_descriptions(control_hardware[0].sensors);
1459+
EXPECT_EQ(sensor_state_descriptions[0].get_prefix_name(), "sensor1");
1460+
EXPECT_EQ(sensor_state_descriptions[0].get_interface_name(), "roll");
1461+
EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll");
1462+
EXPECT_EQ(sensor_state_descriptions[1].get_prefix_name(), "sensor1");
1463+
EXPECT_EQ(sensor_state_descriptions[1].get_interface_name(), "pitch");
1464+
EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch");
1465+
EXPECT_EQ(sensor_state_descriptions[2].get_prefix_name(), "sensor1");
1466+
EXPECT_EQ(sensor_state_descriptions[2].get_interface_name(), "yaw");
1467+
EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw");
1468+
1469+
EXPECT_EQ(sensor_state_descriptions[3].get_prefix_name(), "sensor2");
1470+
EXPECT_EQ(sensor_state_descriptions[3].get_interface_name(), "image");
1471+
EXPECT_EQ(sensor_state_descriptions[3].get_name(), "sensor2/image");
1472+
}
1473+
1474+
TEST_F(TestComponentParser, parse_gpio_state_interface_descriptions_from_hardware_info)
1475+
{
1476+
const std::string urdf_to_test =
1477+
std::string(ros2_control_test_assets::urdf_head) +
1478+
ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio +
1479+
ros2_control_test_assets::urdf_tail;
1480+
const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
1481+
1482+
const auto gpio_state_descriptions =
1483+
parse_state_interface_descriptions(control_hardware[0].gpios);
1484+
EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs");
1485+
EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1");
1486+
EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1");
1487+
EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_analog_IOs");
1488+
EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "analog_input1");
1489+
EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1");
1490+
EXPECT_EQ(gpio_state_descriptions[2].get_prefix_name(), "flange_analog_IOs");
1491+
EXPECT_EQ(gpio_state_descriptions[2].get_interface_name(), "analog_input2");
1492+
EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2");
1493+
1494+
EXPECT_EQ(gpio_state_descriptions[3].get_prefix_name(), "flange_vacuum");
1495+
EXPECT_EQ(gpio_state_descriptions[3].get_interface_name(), "vacuum");
1496+
EXPECT_EQ(gpio_state_descriptions[3].get_name(), "flange_vacuum/vacuum");
1497+
}
1498+
1499+
TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardware_info)
1500+
{
1501+
const std::string urdf_to_test =
1502+
std::string(ros2_control_test_assets::urdf_head) +
1503+
ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio +
1504+
ros2_control_test_assets::urdf_tail;
1505+
const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
1506+
1507+
const auto gpio_state_descriptions =
1508+
parse_command_interface_descriptions(control_hardware[0].gpios);
1509+
EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs");
1510+
EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1");
1511+
EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1");
1512+
1513+
EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_vacuum");
1514+
EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "vacuum");
1515+
EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum");
1516+
}

‎hardware_interface/test/test_handle.cpp

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,11 @@
1414

1515
#include <gmock/gmock.h>
1616
#include "hardware_interface/handle.hpp"
17+
#include "hardware_interface/hardware_info.hpp"
1718

1819
using hardware_interface::CommandInterface;
20+
using hardware_interface::InterfaceDescription;
21+
using hardware_interface::InterfaceInfo;
1922
using hardware_interface::StateInterface;
2023

2124
namespace
@@ -64,3 +67,31 @@ TEST(TestHandle, value_methods_work_on_non_nullptr)
6467
EXPECT_NO_THROW(handle.set_value(0.0));
6568
EXPECT_DOUBLE_EQ(handle.get_value(), 0.0);
6669
}
70+
71+
TEST(TestHandle, interface_description_state_interface_name_getters_work)
72+
{
73+
const std::string POSITION_INTERFACE = "position";
74+
const std::string JOINT_NAME_1 = "joint1";
75+
InterfaceInfo info;
76+
info.name = POSITION_INTERFACE;
77+
InterfaceDescription interface_descr(JOINT_NAME_1, info);
78+
StateInterface handle{interface_descr};
79+
80+
EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE);
81+
EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE);
82+
EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1);
83+
}
84+
85+
TEST(TestHandle, interface_description_command_interface_name_getters_work)
86+
{
87+
const std::string POSITION_INTERFACE = "position";
88+
const std::string JOINT_NAME_1 = "joint1";
89+
InterfaceInfo info;
90+
info.name = POSITION_INTERFACE;
91+
InterfaceDescription interface_descr(JOINT_NAME_1, info);
92+
CommandInterface handle{interface_descr};
93+
94+
EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE);
95+
EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE);
96+
EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1);
97+
}

0 commit comments

Comments
 (0)