From 8fd2e707f80d2f0fe0fbff3ca38582cae54a3ade Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sat, 26 Sep 2020 23:45:03 -0500 Subject: [PATCH 1/9] Add a struct for Interface information, update the test URDF --- .../components/component_info.hpp | 23 ++- .../hardware_interface/components/joint.hpp | 28 ++-- .../hardware_interface/components/sensor.hpp | 14 +- hardware_interface/src/component_parser.cpp | 16 +- .../components/component_lists_management.hpp | 45 ++--- hardware_interface/src/components/joint.cpp | 14 +- hardware_interface/src/components/sensor.cpp | 7 +- .../test/test_component_interfaces.cpp | 156 ++++++++++-------- .../test/test_component_parser.cpp | 68 ++++---- 9 files changed, 203 insertions(+), 168 deletions(-) diff --git a/hardware_interface/include/hardware_interface/components/component_info.hpp b/hardware_interface/include/hardware_interface/components/component_info.hpp index 0dc1b78dd2..a93be6fddb 100644 --- a/hardware_interface/include/hardware_interface/components/component_info.hpp +++ b/hardware_interface/include/hardware_interface/components/component_info.hpp @@ -24,6 +24,23 @@ namespace hardware_interface namespace components { +/** + * \brief This structure stores information about components defined for a specific hardware + * in robot's URDF. + */ +struct InterfaceInfo +{ + /** + * \brief name of the command interfaces that can be set, e.g. "position", "velocity", etc. + * Used by joints. + */ + std::string name; + /** + * \brief (optional) key-value pairs of components parameters, e.g. min_value and max_value. + */ + std::unordered_map parameters; +}; + /** * \brief This structure stores information about components defined for a specific hardware * in robot's URDF. @@ -46,14 +63,14 @@ struct ComponentInfo * \brief name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints. */ - std::vector command_interfaces; + std::vector command_interfaces; /** * \brief name of the state interfaces that can be read, e.g. "position", "velocity", etc. * Used by Joints and Sensors. */ - std::vector state_interfaces; + std::vector state_interfaces; /** - * \brief (optional) key-value pairs of components parameters. + * \brief (optional) key-value pairs of components parameters, e.g. min_value and max_value. */ std::unordered_map parameters; }; diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp index 0101afa6f0..14dd8338bc 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -50,7 +50,6 @@ class Joint * return_type::ERROR otherwise. */ HARDWARE_INTERFACE_PUBLIC - virtual return_type configure(const ComponentInfo & joint_info); /** @@ -59,8 +58,7 @@ class Joint * \return string list with command interfaces. */ HARDWARE_INTERFACE_PUBLIC - virtual - std::vector get_command_interfaces() const; + std::vector get_command_interfaces() const; /** * \brief Provide the list of state interfaces configured for the joint. @@ -68,8 +66,7 @@ class Joint * \return string list with state interfaces. */ HARDWARE_INTERFACE_PUBLIC - virtual - std::vector get_state_interfaces() const; + std::vector get_state_interfaces() const; /** * \brief Get command list from the joint. This function is used in the write function of the @@ -85,9 +82,9 @@ class Joint * is empty; return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - virtual return_type get_command( - std::vector & command, const std::vector & interfaces) const; + std::vector & command, + const std::vector & interfaces) const; /** * \brief Get complete command list for the joint. This function is used by the hardware to get @@ -98,7 +95,6 @@ class Joint * \return return_type::OK always. */ HARDWARE_INTERFACE_EXPORT - virtual return_type get_command(std::vector & command) const; /** @@ -118,9 +114,9 @@ class Joint * (see: https://github.com/ros-controls/ros2_control/issues/129) */ HARDWARE_INTERFACE_EXPORT - virtual return_type set_command( - const std::vector & command, const std::vector & interfaces); + const std::vector & command, + const std::vector & interfaces); /** * \brief Get complete state list from the joint. This function is used by the hardware to get @@ -133,10 +129,9 @@ class Joint * of limits; return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - virtual return_type set_command(const std::vector & command); - /** + /* * \brief Get state list from the joint. This function is used by the controller to get the * actual state of the hardware. The parameters state, and interfaces have the same order and * number of elements. Using the interfaces list, the controller can choose which values to get. @@ -149,10 +144,9 @@ class Joint * is empty; return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - virtual return_type get_state( std::vector & state, - const std::vector & interfaces) const; + const std::vector & interfaces) const; /** * \brief Get complete state list from the joint. This function is used by the controller to get @@ -163,7 +157,6 @@ class Joint * \return return_type::OK always. */ HARDWARE_INTERFACE_EXPORT - virtual return_type get_state(std::vector & state) const; /** @@ -178,9 +171,9 @@ class Joint * defined for the joint; return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - virtual return_type set_state( - const std::vector & state, const std::vector & interfaces); + const std::vector & state, + const std::vector & interfaces); /** * \brief Set complete state list from the joint.This function is used by the hardware to set its @@ -192,7 +185,6 @@ class Joint * joint's state interfaces, return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - virtual return_type set_state(const std::vector & state); protected: diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp index b44fb12718..a0cdf5f55f 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -50,7 +50,6 @@ class Sensor * return_type::ERROR otherwise. */ HARDWARE_INTERFACE_PUBLIC - virtual return_type configure(const ComponentInfo & joint_info); /** @@ -59,8 +58,7 @@ class Sensor * \return string list with state interfaces. */ HARDWARE_INTERFACE_PUBLIC - virtual - std::vector get_state_interfaces() const; + std::vector get_state_interfaces(); /** * \brief Get state list from the sensor. This function is used by the controller to get the @@ -75,9 +73,9 @@ class Sensor * is empty; return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - virtual return_type get_state( - std::vector & state, const std::vector & interfaces) const; + std::vector & state, + const std::vector & interfaces) const; /** * \brief Get complete state list from the sensor. This function is used by the controller to get @@ -88,7 +86,6 @@ class Sensor * \return return_type::OK always. */ HARDWARE_INTERFACE_EXPORT - virtual return_type get_state(std::vector & state) const; /** @@ -103,9 +100,9 @@ class Sensor * defined for the sensor; return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - virtual return_type set_state( - const std::vector & state, const std::vector & interfaces); + const std::vector & state, + const std::vector & interfaces); /** * \brief Set complete state list from the sensor.This function is used by the hardware to set its @@ -117,7 +114,6 @@ class Sensor * sensor's state interfaces, return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - virtual return_type set_state(const std::vector & state); protected: diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 3841be6c76..a3225bec3a 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -132,20 +132,25 @@ std::unordered_map parse_parameters_from_xml( /** * \brief Search XML snippet for definition of interfaceTypes. * - * \param interfaces_it pointer to the interator over interfaces + * \param interfaces_it pointer to the iterator over interfaces * \param interfaceTag interface type tag (command or state) * \return std::vector< std::__cxx11::string > list of interface types * \throws std::runtime_error if the interfaceType text not set in a tag */ -std::vector parse_interfaces_from_xml( +std::vector parse_interfaces_from_xml( const tinyxml2::XMLElement * interfaces_it, const char * interfaceTag) { - std::vector interfaces; + std::vector interfaces; + // TODO(andyz): parse optional attributes like min_value/max_value while (interfaces_it) { const std::string interface_type = get_text_for_element( interfaces_it, std::string(interfaceTag) + " type "); - interfaces.push_back(interface_type); + // TODO(andyz): select the proper type (switch statement?) + // For now, just assume a position type + hardware_interface::components::InterfaceInfo position_interface; + position_interface.name = "position"; + interfaces.push_back(position_interface); interfaces_it = interfaces_it->NextSiblingElement(interfaceTag); } return interfaces; @@ -173,12 +178,13 @@ components::ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component.class_type = get_text_for_element(classType_it, component.name + " " + kClassTypeTag); // Parse commandInterfaceType tags +/* const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTypeTag); if (command_interfaces_it) { component.command_interfaces = parse_interfaces_from_xml( command_interfaces_it, kCommandInterfaceTypeTag); } - +*/ // Parse stateInterfaceType tags const auto * state_interfaces_it = component_it->FirstChildElement(kStateInterfaceTypeTag); if (state_interfaces_it) { diff --git a/hardware_interface/src/components/component_lists_management.hpp b/hardware_interface/src/components/component_lists_management.hpp index deb949a980..784f88078e 100644 --- a/hardware_interface/src/components/component_lists_management.hpp +++ b/hardware_interface/src/components/component_lists_management.hpp @@ -26,7 +26,6 @@ namespace hardware_interface { namespace components { - /** * \brief Get values for queried_interfaces from the int_values. int_values data structure matches * int_interfaces vector. @@ -41,23 +40,10 @@ namespace components * list is is empty; return_type::OK otherwise. */ inline return_type get_internal_values( - std::vector & values, const std::vector & queried_interfaces, - const std::vector & int_interfaces, const std::vector & int_values) + std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, const std::vector & internal_values) { - if (queried_interfaces.size() == 0) { - return return_type::INTERFACE_NOT_PROVIDED; - } - - for (const auto & interface : queried_interfaces) { - auto it = std::find( - int_interfaces.begin(), int_interfaces.end(), interface); - if (it != int_interfaces.end()) { - values.push_back(int_values[std::distance(int_interfaces.begin(), it)]); - } else { - values.clear(); - return return_type::INTERFACE_NOT_FOUND; - } - } + // TODO(andyz) return return_type::OK; } @@ -65,27 +51,27 @@ inline return_type get_internal_values( * \brief Set all internal values to to other vector. Return value is used for API consistency. * * \param values output list of values. - * \param int_values internal values of the component. + * \param internal_values internal values of the component. * \return return_type::OK always. */ inline return_type get_internal_values( - std::vector & values, const std::vector & int_values) + std::vector & values, const std::vector & internal_values) { values.clear(); - for (const auto & int_value : int_values) { + for (const auto & int_value : internal_values) { values.push_back(int_value); } return return_type::OK; } /** - * \brief set values for queried_interfaces to the int_values. int_values data structure matches + * \brief set values for queried_interfaces to the internal_values. internal_values data structure matches * int_interfaces vector. * * \param values values to set. * \param queried_interfaces interfaces for which values are queried. * \param int_interfaces full list of interfaces of a component. - * \param int_values internal values of a component. + * \param internal_values internal values of a component. * \return return return_type::INTERFACE_NOT_PROVIDED if * queried_interfaces list is is empty; return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and * queried_interfaces arguments do not have the same length; return_type::INTERFACE_NOT_FOUND if @@ -96,8 +82,8 @@ inline return_type get_internal_values( * (see: https://github.com/ros-controls/ros2_control/issues/129) */ inline return_type set_internal_values( - const std::vector & values, const std::vector & queried_interfaces, - const std::vector & int_interfaces, std::vector & int_values) + const std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, std::vector & internal_values) { if (queried_interfaces.size() == 0) { return return_type::INTERFACE_NOT_PROVIDED; @@ -106,10 +92,11 @@ inline return_type set_internal_values( return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; } + // TODO(andyz): use .name member for (auto q_it = queried_interfaces.begin(); q_it != queried_interfaces.end(); ++q_it) { auto it = std::find(int_interfaces.begin(), int_interfaces.end(), *q_it); if (it != int_interfaces.end()) { - int_values[std::distance(int_interfaces.begin(), it)] = + internal_values[std::distance(int_interfaces.begin(), it)] = values[std::distance(queried_interfaces.begin(), q_it)]; } else { return return_type::INTERFACE_NOT_FOUND; @@ -122,15 +109,15 @@ inline return_type set_internal_values( * \brief set all values to compoenents internal values. * * \param values values to set. - * \param int_values internal values of a component. + * \param internal_values internal values of a component. * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if the size of the arguments is not equal, * return_type::OK otherwise. */ inline return_type set_internal_values( - const std::vector & values, std::vector & int_values) + const std::vector & values, std::vector & internal_values) { - if (values.size() == int_values.size()) { - int_values = values; + if (values.size() == internal_values.size()) { + internal_values = values; } else { return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; } diff --git a/hardware_interface/src/components/joint.cpp b/hardware_interface/src/components/joint.cpp index 8aaa0c661d..9466a6a427 100644 --- a/hardware_interface/src/components/joint.cpp +++ b/hardware_interface/src/components/joint.cpp @@ -16,6 +16,7 @@ #include #include "hardware_interface/components/joint.hpp" + #include "hardware_interface/components/component_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -38,18 +39,18 @@ return_type Joint::configure(const ComponentInfo & joint_info) return return_type::OK; } -std::vector Joint::get_command_interfaces() const +std::vector Joint::get_command_interfaces() const { return info_.command_interfaces; } -std::vector Joint::get_state_interfaces() const +std::vector Joint::get_state_interfaces() const { return info_.state_interfaces; } return_type Joint::get_command( - std::vector & command, const std::vector & interfaces) const + std::vector & command, const std::vector & interfaces) const { return get_internal_values(command, interfaces, info_.command_interfaces, commands_); } @@ -60,7 +61,8 @@ return_type Joint::get_command(std::vector & command) const } return_type Joint::set_command( - const std::vector & command, const std::vector & interfaces) + const std::vector & command, + const std::vector & interfaces) { return set_internal_values(command, interfaces, info_.command_interfaces, commands_); } @@ -71,7 +73,7 @@ return_type Joint::set_command(const std::vector & command) } return_type Joint::get_state( - std::vector & state, const std::vector & interfaces) const + std::vector & state, const std::vector & interfaces) const { return get_internal_values(state, interfaces, info_.state_interfaces, states_); } @@ -82,7 +84,7 @@ return_type Joint::get_state(std::vector & state) const } return_type Joint::set_state( - const std::vector & state, const std::vector & interfaces) + const std::vector & state, const std::vector & interfaces) { return set_internal_values(state, interfaces, info_.state_interfaces, states_); } diff --git a/hardware_interface/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp index 397bee648d..8dbff5ff3b 100644 --- a/hardware_interface/src/components/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -16,6 +16,7 @@ #include #include "hardware_interface/components/sensor.hpp" + #include "hardware_interface/components/component_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -35,13 +36,13 @@ return_type Sensor::configure(const ComponentInfo & joint_info) return return_type::OK; } -std::vector Sensor::get_state_interfaces() const +std::vector Sensor::get_state_interfaces() { return info_.state_interfaces; } return_type Sensor::get_state( - std::vector & state, const std::vector & interfaces) const + std::vector & state, const std::vector & interfaces) const { return get_internal_values(state, interfaces, info_.state_interfaces, states_); } @@ -52,7 +53,7 @@ return_type Sensor::get_state(std::vector & state) const } return_type Sensor::set_state( - const std::vector & state, const std::vector & interfaces) + const std::vector & state, const std::vector & interfaces) { return set_internal_values(state, interfaces, info_.state_interfaces, states_); } diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 004b9de8a1..7b0c17076c 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -52,15 +52,22 @@ class DummyPositionJoint : public components::Joint if (info_.command_interfaces.size() > 1 || info_.state_interfaces.size() > 1) { return return_type::ERROR; } + + hardware_interface::components::InterfaceInfo dummy_position_interface; + dummy_position_interface.name = HW_IF_POSITION; + dummy_position_interface.parameters.insert(std::make_pair("min_value", -1)); + dummy_position_interface.parameters.insert(std::make_pair("max_value", 1)); + if (info_.command_interfaces.size() == 0) { - info_.command_interfaces.push_back(HW_IF_POSITION); + info_.command_interfaces.push_back(dummy_position_interface); commands_.resize(1); } if (info_.state_interfaces.size() == 0) { - info_.state_interfaces.push_back(HW_IF_POSITION); + info_.state_interfaces.push_back(dummy_position_interface); states_.resize(1); } + // TODO(andyz): Delete max_position_ = stod(info_.parameters["max_position"]); min_position_ = stod(info_.parameters["min_position"]); return return_type::OK; @@ -108,12 +115,25 @@ class DummyForceTorqueSensor : public components::Sensor return return_type::ERROR; } if (info_.state_interfaces.size() == 0) { - info_.state_interfaces.push_back("force_x"); - info_.state_interfaces.push_back("force_y"); - info_.state_interfaces.push_back("force_z"); - info_.state_interfaces.push_back("torque_x"); - info_.state_interfaces.push_back("torque_y"); - info_.state_interfaces.push_back("torque_z"); + hardware_interface::components::InterfaceInfo dummy_ft_interface; + + dummy_ft_interface.name = "force_x"; + info_.state_interfaces.push_back(dummy_ft_interface); + + dummy_ft_interface.name = "force_y"; + info_.state_interfaces.push_back(dummy_ft_interface); + + dummy_ft_interface.name = "force_z"; + info_.state_interfaces.push_back(dummy_ft_interface); + + dummy_ft_interface.name = "torque_x"; + info_.state_interfaces.push_back(dummy_ft_interface); + + dummy_ft_interface.name = "torque_y"; + info_.state_interfaces.push_back(dummy_ft_interface); + + dummy_ft_interface.name = "torque_z"; + info_.state_interfaces.push_back(dummy_ft_interface); } states_ = {1.34, 5.67, 8.21, 5.63, 5.99, 4.32}; return return_type::OK; @@ -160,13 +180,13 @@ class DummyActuatorHardware : public ActuatorHardwareInterface return_type read_joint(std::shared_ptr joint) const override { - std::vector interfaces = joint->get_state_interfaces(); + std::vector interfaces = joint->get_state_interfaces(); return joint->set_state(hw_values_, interfaces); } return_type write_joint(const std::shared_ptr joint) override { - std::vector interfaces = joint->get_command_interfaces(); + std::vector interfaces = joint->get_command_interfaces(); return joint->get_command(hw_values_, interfaces); } @@ -289,13 +309,13 @@ class DummySystemHardware : public SystemHardwareInterface return_type read_joints(std::vector> & joints) const override { return_type ret = return_type::OK; - std::vector interfaces; - std::vector joint_values; + std::vector interfaces; + std::vector jointernal_values; for (uint i = 0; i < joints.size(); i++) { - joint_values.clear(); - joint_values.push_back(joints_hw_values_[i]); + jointernal_values.clear(); + jointernal_values.push_back(joints_hw_values_[i]); interfaces = joints[i]->get_state_interfaces(); - ret = joints[i]->set_state(joint_values, interfaces); + ret = joints[i]->set_state(jointernal_values, interfaces); if (ret != return_type::OK) { break; } @@ -308,7 +328,7 @@ class DummySystemHardware : public SystemHardwareInterface return_type ret = return_type::OK; for (const auto & joint : joints) { std::vector values; - std::vector interfaces = joint->get_command_interfaces(); + std::vector interfaces = joint->get_command_interfaces(); ret = joint->get_command(values, interfaces); if (ret != return_type::OK) { break; @@ -371,21 +391,21 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) EXPECT_EQ(joint.configure(joint_info), return_type::OK); ASSERT_THAT(joint.get_command_interfaces(), SizeIs(1)); - EXPECT_EQ(joint.get_command_interfaces()[0], hardware_interface::HW_IF_POSITION); + EXPECT_EQ(joint.get_command_interfaces()[0].name, hardware_interface::HW_IF_POSITION); ASSERT_THAT(joint.get_state_interfaces(), SizeIs(1)); EXPECT_EQ(joint.get_state_interfaces()[0], hardware_interface::HW_IF_POSITION); // Command getters and setters - std::vector interfaces; + std::vector interfaces; std::vector input; input.push_back(2.1); EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); - interfaces.push_back(hardware_interface::HW_IF_POSITION); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); +// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); +// EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); +// interfaces.push_back(hardware_interface::HW_IF_POSITION); +// EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_POSITION); + interfaces.push_back(joint.get_command_interfaces()[0]); input.clear(); input.push_back(1.2); EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); @@ -393,13 +413,13 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) std::vector output; interfaces.clear(); EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_POSITION); + interfaces.push_back(joint.get_command_interfaces()[0]); EXPECT_EQ(joint.get_command(output, interfaces), return_type::OK); ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 1.2); interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); +// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); +// EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); input.clear(); EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); @@ -410,25 +430,25 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 2.1); - // State getters and setters - interfaces.clear(); - input.clear(); - input.push_back(2.1); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); - interfaces.push_back(hardware_interface::HW_IF_POSITION); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_POSITION); - input.clear(); - input.push_back(1.2); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); + // // State getters and setters + // interfaces.clear(); + // input.clear(); + // input.push_back(2.1); + // EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); + // interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + // EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); + // interfaces.push_back(hardware_interface::HW_IF_POSITION); + // EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + // interfaces.clear(); + // interfaces.push_back(hardware_interface::HW_IF_POSITION); + // input.clear(); + // input.push_back(1.2); + // EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); output.clear(); interfaces.clear(); EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_POSITION); + interfaces.push_back(joint.get_command_interfaces()[0]); EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 1.2); @@ -446,8 +466,11 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) EXPECT_EQ(output[0], 2.1); // Test DummyPositionJoint - joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); - joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + // Cannot push a velocity interface + joint_info.command_interfaces.push_back(joint.get_command_interfaces()[0]); + hardware_interface::components::InterfaceInfo velocity_interface; + velocity_interface.name = hardware_interface::HW_IF_VELOCITY; + joint_info.command_interfaces.push_back(velocity_interface); EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); } @@ -456,27 +479,25 @@ TEST_F(TestComponentInterfaces, multi_joint_example_component_works) DummyMultiJoint joint; joint_info.name = "DummyMultiJoint"; - joint_info.parameters["max_position"] = "3.14"; - joint_info.parameters["min_position"] = "-3.14"; - joint_info.parameters["max_velocity"] = "1.14"; - joint_info.parameters["min_velocity"] = "-1.14"; EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); - joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); - joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + joint_info.command_interfaces.push_back(joint.get_command_interfaces()[0]); + hardware_interface::components::InterfaceInfo velocity_interface; + velocity_interface.name = hardware_interface::HW_IF_VELOCITY; + joint_info.command_interfaces.push_back(velocity_interface); EXPECT_EQ(joint.configure(joint_info), return_type::OK); ASSERT_THAT(joint.get_command_interfaces(), SizeIs(2)); - EXPECT_EQ(joint.get_command_interfaces()[0], hardware_interface::HW_IF_POSITION); + EXPECT_EQ(joint.get_command_interfaces()[0].name, hardware_interface::HW_IF_POSITION); ASSERT_THAT(joint.get_state_interfaces(), SizeIs(0)); joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); joint_info.state_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); EXPECT_EQ(joint.configure(joint_info), return_type::OK); ASSERT_THAT(joint.get_state_interfaces(), SizeIs(2)); - EXPECT_EQ(joint.get_command_interfaces()[1], hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(joint.get_command_interfaces()[1].name, hardware_interface::HW_IF_VELOCITY); // Command getters and setters std::vector interfaces; @@ -688,25 +709,28 @@ TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) EXPECT_EQ(system.read_sensors(sensors), return_type::OK); std::vector output; - std::vector interfaces = sensor->get_state_interfaces(); - EXPECT_EQ(sensor->get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(6)); - EXPECT_EQ(output[2], -8.7); - ASSERT_THAT(interfaces, SizeIs(6)); - EXPECT_EQ(interfaces[4], "torque_y"); + { + std::vector interfaces = sensor->get_state_interfaces(); + EXPECT_EQ(sensor->get_state(output, interfaces), return_type::OK); + ASSERT_THAT(output, SizeIs(6)); + EXPECT_EQ(output[2], -8.7); + ASSERT_THAT(interfaces, SizeIs(6)); + EXPECT_EQ(interfaces[4], "torque_y"); + } output.clear(); - interfaces.clear(); EXPECT_EQ(system.read_joints(joints), return_type::OK); - interfaces = joint1->get_command_interfaces(); - EXPECT_EQ(joint1->get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], -1.575); - ASSERT_THAT(interfaces, SizeIs(1)); - EXPECT_EQ(interfaces[0], hardware_interface::HW_IF_POSITION); + { + std::vector interfaces = joint1->get_command_interfaces(); + EXPECT_EQ(joint1->get_state(output, interfaces), return_type::OK); + ASSERT_THAT(output, SizeIs(1)); + EXPECT_EQ(output[0], -1.575); + ASSERT_THAT(interfaces, SizeIs(1)); + EXPECT_EQ(interfaces[0].name, hardware_interface::HW_IF_POSITION); + } output.clear(); - interfaces.clear(); - interfaces = joint2->get_state_interfaces(); + + std::vector interfaces = joint2->get_state_interfaces(); EXPECT_EQ(joint2->get_state(output, interfaces), return_type::OK); ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], -0.7543); diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 35dac0768b..7603831c2b 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -172,7 +172,7 @@ class TestComponentParser : public Test )"; -// 2. Industrial Robots with multiple interfaces (can not be written at the same time) +// 2. Industrial Robots with multiple interfaces (cannot be written at the same time) valid_urdf_ros2_control_system_multi_interface_ = R"( @@ -183,27 +183,35 @@ class TestComponentParser : public Test ros2_control_components/MultiInterfaceJoint - position - velocity - effort + + -1 + 1 + + + + -1 + 1 + + + + -0.5 + 0.5 + + position velocity effort - -1 - 1 - -1 - 1 - -0.5 - 0.5 ros2_control_components/MultiInterfaceJoint - position + + -1 + 1 + + position velocity effort - -1 - 1 )"; @@ -311,9 +319,10 @@ class TestComponentParser : public Test ros2_control_components/VelocityJoint - velocity - -1 - 1 + + -1 + 1 + transmission_interface/SimpleTansmission @@ -328,9 +337,10 @@ class TestComponentParser : public Test ros2_control_components/VelocityJoint - velocity - -1 - 1 + + -1 + 1 + @@ -697,9 +707,9 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface EXPECT_EQ(hardware_info.joints[0].type, "joint"); EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/MultiInterfaceJoint"); ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(3)); - EXPECT_EQ(hardware_info.joints[0].command_interfaces[0], "position"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "position"); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(3)); - EXPECT_EQ(hardware_info.joints[0].state_interfaces[1], "velocity"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, "velocity"); ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(6)); EXPECT_EQ(hardware_info.joints[0].parameters.at("min_effort_value"), "-0.5"); @@ -708,7 +718,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface EXPECT_EQ(hardware_info.joints[1].class_type, "ros2_control_components/MultiInterfaceJoint"); ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3)); - EXPECT_EQ(hardware_info.joints[1].state_interfaces[2], "effort"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, "effort"); ASSERT_THAT(hardware_info.joints[1].parameters, SizeIs(2)); EXPECT_EQ(hardware_info.joints[1].parameters.at("min_position_value"), "-1"); } @@ -863,7 +873,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.joints[0].type, "joint"); EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/VelocityJoint"); ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); - EXPECT_EQ(hardware_info.joints[0].command_interfaces[0], "velocity"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "velocity"); ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); EXPECT_EQ(hardware_info.joints[0].parameters.at("max_velocity_value"), "1"); @@ -889,7 +899,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.joints[0].type, "joint"); EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/VelocityJoint"); ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); - EXPECT_EQ(hardware_info.joints[0].command_interfaces[0], "velocity"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "velocity"); ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); EXPECT_EQ(hardware_info.joints[0].parameters.at("min_velocity_value"), "-1"); @@ -910,7 +920,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/PositionJoint"); ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(0)); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); - EXPECT_EQ(hardware_info.joints[0].state_interfaces[0], "position"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, "position"); ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); EXPECT_EQ(hardware_info.joints[0].parameters.at("max_position_value"), "${PI}"); EXPECT_EQ(hardware_info.joints[0].parameters.at("min_position_value"), "${-PI}"); @@ -932,7 +942,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/PositionJoint"); ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(0)); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); - EXPECT_EQ(hardware_info.joints[0].state_interfaces[0], "position"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, "position"); ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); EXPECT_EQ(hardware_info.joints[0].parameters.at("max_position_value"), "${PI}"); EXPECT_EQ(hardware_info.joints[0].parameters.at("min_position_value"), "${-PI}"); @@ -998,8 +1008,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only) EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); EXPECT_EQ(hardware_info.sensors[0].class_type, "ros2_control_components/IMUSensor"); ASSERT_THAT(hardware_info.sensors[0].state_interfaces, SizeIs(2)); - EXPECT_EQ(hardware_info.sensors[0].state_interfaces[0], "velocity"); - EXPECT_EQ(hardware_info.sensors[0].state_interfaces[1], "acceleration"); + EXPECT_EQ(hardware_info.sensors[0].state_interfaces[0].name, "velocity"); + EXPECT_EQ(hardware_info.sensors[0].state_interfaces[1].name, "acceleration"); ASSERT_THAT(hardware_info.sensors[0].parameters, SizeIs(4)); EXPECT_EQ(hardware_info.sensors[0].parameters.at("min_acceleration_value"), "-10"); EXPECT_EQ(hardware_info.sensors[0].parameters.at("max_velocity_value"), "23"); @@ -1008,7 +1018,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only) EXPECT_EQ(hardware_info.sensors[1].type, "sensor"); EXPECT_EQ(hardware_info.sensors[1].class_type, "ros2_control_components/2DImageSensor"); ASSERT_THAT(hardware_info.sensors[1].state_interfaces, SizeIs(1)); - EXPECT_EQ(hardware_info.sensors[1].state_interfaces[0], "image"); + EXPECT_EQ(hardware_info.sensors[1].state_interfaces[0].name, "image"); ASSERT_THAT(hardware_info.sensors[1].parameters, SizeIs(2)); EXPECT_EQ(hardware_info.sensors[1].parameters.at("min_image_value"), "0"); EXPECT_EQ(hardware_info.sensors[1].parameters.at("max_image_value"), "255"); From b7cfbc228354a1b78d19e418782cd241a1da1300 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 27 Sep 2020 13:25:48 -0500 Subject: [PATCH 2/9] Add a name retrieval function. Minor renaming. --- .../components/component_info.hpp | 8 ++++++-- .../hardware_interface/components/joint.hpp | 10 +++++++++- .../test/test_component_interfaces.cpp | 8 ++------ .../test/test_component_parser.cpp | 16 ++++++++-------- 4 files changed, 25 insertions(+), 17 deletions(-) diff --git a/hardware_interface/include/hardware_interface/components/component_info.hpp b/hardware_interface/include/hardware_interface/components/component_info.hpp index a93be6fddb..487895ce14 100644 --- a/hardware_interface/include/hardware_interface/components/component_info.hpp +++ b/hardware_interface/include/hardware_interface/components/component_info.hpp @@ -36,9 +36,13 @@ struct InterfaceInfo */ std::string name; /** - * \brief (optional) key-value pairs of components parameters, e.g. min_value and max_value. + * \brief (optional) minimal allowed values of the interface. + */ + std::string min_values; + /** + * \brief (optional) maximal allowed values of the interface. */ - std::unordered_map parameters; + std::string max_values; }; /** diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp index 14dd8338bc..0e70026935 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -52,10 +52,18 @@ class Joint HARDWARE_INTERFACE_PUBLIC return_type configure(const ComponentInfo & joint_info); + // TODO(andyz): implement this /** * \brief Provide the list of command interfaces configured for the joint. * - * \return string list with command interfaces. + * \return vector of command interface names. + */ + std::vector get_command_interface_names() const; + + /** + * \brief Provide the list of command interfaces configured for the joint. + * + * \return vector of command interfaces. */ HARDWARE_INTERFACE_PUBLIC std::vector get_command_interfaces() const; diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 7b0c17076c..c5c2f58f0c 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -67,14 +67,10 @@ class DummyPositionJoint : public components::Joint states_.resize(1); } - // TODO(andyz): Delete - max_position_ = stod(info_.parameters["max_position"]); - min_position_ = stod(info_.parameters["min_position"]); + // TODO(andyz): Add min/max parameters to the interface + return return_type::OK; } - -private: - double max_position_, min_position_; }; class DummyMultiJoint : public components::Joint diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 7603831c2b..2a37498f11 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -184,18 +184,18 @@ class TestComponentParser : public Test ros2_control_components/MultiInterfaceJoint - -1 - 1 + -1 + 1 - -1 - 1 + -1 + 1 - -0.5 - 0.5 + -0.5 + 0.5 position @@ -205,8 +205,8 @@ class TestComponentParser : public Test ros2_control_components/MultiInterfaceJoint - -1 - 1 + -1 + 1 position From 09d46615e6818c76053382ef8d58d713ad6503bc Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 27 Sep 2020 13:47:33 -0500 Subject: [PATCH 3/9] Use vector args for API simplicity. --- .../components/component_info.hpp | 2 +- .../hardware_interface/components/joint.hpp | 29 ++++++++++++------- .../hardware_interface/components/sensor.hpp | 12 ++++---- hardware_interface/src/component_parser.cpp | 6 ++-- .../components/component_lists_management.hpp | 24 +++++++-------- hardware_interface/src/components/sensor.cpp | 6 ++-- hardware_interface/src/robot_hardware.cpp | 10 +++---- .../test/test_register_joints.cpp | 4 +-- 8 files changed, 50 insertions(+), 43 deletions(-) diff --git a/hardware_interface/include/hardware_interface/components/component_info.hpp b/hardware_interface/include/hardware_interface/components/component_info.hpp index 487895ce14..7989ec588a 100644 --- a/hardware_interface/include/hardware_interface/components/component_info.hpp +++ b/hardware_interface/include/hardware_interface/components/component_info.hpp @@ -74,7 +74,7 @@ struct ComponentInfo */ std::vector state_interfaces; /** - * \brief (optional) key-value pairs of components parameters, e.g. min_value and max_value. + * \brief (optional) key-value pairs of component parameters, e.g. min/max values or serial number. */ std::unordered_map parameters; }; diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp index 0e70026935..3546039839 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -55,19 +55,26 @@ class Joint // TODO(andyz): implement this /** * \brief Provide the list of command interfaces configured for the joint. - * * \return vector of command interface names. */ + HARDWARE_INTERFACE_PUBLIC std::vector get_command_interface_names() const; /** * \brief Provide the list of command interfaces configured for the joint. - * * \return vector of command interfaces. */ HARDWARE_INTERFACE_PUBLIC std::vector get_command_interfaces() const; + // TODO(andyz): implement this + /** + * \brief Provide the list of state interfaces configured for the joint. + * \return vector of state interface names. + */ + HARDWARE_INTERFACE_PUBLIC + std::vector get_state_interface_names() const; + /** * \brief Provide the list of state interfaces configured for the joint. * @@ -83,7 +90,7 @@ class Joint * provide. * * \param command list of doubles with commands for the hardware. - * \param interfaces list of interfaces on which commands have to set. + * \param interfaces list of interface names to retrieve from. * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if command and interfaces arguments do not * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces @@ -92,7 +99,7 @@ class Joint HARDWARE_INTERFACE_EXPORT return_type get_command( std::vector & command, - const std::vector & interfaces) const; + const std::vector & interfaces) const; /** * \brief Get complete command list for the joint. This function is used by the hardware to get @@ -111,7 +118,7 @@ class Joint * of elements. Using the interfaces list, the controller can choose which values to set. * * \param command list of doubles with commands for the hardware. - * \param interfaces list of interfaces on which commands have to be provided. + * \param interfaces list of interface names on which commands have to be provided. * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if command and interfaces arguments do not * have the same length; return_type::COMMAND_OUT_OF_LIMITS if one of the command values is out * of limits; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not @@ -124,7 +131,7 @@ class Joint HARDWARE_INTERFACE_EXPORT return_type set_command( const std::vector & command, - const std::vector & interfaces); + const std::vector & interfaces); /** * \brief Get complete state list from the joint. This function is used by the hardware to get @@ -139,13 +146,13 @@ class Joint HARDWARE_INTERFACE_EXPORT return_type set_command(const std::vector & command); - /* + /** * \brief Get state list from the joint. This function is used by the controller to get the * actual state of the hardware. The parameters state, and interfaces have the same order and * number of elements. Using the interfaces list, the controller can choose which values to get. * * \param state list of doubles with states of the hardware. - * \param interfaces list of interfaces on which states have to be provided. + * \param interfaces list of interface names to provide for. * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces @@ -154,7 +161,7 @@ class Joint HARDWARE_INTERFACE_EXPORT return_type get_state( std::vector & state, - const std::vector & interfaces) const; + const std::vector & interfaces) const; /** * \brief Get complete state list from the joint. This function is used by the controller to get @@ -173,7 +180,7 @@ class Joint * the interfaces list, the hardware can choose which values to set. * * \param state list of doubles with states of the hardware. - * \param interfaces list of interfaces on which states have to be provided. + * \param interfaces list of interface names to provide for. * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not * defined for the joint; return_type::OK otherwise. @@ -181,7 +188,7 @@ class Joint HARDWARE_INTERFACE_EXPORT return_type set_state( const std::vector & state, - const std::vector & interfaces); + const std::vector & interfaces); /** * \brief Set complete state list from the joint.This function is used by the hardware to set its diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp index a0cdf5f55f..0ec0d42aae 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -55,10 +55,10 @@ class Sensor /** * \brief Provide the list of state interfaces configured for the sensor. * - * \return string list with state interfaces. + * \return list of state interface names. */ HARDWARE_INTERFACE_PUBLIC - std::vector get_state_interfaces(); + std::vector get_state_interfaces(); /** * \brief Get state list from the sensor. This function is used by the controller to get the @@ -66,7 +66,7 @@ class Sensor * number of elements. Using the interfaces list, the controller can choose which values to get. * * \param state list of doubles with states of the hardware. - * \param interfaces list of interfaces on which states have to be provided. + * \param interfaces list of interface names to provide for. * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not * defined for the sensor; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces @@ -75,7 +75,7 @@ class Sensor HARDWARE_INTERFACE_EXPORT return_type get_state( std::vector & state, - const std::vector & interfaces) const; + const std::vector & interfaces) const; /** * \brief Get complete state list from the sensor. This function is used by the controller to get @@ -94,7 +94,7 @@ class Sensor * the interfaces list, the hardware can choose which values to set. * * \param state list of doubles with states of the hardware. - * \param interfaces list of interfaces on which states have to be provided. + * \param interfaces list of interface names to provide for. * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not * defined for the sensor; return_type::OK otherwise. @@ -102,7 +102,7 @@ class Sensor HARDWARE_INTERFACE_EXPORT return_type set_state( const std::vector & state, - const std::vector & interfaces); + const std::vector & interfaces); /** * \brief Set complete state list from the sensor.This function is used by the hardware to set its diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index a3225bec3a..ed39159dec 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -142,14 +142,14 @@ std::vector parse_interfaces_from_xml( { std::vector interfaces; - // TODO(andyz): parse optional attributes like min_value/max_value + // TODO(andyz): parse optional min/max attributes while (interfaces_it) { const std::string interface_type = get_text_for_element( interfaces_it, std::string(interfaceTag) + " type "); // TODO(andyz): select the proper type (switch statement?) // For now, just assume a position type - hardware_interface::components::InterfaceInfo position_interface; - position_interface.name = "position"; + hardware_interface::components::InterfaceInfo interface; + interface.name = "position"; interfaces.push_back(position_interface); interfaces_it = interfaces_it->NextSiblingElement(interfaceTag); } diff --git a/hardware_interface/src/components/component_lists_management.hpp b/hardware_interface/src/components/component_lists_management.hpp index 784f88078e..7c37c1fa66 100644 --- a/hardware_interface/src/components/component_lists_management.hpp +++ b/hardware_interface/src/components/component_lists_management.hpp @@ -27,21 +27,21 @@ namespace hardware_interface namespace components { /** - * \brief Get values for queried_interfaces from the int_values. int_values data structure matches - * int_interfaces vector. + * \brief Get values for queried_interfaces from the internal_values. internal_values data structure matches + * internal_interfaces vector. * * \param values values to return. * \param queried_interfaces interfaces for which values are queried. - * \param int_interfaces full list of interfaces of a component. + * \param internal_interfaces full list of interfaces of a component. * \param int_values internal values of a component. * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and queried_interfaces arguments * do not have the same length; return_type::INTERFACE_NOT_FOUND if one of queried_interfaces is - * not defined in int_interfaces; return return_type::INTERFACE_NOT_PROVIDED if queried_interfaces + * not defined in internal_interfaces; return return_type::INTERFACE_NOT_PROVIDED if queried_interfaces * list is is empty; return_type::OK otherwise. */ inline return_type get_internal_values( std::vector & values, const std::vector & queried_interfaces, - const std::vector & int_interfaces, const std::vector & internal_values) + const std::vector & internal_interfaces, const std::vector & internal_values) { // TODO(andyz) return return_type::OK; @@ -66,16 +66,16 @@ inline return_type get_internal_values( /** * \brief set values for queried_interfaces to the internal_values. internal_values data structure matches - * int_interfaces vector. + * internal_interfaces vector. * * \param values values to set. * \param queried_interfaces interfaces for which values are queried. - * \param int_interfaces full list of interfaces of a component. + * \param internal_interfaces full list of interfaces of a component. * \param internal_values internal values of a component. * \return return return_type::INTERFACE_NOT_PROVIDED if * queried_interfaces list is is empty; return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and * queried_interfaces arguments do not have the same length; return_type::INTERFACE_NOT_FOUND if - * one of queried_interfaces is not defined in int_interfaces; return_type::OK otherwise. + * one of queried_interfaces is not defined in internal_interfaces; return_type::OK otherwise. * * \todo The error handling in this function could lead to incosistant command or state variables * for different interfaces. This should be changed in the future. @@ -83,7 +83,7 @@ inline return_type get_internal_values( */ inline return_type set_internal_values( const std::vector & values, const std::vector & queried_interfaces, - const std::vector & int_interfaces, std::vector & internal_values) + const std::vector & internal_interfaces, std::vector & internal_values) { if (queried_interfaces.size() == 0) { return return_type::INTERFACE_NOT_PROVIDED; @@ -94,9 +94,9 @@ inline return_type set_internal_values( // TODO(andyz): use .name member for (auto q_it = queried_interfaces.begin(); q_it != queried_interfaces.end(); ++q_it) { - auto it = std::find(int_interfaces.begin(), int_interfaces.end(), *q_it); - if (it != int_interfaces.end()) { - internal_values[std::distance(int_interfaces.begin(), it)] = + auto it = std::find(internal_interfaces.begin(), internal_interfaces.end(), *q_it); + if (it != internal_interfaces.end()) { + internal_values[std::distance(internal_interfaces.begin(), it)] = values[std::distance(queried_interfaces.begin(), q_it)]; } else { return return_type::INTERFACE_NOT_FOUND; diff --git a/hardware_interface/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp index 8dbff5ff3b..0229e8b14f 100644 --- a/hardware_interface/src/components/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -36,13 +36,13 @@ return_type Sensor::configure(const ComponentInfo & joint_info) return return_type::OK; } -std::vector Sensor::get_state_interfaces() +std::vector Sensor::get_state_interfaces() { return info_.state_interfaces; } return_type Sensor::get_state( - std::vector & state, const std::vector & interfaces) const + std::vector & state, const std::vector & interfaces) const { return get_internal_values(state, interfaces, info_.state_interfaces, states_); } @@ -53,7 +53,7 @@ return_type Sensor::get_state(std::vector & state) const } return_type Sensor::set_state( - const std::vector & state, const std::vector & interfaces) + const std::vector & state, const std::vector & interfaces) { return set_internal_values(state, interfaces, info_.state_interfaces, states_); } diff --git a/hardware_interface/src/robot_hardware.cpp b/hardware_interface/src/robot_hardware.cpp index e23dd6e3d4..1fde6be766 100644 --- a/hardware_interface/src/robot_hardware.cpp +++ b/hardware_interface/src/robot_hardware.cpp @@ -349,13 +349,13 @@ std::vector get_registered_handles(control_msgs::msg::DynamicJointSt assert(registered.joint_names.size() == registered.interface_values.size()); for (auto i = 0u; i < handle_names.size(); ++i) { - auto & joint_interfaces = interface_values[i]; - assert(joint_interfaces.interface_names.size() == joint_interfaces.values.size()); + auto & jointernal_interfaces = interface_values[i]; + assert(jointernal_interfaces.interface_names.size() == jointernal_interfaces.values.size()); - for (auto j = 0u; j < joint_interfaces.interface_names.size(); ++j) { + for (auto j = 0u; j < jointernal_interfaces.interface_names.size(); ++j) { result.emplace_back( - handle_names[i], joint_interfaces.interface_names[j], - &joint_interfaces.values[j]); + handle_names[i], jointernal_interfaces.interface_names[j], + &jointernal_interfaces.values[j]); } } diff --git a/hardware_interface/test/test_register_joints.cpp b/hardware_interface/test/test_register_joints.cpp index 35fb3a5eec..dff8326579 100644 --- a/hardware_interface/test/test_register_joints.cpp +++ b/hardware_interface/test/test_register_joints.cpp @@ -71,13 +71,13 @@ TEST_F(TestJoints, no_jointss_registered_return_empty_on_all_fronts) EXPECT_EQ(hw::return_type::ERROR, robot_hw_.get_joint_handle(handle)); } -TEST_F(TestJoints, can_register_joint_interfaces) +TEST_F(TestJoints, can_register_jointernal_interfaces) { EXPECT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, FOO_INTERFACE)); EXPECT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, BAR_INTERFACE)); } -TEST_F(TestJoints, can_not_double_register_joint_interfaces) +TEST_F(TestJoints, can_not_double_register_jointernal_interfaces) { EXPECT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, FOO_INTERFACE)); EXPECT_EQ(hw::return_type::OK, robot_hw_.register_joint(JOINT_NAME, BAR_INTERFACE)); From e4534c99a45b33bc06d6535e64621be82c34f836 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 28 Sep 2020 20:52:41 -0500 Subject: [PATCH 4/9] Fix several build errors. Parsing tests pass! --- .../components/component_info.hpp | 4 +- .../hardware_interface/components/joint.hpp | 4 +- .../hardware_interface/components/sensor.hpp | 12 ++- hardware_interface/src/component_parser.cpp | 25 ++++-- .../components/component_lists_management.hpp | 31 +++++-- hardware_interface/src/components/joint.cpp | 32 +++++-- hardware_interface/src/components/sensor.cpp | 14 ++- .../test/test_component_interfaces.cpp | 86 +++++++++---------- .../test/test_component_parser.cpp | 44 ++++++---- 9 files changed, 156 insertions(+), 96 deletions(-) diff --git a/hardware_interface/include/hardware_interface/components/component_info.hpp b/hardware_interface/include/hardware_interface/components/component_info.hpp index 7989ec588a..5e8e30689e 100644 --- a/hardware_interface/include/hardware_interface/components/component_info.hpp +++ b/hardware_interface/include/hardware_interface/components/component_info.hpp @@ -38,11 +38,11 @@ struct InterfaceInfo /** * \brief (optional) minimal allowed values of the interface. */ - std::string min_values; + std::string min; /** * \brief (optional) maximal allowed values of the interface. */ - std::string max_values; + std::string max; }; /** diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp index 3546039839..428da42591 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -52,7 +52,6 @@ class Joint HARDWARE_INTERFACE_PUBLIC return_type configure(const ComponentInfo & joint_info); - // TODO(andyz): implement this /** * \brief Provide the list of command interfaces configured for the joint. * \return vector of command interface names. @@ -67,7 +66,6 @@ class Joint HARDWARE_INTERFACE_PUBLIC std::vector get_command_interfaces() const; - // TODO(andyz): implement this /** * \brief Provide the list of state interfaces configured for the joint. * \return vector of state interface names. @@ -131,7 +129,7 @@ class Joint HARDWARE_INTERFACE_EXPORT return_type set_command( const std::vector & command, - const std::vector & interfaces); + const std::vector & interfaces); /** * \brief Get complete state list from the joint. This function is used by the hardware to get diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp index 0ec0d42aae..ddc3c2e85f 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -52,13 +52,21 @@ class Sensor HARDWARE_INTERFACE_PUBLIC return_type configure(const ComponentInfo & joint_info); + // TODO(andyz): implement this + /** + * \brief Provide the list of state interfaces configured for the joint. + * \return vector of state interface names. + */ + HARDWARE_INTERFACE_PUBLIC + std::vector get_state_interface_names() const; + /** * \brief Provide the list of state interfaces configured for the sensor. * - * \return list of state interface names. + * \return list of state interfaces. */ HARDWARE_INTERFACE_PUBLIC - std::vector get_state_interfaces(); + std::vector get_state_interfaces() const; /** * \brief Get state list from the sensor. This function is used by the controller to get the diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index ed39159dec..30f86f459e 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -142,17 +142,27 @@ std::vector parse_interfaces_from_xml( { std::vector interfaces; + // TODO(andyz): parse optional min/max attributes + while (interfaces_it) { - const std::string interface_type = get_text_for_element( - interfaces_it, std::string(interfaceTag) + " type "); - // TODO(andyz): select the proper type (switch statement?) - // For now, just assume a position type hardware_interface::components::InterfaceInfo interface; - interface.name = "position"; - interfaces.push_back(position_interface); + + // Joint interfaces have a name attribute + if (std::string(interfaceTag) == "commandInterfaceType") { + const std::string interface_name = get_attribute_value(interfaces_it, "name", std::string(interfaceTag)); + interface.name = interface_name; + } + // State interfaces have an element to define the type, not a name attribute + if (std::string(interfaceTag) == "stateInterfaceType") { + const std::string interface_type = get_text_for_element(interfaces_it, std::string(interfaceTag) + " type "); + interface.name = interface_type; + } + + interfaces.push_back(interface); interfaces_it = interfaces_it->NextSiblingElement(interfaceTag); } + return interfaces; } @@ -178,13 +188,12 @@ components::ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component.class_type = get_text_for_element(classType_it, component.name + " " + kClassTypeTag); // Parse commandInterfaceType tags -/* const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTypeTag); if (command_interfaces_it) { component.command_interfaces = parse_interfaces_from_xml( command_interfaces_it, kCommandInterfaceTypeTag); } -*/ + // Parse stateInterfaceType tags const auto * state_interfaces_it = component_it->FirstChildElement(kStateInterfaceTypeTag); if (state_interfaces_it) { diff --git a/hardware_interface/src/components/component_lists_management.hpp b/hardware_interface/src/components/component_lists_management.hpp index 7c37c1fa66..6ed7025993 100644 --- a/hardware_interface/src/components/component_lists_management.hpp +++ b/hardware_interface/src/components/component_lists_management.hpp @@ -31,19 +31,32 @@ namespace components * internal_interfaces vector. * * \param values values to return. - * \param queried_interfaces interfaces for which values are queried. - * \param internal_interfaces full list of interfaces of a component. - * \param int_values internal values of a component. + * \param queried_interfaces interface names for which values are queried. + * \param internal_interfaces full list of command OR state interfaces of a component. + * \param internal_values internal values of a component. * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and queried_interfaces arguments * do not have the same length; return_type::INTERFACE_NOT_FOUND if one of queried_interfaces is * not defined in internal_interfaces; return return_type::INTERFACE_NOT_PROVIDED if queried_interfaces * list is is empty; return_type::OK otherwise. */ inline return_type get_internal_values( - std::vector & values, const std::vector & queried_interfaces, - const std::vector & internal_interfaces, const std::vector & internal_values) + std::vector & values, const std::vector & queried_interfaces, + const std::vector & internal_interfaces, const std::vector & internal_values) { - // TODO(andyz) + if (queried_interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + + for (const auto & interface : queried_interfaces) { + auto it = std::find( + internal_interfaces.begin(), internal_interfaces.end(), interface); + if (it != internal_interfaces.end()) { + values.push_back(internal_values[std::distance(internal_interfaces.begin(), it)]); + } else { + values.clear(); + return return_type::INTERFACE_NOT_FOUND; + } + } return return_type::OK; } @@ -82,8 +95,8 @@ inline return_type get_internal_values( * (see: https://github.com/ros-controls/ros2_control/issues/129) */ inline return_type set_internal_values( - const std::vector & values, const std::vector & queried_interfaces, - const std::vector & internal_interfaces, std::vector & internal_values) + const std::vector & values, const std::vector & queried_interfaces, + const std::vector & internal_interfaces, std::vector & internal_values) { if (queried_interfaces.size() == 0) { return return_type::INTERFACE_NOT_PROVIDED; @@ -92,7 +105,6 @@ inline return_type set_internal_values( return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; } - // TODO(andyz): use .name member for (auto q_it = queried_interfaces.begin(); q_it != queried_interfaces.end(); ++q_it) { auto it = std::find(internal_interfaces.begin(), internal_interfaces.end(), *q_it); if (it != internal_interfaces.end()) { @@ -102,6 +114,7 @@ inline return_type set_internal_values( return return_type::INTERFACE_NOT_FOUND; } } + return return_type::OK; } diff --git a/hardware_interface/src/components/joint.cpp b/hardware_interface/src/components/joint.cpp index 9466a6a427..eb4a94df90 100644 --- a/hardware_interface/src/components/joint.cpp +++ b/hardware_interface/src/components/joint.cpp @@ -39,6 +39,14 @@ return_type Joint::configure(const ComponentInfo & joint_info) return return_type::OK; } +std::vector Joint::get_command_interface_names() const +{ + std::vector command_interface_names; + for (auto interface : info_.command_interfaces) + command_interface_names.push_back(interface.name); + return command_interface_names; +} + std::vector Joint::get_command_interfaces() const { return info_.command_interfaces; @@ -50,9 +58,9 @@ std::vector Joint::get_state_interfaces() const } return_type Joint::get_command( - std::vector & command, const std::vector & interfaces) const + std::vector & command, const std::vector & interfaces) const { - return get_internal_values(command, interfaces, info_.command_interfaces, commands_); + return get_internal_values(command, interfaces, get_command_interface_names(), commands_); } return_type Joint::get_command(std::vector & command) const @@ -62,9 +70,9 @@ return_type Joint::get_command(std::vector & command) const return_type Joint::set_command( const std::vector & command, - const std::vector & interfaces) + const std::vector & interfaces) { - return set_internal_values(command, interfaces, info_.command_interfaces, commands_); + return set_internal_values(command, interfaces, get_command_interface_names(), commands_); } return_type Joint::set_command(const std::vector & command) @@ -72,10 +80,18 @@ return_type Joint::set_command(const std::vector & command) return set_internal_values(command, commands_); } +std::vector Joint::get_state_interface_names() const +{ + std::vector state_interface_names; + for (auto interface : info_.state_interfaces) + state_interface_names.push_back(interface.name); + return state_interface_names; +} + return_type Joint::get_state( - std::vector & state, const std::vector & interfaces) const + std::vector & state, const std::vector & interfaces) const { - return get_internal_values(state, interfaces, info_.state_interfaces, states_); + return get_internal_values(state, interfaces, get_state_interface_names(), states_); } return_type Joint::get_state(std::vector & state) const @@ -84,9 +100,9 @@ return_type Joint::get_state(std::vector & state) const } return_type Joint::set_state( - const std::vector & state, const std::vector & interfaces) + const std::vector & state, const std::vector & interfaces) { - return set_internal_values(state, interfaces, info_.state_interfaces, states_); + return set_internal_values(state, interfaces, get_state_interface_names(), states_); } return_type Joint::set_state(const std::vector & state) diff --git a/hardware_interface/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp index 0229e8b14f..0c5046b462 100644 --- a/hardware_interface/src/components/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -36,7 +36,15 @@ return_type Sensor::configure(const ComponentInfo & joint_info) return return_type::OK; } -std::vector Sensor::get_state_interfaces() +std::vector Sensor::get_state_interface_names() const +{ + std::vector state_interface_names; + for (auto interface : info_.state_interfaces) + state_interface_names.push_back(interface.name); + return state_interface_names; +} + +std::vector Sensor::get_state_interfaces() const { return info_.state_interfaces; } @@ -44,7 +52,7 @@ std::vector Sensor::get_state_interfaces() return_type Sensor::get_state( std::vector & state, const std::vector & interfaces) const { - return get_internal_values(state, interfaces, info_.state_interfaces, states_); + return get_internal_values(state, interfaces, get_state_interface_names(), states_); } return_type Sensor::get_state(std::vector & state) const @@ -55,7 +63,7 @@ return_type Sensor::get_state(std::vector & state) const return_type Sensor::set_state( const std::vector & state, const std::vector & interfaces) { - return set_internal_values(state, interfaces, info_.state_interfaces, states_); + return set_internal_values(state, interfaces, get_state_interface_names(), states_); } return_type Sensor::set_state(const std::vector & state) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index c5c2f58f0c..86b53409dc 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -55,8 +55,8 @@ class DummyPositionJoint : public components::Joint hardware_interface::components::InterfaceInfo dummy_position_interface; dummy_position_interface.name = HW_IF_POSITION; - dummy_position_interface.parameters.insert(std::make_pair("min_value", -1)); - dummy_position_interface.parameters.insert(std::make_pair("max_value", 1)); + dummy_position_interface.max = "1"; + dummy_position_interface.min = "-1"; if (info_.command_interfaces.size() == 0) { info_.command_interfaces.push_back(dummy_position_interface); @@ -176,13 +176,13 @@ class DummyActuatorHardware : public ActuatorHardwareInterface return_type read_joint(std::shared_ptr joint) const override { - std::vector interfaces = joint->get_state_interfaces(); + std::vector interfaces = joint->get_state_interface_names(); return joint->set_state(hw_values_, interfaces); } return_type write_joint(const std::shared_ptr joint) override { - std::vector interfaces = joint->get_command_interfaces(); + std::vector interfaces = joint->get_command_interface_names(); return joint->get_command(hw_values_, interfaces); } @@ -305,12 +305,12 @@ class DummySystemHardware : public SystemHardwareInterface return_type read_joints(std::vector> & joints) const override { return_type ret = return_type::OK; - std::vector interfaces; + std::vector interfaces; std::vector jointernal_values; for (uint i = 0; i < joints.size(); i++) { jointernal_values.clear(); jointernal_values.push_back(joints_hw_values_[i]); - interfaces = joints[i]->get_state_interfaces(); + interfaces = joints[i]->get_state_interface_names(); ret = joints[i]->set_state(jointernal_values, interfaces); if (ret != return_type::OK) { break; @@ -324,7 +324,7 @@ class DummySystemHardware : public SystemHardwareInterface return_type ret = return_type::OK; for (const auto & joint : joints) { std::vector values; - std::vector interfaces = joint->get_command_interfaces(); + std::vector interfaces = joint->get_command_interface_names(); ret = joint->get_command(values, interfaces); if (ret != return_type::OK) { break; @@ -389,19 +389,19 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) ASSERT_THAT(joint.get_command_interfaces(), SizeIs(1)); EXPECT_EQ(joint.get_command_interfaces()[0].name, hardware_interface::HW_IF_POSITION); ASSERT_THAT(joint.get_state_interfaces(), SizeIs(1)); - EXPECT_EQ(joint.get_state_interfaces()[0], hardware_interface::HW_IF_POSITION); + EXPECT_EQ(joint.get_state_interface_names()[0], hardware_interface::HW_IF_POSITION); // Command getters and setters - std::vector interfaces; + std::vector interfaces; std::vector input; input.push_back(2.1); EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); -// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); -// EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); -// interfaces.push_back(hardware_interface::HW_IF_POSITION); -// EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); interfaces.clear(); - interfaces.push_back(joint.get_command_interfaces()[0]); + interfaces.push_back(joint.get_command_interface_names()[0]); input.clear(); input.push_back(1.2); EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); @@ -409,13 +409,13 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) std::vector output; interfaces.clear(); EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(joint.get_command_interfaces()[0]); + interfaces.push_back(joint.get_command_interface_names()[0]); EXPECT_EQ(joint.get_command(output, interfaces), return_type::OK); ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 1.2); interfaces.clear(); -// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); -// EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); input.clear(); EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); @@ -426,25 +426,25 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 2.1); - // // State getters and setters - // interfaces.clear(); - // input.clear(); - // input.push_back(2.1); - // EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - // interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - // EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); - // interfaces.push_back(hardware_interface::HW_IF_POSITION); - // EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - // interfaces.clear(); - // interfaces.push_back(hardware_interface::HW_IF_POSITION); - // input.clear(); - // input.push_back(1.2); - // EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); + // State getters and setters + interfaces.clear(); + input.clear(); + input.push_back(2.1); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + input.clear(); + input.push_back(1.2); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); output.clear(); interfaces.clear(); EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(joint.get_command_interfaces()[0]); + interfaces.push_back(joint.get_command_interface_names()[0]); EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 1.2); @@ -489,8 +489,8 @@ TEST_F(TestComponentInterfaces, multi_joint_example_component_works) EXPECT_EQ(joint.get_command_interfaces()[0].name, hardware_interface::HW_IF_POSITION); ASSERT_THAT(joint.get_state_interfaces(), SizeIs(0)); - joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); - joint_info.state_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + joint_info.state_interfaces.push_back(velocity_interface); + joint_info.state_interfaces.push_back(velocity_interface); EXPECT_EQ(joint.configure(joint_info), return_type::OK); ASSERT_THAT(joint.get_state_interfaces(), SizeIs(2)); EXPECT_EQ(joint.get_command_interfaces()[1].name, hardware_interface::HW_IF_VELOCITY); @@ -571,8 +571,8 @@ TEST_F(TestComponentInterfaces, sensor_example_component_works) EXPECT_EQ(sensor.configure(sensor_info), return_type::OK); ASSERT_THAT(sensor.get_state_interfaces(), SizeIs(6)); - EXPECT_EQ(sensor.get_state_interfaces()[0], "force_x"); - EXPECT_EQ(sensor.get_state_interfaces()[5], "torque_z"); + EXPECT_EQ(sensor.get_state_interface_names()[0], "force_x"); + EXPECT_EQ(sensor.get_state_interface_names()[5], "torque_z"); std::vector input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; std::vector output; std::vector interfaces; @@ -594,7 +594,7 @@ TEST_F(TestComponentInterfaces, sensor_example_component_works) interfaces.push_back(hardware_interface::HW_IF_VELOCITY); EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); interfaces.clear(); - interfaces = sensor.get_state_interfaces(); + interfaces = sensor.get_state_interface_names(); EXPECT_EQ(sensor.set_state(input, interfaces), return_type::OK); output.clear(); @@ -635,7 +635,7 @@ TEST_F(TestComponentInterfaces, actuator_hardware_interface_works) EXPECT_EQ(actuator_hw.start(), return_type::OK); EXPECT_EQ(actuator_hw.get_status(), hardware_interface_status::STARTED); EXPECT_EQ(actuator_hw.read_joint(joint), return_type::OK); - std::vector interfaces = joint->get_state_interfaces(); + std::vector interfaces = joint->get_state_interface_names(); std::vector output; EXPECT_EQ(joint->get_state(output, interfaces), return_type::OK); ASSERT_THAT(output, SizeIs(1)); @@ -665,7 +665,7 @@ TEST_F(TestComponentInterfaces, sensor_interface_with_hardware_works) sensors.push_back(sensor); EXPECT_EQ(sensor_hw.read_sensors(sensors), return_type::OK); std::vector output; - std::vector interfaces = sensor->get_state_interfaces(); + std::vector interfaces = sensor->get_state_interface_names(); EXPECT_EQ(sensor->get_state(output, interfaces), return_type::OK); EXPECT_EQ(output[2], 3.4); ASSERT_THAT(interfaces, SizeIs(6)); @@ -706,7 +706,7 @@ TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) EXPECT_EQ(system.read_sensors(sensors), return_type::OK); std::vector output; { - std::vector interfaces = sensor->get_state_interfaces(); + std::vector interfaces = sensor->get_state_interface_names(); EXPECT_EQ(sensor->get_state(output, interfaces), return_type::OK); ASSERT_THAT(output, SizeIs(6)); EXPECT_EQ(output[2], -8.7); @@ -717,16 +717,16 @@ TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) EXPECT_EQ(system.read_joints(joints), return_type::OK); { - std::vector interfaces = joint1->get_command_interfaces(); + std::vector interfaces = joint1->get_command_interface_names(); EXPECT_EQ(joint1->get_state(output, interfaces), return_type::OK); ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], -1.575); ASSERT_THAT(interfaces, SizeIs(1)); - EXPECT_EQ(interfaces[0].name, hardware_interface::HW_IF_POSITION); + EXPECT_EQ(interfaces[0], hardware_interface::HW_IF_POSITION); } output.clear(); - std::vector interfaces = joint2->get_state_interfaces(); + std::vector interfaces = joint2->get_state_interface_names(); EXPECT_EQ(joint2->get_state(output, interfaces), return_type::OK); ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], -0.7543); diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 2a37498f11..80786efe2d 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -173,6 +173,7 @@ class TestComponentParser : public Test )"; // 2. Industrial Robots with multiple interfaces (cannot be written at the same time) +// Note, joint parameters can be any string valid_urdf_ros2_control_system_multi_interface_ = R"( @@ -201,6 +202,8 @@ class TestComponentParser : public Test position velocity effort + + A42B1 ros2_control_components/MultiInterfaceJoint @@ -212,6 +215,8 @@ class TestComponentParser : public Test position velocity effort + + A42B2 )"; @@ -313,16 +318,18 @@ class TestComponentParser : public Test R"( - ros2_control_demo_hardware/Velocity_Actuator_Hadware + ros2_control_demo_hardware/Velocity_Actuator_Hardware 1.23 3 ros2_control_components/VelocityJoint - -1 - 1 + -1 + 1 + + D transmission_interface/SimpleTansmission @@ -331,15 +338,15 @@ class TestComponentParser : public Test - ros2_control_demo_hardware/Velocity_Actuator_Hadware + ros2_control_demo_hardware/Velocity_Actuator_Hardware 1.23 3 ros2_control_components/VelocityJoint - -1 - 1 + -1 + 1 @@ -432,7 +439,7 @@ class TestComponentParser : public Test R"( - ros2_control_demo_hardware/Velocity_Actuator_Hadware + ros2_control_demo_hardware/Velocity_Actuator_Hardware 1.13 3 @@ -710,8 +717,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "position"); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(3)); EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, "velocity"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(6)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("min_effort_value"), "-0.5"); + ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[0].parameters.at("serial_number"), "A42B1"); EXPECT_EQ(hardware_info.joints[1].name, "joint2"); EXPECT_EQ(hardware_info.joints[1].type, "joint"); @@ -719,8 +726,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3)); EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, "effort"); - ASSERT_THAT(hardware_info.joints[1].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[1].parameters.at("min_position_value"), "-1"); + ASSERT_THAT(hardware_info.joints[1].parameters, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[1].parameters.at("serial_number"), "A42B2"); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sensor) @@ -864,7 +871,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/Velocity_Actuator_Hadware"); + "ros2_control_demo_hardware/Velocity_Actuator_Hardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23"); @@ -874,8 +881,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/VelocityJoint"); ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "velocity"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("max_velocity_value"), "1"); + ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[0].parameters.at("example_param_current_rating"), "D"); ASSERT_THAT(hardware_info.transmissions, SizeIs(1)); EXPECT_EQ(hardware_info.transmissions[0].name, "transmission1"); @@ -890,7 +897,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/Velocity_Actuator_Hadware"); + "ros2_control_demo_hardware/Velocity_Actuator_Hardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "3"); @@ -900,8 +907,9 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/VelocityJoint"); ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "velocity"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("min_velocity_value"), "-1"); + // TODO(andyz): parse min/max value for this joint +// ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); +// EXPECT_EQ(hardware_info.joints[0].parameters.at("min_velocity_value"), "-1"); hardware_info = control_hardware.at(2); @@ -1036,7 +1044,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/Velocity_Actuator_Hadware"); + "ros2_control_demo_hardware/Velocity_Actuator_Hardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.13"); From 8f366be56713a99d7ce5e6f171906ec966253e76 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sat, 3 Oct 2020 22:13:12 -0500 Subject: [PATCH 5/9] Fix member variable naming in test_component_interfaces --- .../test/test_component_interfaces.cpp | 61 ++++++++++--------- 1 file changed, 31 insertions(+), 30 deletions(-) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 86b53409dc..365afd2e17 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -367,17 +367,17 @@ using hardware_interface::hardware_interfaces_components_test::DummySystemHardwa class TestComponentInterfaces : public Test { protected: - ComponentInfo joint_info; - ComponentInfo sensor_info; + ComponentInfo joint_info_; + ComponentInfo sensor_info_; void SetUp() override { - joint_info.name = "DummyPositionJoint"; - joint_info.parameters["max_position"] = "3.14"; - joint_info.parameters["min_position"] = "-3.14"; + joint_info_.name = "DummyPositionJoint"; + joint_info_.parameters["max_position"] = "3.14"; + joint_info_.parameters["min_position"] = "-3.14"; - sensor_info.name = "DummyForceTorqueSensor"; - sensor_info.parameters["frame_id"] = "tcp_link"; + sensor_info_.name = "DummyForceTorqueSensor"; + sensor_info_.parameters["frame_id"] = "tcp_link"; } }; @@ -385,7 +385,7 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) { DummyPositionJoint joint; - EXPECT_EQ(joint.configure(joint_info), return_type::OK); + EXPECT_EQ(joint.configure(joint_info_), return_type::OK); ASSERT_THAT(joint.get_command_interfaces(), SizeIs(1)); EXPECT_EQ(joint.get_command_interfaces()[0].name, hardware_interface::HW_IF_POSITION); ASSERT_THAT(joint.get_state_interfaces(), SizeIs(1)); @@ -463,35 +463,35 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) // Test DummyPositionJoint // Cannot push a velocity interface - joint_info.command_interfaces.push_back(joint.get_command_interfaces()[0]); + joint_info_.command_interfaces.push_back(joint.get_command_interfaces()[0]); hardware_interface::components::InterfaceInfo velocity_interface; velocity_interface.name = hardware_interface::HW_IF_VELOCITY; - joint_info.command_interfaces.push_back(velocity_interface); - EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); + joint_info_.command_interfaces.push_back(velocity_interface); + EXPECT_EQ(joint.configure(joint_info_), return_type::ERROR); } TEST_F(TestComponentInterfaces, multi_joint_example_component_works) { DummyMultiJoint joint; - joint_info.name = "DummyMultiJoint"; + joint_info_.name = "DummyMultiJoint"; - EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); + EXPECT_EQ(joint.configure(joint_info_), return_type::ERROR); - joint_info.command_interfaces.push_back(joint.get_command_interfaces()[0]); - hardware_interface::components::InterfaceInfo velocity_interface; - velocity_interface.name = hardware_interface::HW_IF_VELOCITY; - joint_info.command_interfaces.push_back(velocity_interface); - - EXPECT_EQ(joint.configure(joint_info), return_type::OK); +// joint_info_.command_interfaces.push_back(joint.get_command_interfaces()[0]); +// hardware_interface::components::InterfaceInfo velocity_interface; +// velocity_interface.name = hardware_interface::HW_IF_VELOCITY; +// joint_info_.command_interfaces.push_back(velocity_interface); +/* + EXPECT_EQ(joint.configure(joint_info_), return_type::OK); ASSERT_THAT(joint.get_command_interfaces(), SizeIs(2)); EXPECT_EQ(joint.get_command_interfaces()[0].name, hardware_interface::HW_IF_POSITION); ASSERT_THAT(joint.get_state_interfaces(), SizeIs(0)); - joint_info.state_interfaces.push_back(velocity_interface); - joint_info.state_interfaces.push_back(velocity_interface); - EXPECT_EQ(joint.configure(joint_info), return_type::OK); + joint_info_.state_interfaces.push_back(velocity_interface); + joint_info_.state_interfaces.push_back(velocity_interface); + EXPECT_EQ(joint.configure(joint_info_), return_type::OK); ASSERT_THAT(joint.get_state_interfaces(), SizeIs(2)); EXPECT_EQ(joint.get_command_interfaces()[1].name, hardware_interface::HW_IF_VELOCITY); @@ -563,13 +563,14 @@ TEST_F(TestComponentInterfaces, multi_joint_example_component_works) EXPECT_EQ(joint.get_state(output), return_type::OK); ASSERT_THAT(output, SizeIs(2)); EXPECT_EQ(output[0], 2.1); +*/ } TEST_F(TestComponentInterfaces, sensor_example_component_works) { DummyForceTorqueSensor sensor; - EXPECT_EQ(sensor.configure(sensor_info), return_type::OK); + EXPECT_EQ(sensor.configure(sensor_info_), return_type::OK); ASSERT_THAT(sensor.get_state_interfaces(), SizeIs(6)); EXPECT_EQ(sensor.get_state_interface_names()[0], "force_x"); EXPECT_EQ(sensor.get_state_interface_names()[5], "torque_z"); @@ -614,8 +615,8 @@ TEST_F(TestComponentInterfaces, sensor_example_component_works) ASSERT_THAT(output, SizeIs(6)); EXPECT_EQ(output[5], 12.3); - sensor_info.parameters.clear(); - EXPECT_EQ(sensor.configure(sensor_info), return_type::ERROR); + sensor_info_.parameters.clear(); + EXPECT_EQ(sensor.configure(sensor_info_), return_type::ERROR); } TEST_F(TestComponentInterfaces, actuator_hardware_interface_works) @@ -628,7 +629,7 @@ TEST_F(TestComponentInterfaces, actuator_hardware_interface_works) actuator_hw_info.hardware_parameters["example_param_write_for_sec"] = "2"; actuator_hw_info.hardware_parameters["example_param_read_for_sec"] = "3"; - EXPECT_EQ(joint->configure(joint_info), return_type::OK); + EXPECT_EQ(joint->configure(joint_info_), return_type::OK); EXPECT_EQ(actuator_hw.configure(actuator_hw_info), return_type::OK); EXPECT_EQ(actuator_hw.get_status(), hardware_interface_status::CONFIGURED); @@ -655,7 +656,7 @@ TEST_F(TestComponentInterfaces, sensor_interface_with_hardware_works) sensor_hw_info.name = "DummySensor"; sensor_hw_info.hardware_parameters["binary_to_voltage_factor"] = "0.0048828125"; - EXPECT_EQ(sensor->configure(sensor_info), return_type::OK); + EXPECT_EQ(sensor->configure(sensor_info_), return_type::OK); EXPECT_EQ(sensor_hw.configure(sensor_hw_info), return_type::OK); EXPECT_EQ(sensor_hw.get_status(), hardware_interface_status::CONFIGURED); @@ -688,9 +689,9 @@ TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) std::vector> sensors; sensors.push_back(sensor); - EXPECT_EQ(joint1->configure(joint_info), return_type::OK); - EXPECT_EQ(joint2->configure(joint_info), return_type::OK); - EXPECT_EQ(sensor->configure(sensor_info), return_type::OK); + EXPECT_EQ(joint1->configure(joint_info_), return_type::OK); + EXPECT_EQ(joint2->configure(joint_info_), return_type::OK); + EXPECT_EQ(sensor->configure(sensor_info_), return_type::OK); HardwareInfo system_hw_info; system_hw_info.name = "DummyActuatorHardware"; From 18ed9737700bc80b5cd1be846dd90d71669a2d8a Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sat, 3 Oct 2020 23:18:55 -0500 Subject: [PATCH 6/9] Update test_component_interfaces. All pass! --- .../test/test_component_interfaces.cpp | 37 ++++++++++--------- 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 365afd2e17..cd5e09a6ad 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -67,8 +67,6 @@ class DummyPositionJoint : public components::Joint states_.resize(1); } - // TODO(andyz): Add min/max parameters to the interface - return return_type::OK; } }; @@ -86,16 +84,11 @@ class DummyMultiJoint : public components::Joint return return_type::ERROR; } - max_position_ = stod(info_.parameters["max_position"]); - min_position_ = stod(info_.parameters["min_position"]); - max_velocity_ = stod(info_.parameters["max_velocity"]); - min_velocity_ = stod(info_.parameters["min_velocity"]); + info_.command_interfaces = joint_info.command_interfaces; + info_.state_interfaces = joint_info.state_interfaces; + return return_type::OK; } - -private: - double max_position_, min_position_; - double max_velocity_, min_velocity_; }; class DummyForceTorqueSensor : public components::Sensor @@ -475,14 +468,21 @@ TEST_F(TestComponentInterfaces, multi_joint_example_component_works) DummyMultiJoint joint; joint_info_.name = "DummyMultiJoint"; - + // Error if fewer than 2 interfaces for a MultiJoint EXPECT_EQ(joint.configure(joint_info_), return_type::ERROR); -// joint_info_.command_interfaces.push_back(joint.get_command_interfaces()[0]); -// hardware_interface::components::InterfaceInfo velocity_interface; -// velocity_interface.name = hardware_interface::HW_IF_VELOCITY; -// joint_info_.command_interfaces.push_back(velocity_interface); -/* + // Define position and velocity interfaces + hardware_interface::components::InterfaceInfo position_interface; + position_interface.name = hardware_interface::HW_IF_POSITION; + position_interface.min = -1; + position_interface.max = 1; + joint_info_.command_interfaces.push_back(position_interface); + hardware_interface::components::InterfaceInfo velocity_interface; + velocity_interface.name = hardware_interface::HW_IF_VELOCITY; + joint_info_.command_interfaces.push_back(velocity_interface); + velocity_interface.min = -1; + velocity_interface.max = 1; + EXPECT_EQ(joint.configure(joint_info_), return_type::OK); ASSERT_THAT(joint.get_command_interfaces(), SizeIs(2)); @@ -540,8 +540,10 @@ TEST_F(TestComponentInterfaces, multi_joint_example_component_works) EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); interfaces.push_back(hardware_interface::HW_IF_POSITION); EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + + interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_POSITION); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); input.clear(); input.push_back(1.2); EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); @@ -563,7 +565,6 @@ TEST_F(TestComponentInterfaces, multi_joint_example_component_works) EXPECT_EQ(joint.get_state(output), return_type::OK); ASSERT_THAT(output, SizeIs(2)); EXPECT_EQ(output[0], 2.1); -*/ } TEST_F(TestComponentInterfaces, sensor_example_component_works) From 5f3bf38882c3de12af6b101ba52f7928bcdffb93 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 5 Oct 2020 09:39:14 -0500 Subject: [PATCH 7/9] Parse command interface min/max attributes --- hardware_interface/src/component_parser.cpp | 19 ++++++++++++++----- .../test/test_component_parser.cpp | 5 ++--- 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 30f86f459e..9602c74517 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -34,6 +34,8 @@ constexpr const auto kSensorTag = "sensor"; constexpr const auto kTransmissionTag = "transmission"; constexpr const auto kCommandInterfaceTypeTag = "commandInterfaceType"; constexpr const auto kStateInterfaceTypeTag = "stateInterfaceType"; +constexpr const auto kMinTag = "min"; +constexpr const auto kMaxTag = "max"; } // namespace namespace hardware_interface @@ -64,7 +66,7 @@ std::string get_text_for_element( * If attribute is not found throws an error. * * \param element_it XMLElement iterator to search for the attribute - * \param attribute_name atribute name to serach for and return value + * \param attribute_name attribute name to search for and return value * \param tag_name parent tag name where attribute is searched for (used for error output) * \return attribute value * \throws std::runtime_error if attribute is not found @@ -88,7 +90,7 @@ std::string get_attribute_value( * If attribute is not found throws an error. * * \param element_it XMLElement iterator to search for the attribute - * \param attribute_name atribute name to serach for and return value + * \param attribute_name atribute name to search for and return value * \param tag_name parent tag name where attribute is searched for (used for error output) * \return attribute value * \throws std::runtime_error if attribute is not found @@ -142,9 +144,6 @@ std::vector parse_interfaces_from_xml( { std::vector interfaces; - - // TODO(andyz): parse optional min/max attributes - while (interfaces_it) { hardware_interface::components::InterfaceInfo interface; @@ -152,6 +151,16 @@ std::vector parse_interfaces_from_xml( if (std::string(interfaceTag) == "commandInterfaceType") { const std::string interface_name = get_attribute_value(interfaces_it, "name", std::string(interfaceTag)); interface.name = interface_name; + + // Optional min/max attributes + std::unordered_map interface_params = + parse_parameters_from_xml(interfaces_it->FirstChildElement(kParamTag)); + std::unordered_map::const_iterator interface_param = interface_params.find(kMinTag); + if (interface_param != interface_params.end()) + interface.min = interface_param->second; + interface_param = interface_params.find(kMaxTag); + if (interface_param != interface_params.end()) + interface.max = interface_param->second; } // State interfaces have an element to define the type, not a name attribute if (std::string(interfaceTag) == "stateInterfaceType") { diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 80786efe2d..4f563886af 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -907,9 +907,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/VelocityJoint"); ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "velocity"); - // TODO(andyz): parse min/max value for this joint -// ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); -// EXPECT_EQ(hardware_info.joints[0].parameters.at("min_velocity_value"), "-1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1"); hardware_info = control_hardware.at(2); From a8828e7d08dd778eb599fc5f5a806118cf736683 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 5 Oct 2020 10:14:44 -0500 Subject: [PATCH 8/9] Format and lint --- hardware_interface/src/component_parser.cpp | 17 ++++++++++++----- .../components/component_lists_management.hpp | 2 +- hardware_interface/src/components/joint.cpp | 6 ++++-- hardware_interface/src/components/sensor.cpp | 3 ++- 4 files changed, 19 insertions(+), 9 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 9602c74517..1798170b94 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -149,22 +149,29 @@ std::vector parse_interfaces_from_xml( // Joint interfaces have a name attribute if (std::string(interfaceTag) == "commandInterfaceType") { - const std::string interface_name = get_attribute_value(interfaces_it, "name", std::string(interfaceTag)); + const std::string interface_name = get_attribute_value( + interfaces_it, "name", + std::string(interfaceTag)); interface.name = interface_name; // Optional min/max attributes std::unordered_map interface_params = parse_parameters_from_xml(interfaces_it->FirstChildElement(kParamTag)); - std::unordered_map::const_iterator interface_param = interface_params.find(kMinTag); - if (interface_param != interface_params.end()) + std::unordered_map::const_iterator interface_param = + interface_params.find(kMinTag); + if (interface_param != interface_params.end()) { interface.min = interface_param->second; + } interface_param = interface_params.find(kMaxTag); - if (interface_param != interface_params.end()) + if (interface_param != interface_params.end()) { interface.max = interface_param->second; + } } // State interfaces have an element to define the type, not a name attribute if (std::string(interfaceTag) == "stateInterfaceType") { - const std::string interface_type = get_text_for_element(interfaces_it, std::string(interfaceTag) + " type "); + const std::string interface_type = get_text_for_element( + interfaces_it, + std::string(interfaceTag) + " type "); interface.name = interface_type; } diff --git a/hardware_interface/src/components/component_lists_management.hpp b/hardware_interface/src/components/component_lists_management.hpp index 6ed7025993..8f8cdfb9b0 100644 --- a/hardware_interface/src/components/component_lists_management.hpp +++ b/hardware_interface/src/components/component_lists_management.hpp @@ -43,7 +43,7 @@ inline return_type get_internal_values( std::vector & values, const std::vector & queried_interfaces, const std::vector & internal_interfaces, const std::vector & internal_values) { - if (queried_interfaces.size() == 0) { + if (queried_interfaces.size() == 0) { return return_type::INTERFACE_NOT_PROVIDED; } diff --git a/hardware_interface/src/components/joint.cpp b/hardware_interface/src/components/joint.cpp index eb4a94df90..bf08fe0cf4 100644 --- a/hardware_interface/src/components/joint.cpp +++ b/hardware_interface/src/components/joint.cpp @@ -42,8 +42,9 @@ return_type Joint::configure(const ComponentInfo & joint_info) std::vector Joint::get_command_interface_names() const { std::vector command_interface_names; - for (auto interface : info_.command_interfaces) + for (auto interface : info_.command_interfaces) { command_interface_names.push_back(interface.name); + } return command_interface_names; } @@ -83,8 +84,9 @@ return_type Joint::set_command(const std::vector & command) std::vector Joint::get_state_interface_names() const { std::vector state_interface_names; - for (auto interface : info_.state_interfaces) + for (auto interface : info_.state_interfaces) { state_interface_names.push_back(interface.name); + } return state_interface_names; } diff --git a/hardware_interface/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp index 0c5046b462..f2b51e369b 100644 --- a/hardware_interface/src/components/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -39,8 +39,9 @@ return_type Sensor::configure(const ComponentInfo & joint_info) std::vector Sensor::get_state_interface_names() const { std::vector state_interface_names; - for (auto interface : info_.state_interfaces) + for (auto interface : info_.state_interfaces) { state_interface_names.push_back(interface.name); + } return state_interface_names; } From 25ae595190673d3aecb008400bc94b00eaa8aac5 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 7 Oct 2020 15:40:27 -0500 Subject: [PATCH 9/9] Minor renaming and whitespace cleanup --- hardware_interface/src/components/sensor.cpp | 3 +-- hardware_interface/test/test_component_interfaces.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/hardware_interface/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp index f2b51e369b..af58cb64a8 100644 --- a/hardware_interface/src/components/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -15,9 +15,8 @@ #include #include -#include "hardware_interface/components/sensor.hpp" - #include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/sensor.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "./component_lists_management.hpp" diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index cd5e09a6ad..3871a4dba1 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -299,12 +299,12 @@ class DummySystemHardware : public SystemHardwareInterface { return_type ret = return_type::OK; std::vector interfaces; - std::vector jointernal_values; + std::vector joint_values; for (uint i = 0; i < joints.size(); i++) { - jointernal_values.clear(); - jointernal_values.push_back(joints_hw_values_[i]); + joint_values.clear(); + joint_values.push_back(joints_hw_values_[i]); interfaces = joints[i]->get_state_interface_names(); - ret = joints[i]->set_state(jointernal_values, interfaces); + ret = joints[i]->set_state(joint_values, interfaces); if (ret != return_type::OK) { break; }