diff --git a/hardware_interface/include/hardware_interface/components/component_info.hpp b/hardware_interface/include/hardware_interface/components/component_info.hpp index 0dc1b78dd2..5e8e30689e 100644 --- a/hardware_interface/include/hardware_interface/components/component_info.hpp +++ b/hardware_interface/include/hardware_interface/components/component_info.hpp @@ -24,6 +24,27 @@ 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) minimal allowed values of the interface. + */ + std::string min; + /** + * \brief (optional) maximal allowed values of the interface. + */ + std::string max; +}; + /** * \brief This structure stores information about components defined for a specific hardware * in robot's URDF. @@ -46,14 +67,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 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 0101afa6f0..428da42591 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -50,17 +50,28 @@ class Joint * return_type::ERROR otherwise. */ HARDWARE_INTERFACE_PUBLIC - virtual return_type configure(const ComponentInfo & joint_info); /** * \brief Provide the list of command interfaces configured for the joint. - * - * \return string list with command interfaces. + * \return vector of command interface names. */ HARDWARE_INTERFACE_PUBLIC - virtual - std::vector get_command_interfaces() const; + 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; + + /** + * \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. @@ -68,8 +79,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 @@ -78,16 +88,16 @@ 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 * 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 +108,6 @@ class Joint * \return return_type::OK always. */ HARDWARE_INTERFACE_EXPORT - virtual return_type get_command(std::vector & command) const; /** @@ -107,7 +116,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 @@ -118,9 +127,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,7 +142,6 @@ class Joint * of limits; return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - virtual return_type set_command(const std::vector & command); /** @@ -142,14 +150,13 @@ class Joint * 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 * is empty; return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - virtual return_type get_state( std::vector & state, const std::vector & interfaces) const; @@ -163,7 +170,6 @@ class Joint * \return return_type::OK always. */ HARDWARE_INTERFACE_EXPORT - virtual return_type get_state(std::vector & state) const; /** @@ -172,15 +178,15 @@ 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. */ 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 +198,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..ddc3c2e85f 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -50,17 +50,23 @@ class Sensor * return_type::ERROR otherwise. */ HARDWARE_INTERFACE_PUBLIC - virtual 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 string list with state interfaces. + * \return list of state interfaces. */ HARDWARE_INTERFACE_PUBLIC - virtual - std::vector get_state_interfaces() const; + std::vector get_state_interfaces() const; /** * \brief Get state list from the sensor. This function is used by the controller to get the @@ -68,16 +74,16 @@ 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 * 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 +94,6 @@ class Sensor * \return return_type::OK always. */ HARDWARE_INTERFACE_EXPORT - virtual return_type get_state(std::vector & state) const; /** @@ -97,15 +102,15 @@ 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. */ 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 +122,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..1798170b94 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 @@ -132,22 +134,51 @@ 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; while (interfaces_it) { - const std::string interface_type = get_text_for_element( - interfaces_it, std::string(interfaceTag) + " type "); - interfaces.push_back(interface_type); + hardware_interface::components::InterfaceInfo 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; + + // 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") { + 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; } diff --git a/hardware_interface/src/components/component_lists_management.hpp b/hardware_interface/src/components/component_lists_management.hpp index deb949a980..8f8cdfb9b0 100644 --- a/hardware_interface/src/components/component_lists_management.hpp +++ b/hardware_interface/src/components/component_lists_management.hpp @@ -26,23 +26,22 @@ 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 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 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 & int_values) + const std::vector & internal_interfaces, const std::vector & internal_values) { if (queried_interfaces.size() == 0) { return return_type::INTERFACE_NOT_PROVIDED; @@ -50,9 +49,9 @@ inline return_type get_internal_values( 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)]); + 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; @@ -65,31 +64,31 @@ 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 - * int_interfaces vector. + * \brief set values for queried_interfaces to the internal_values. internal_values data structure matches + * 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 int_values internal values 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. @@ -97,7 +96,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 & int_values) + const std::vector & internal_interfaces, std::vector & internal_values) { if (queried_interfaces.size() == 0) { return return_type::INTERFACE_NOT_PROVIDED; @@ -107,14 +106,15 @@ inline return_type set_internal_values( } 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)] = + 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; } } + return return_type::OK; } @@ -122,15 +122,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..bf08fe0cf4 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,12 +39,21 @@ return_type Joint::configure(const ComponentInfo & joint_info) return return_type::OK; } -std::vector Joint::get_command_interfaces() const +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; } -std::vector Joint::get_state_interfaces() const +std::vector Joint::get_state_interfaces() const { return info_.state_interfaces; } @@ -51,7 +61,7 @@ std::vector Joint::get_state_interfaces() const return_type Joint::get_command( 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 @@ -60,9 +70,10 @@ 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_); + return set_internal_values(command, interfaces, get_command_interface_names(), commands_); } return_type Joint::set_command(const std::vector & command) @@ -70,10 +81,19 @@ 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 { - 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,7 +104,7 @@ return_type Joint::get_state(std::vector & state) const return_type Joint::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 Joint::set_state(const std::vector & state) diff --git a/hardware_interface/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp index 397bee648d..af58cb64a8 100644 --- a/hardware_interface/src/components/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -15,8 +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" @@ -35,7 +35,16 @@ return_type Sensor::configure(const ComponentInfo & joint_info) return return_type::OK; } -std::vector Sensor::get_state_interfaces() const +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; } @@ -43,7 +52,7 @@ std::vector Sensor::get_state_interfaces() const 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 @@ -54,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/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_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 0526be9bf0..1458c77293 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -52,22 +52,23 @@ 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.max = "1"; + dummy_position_interface.min = "-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); } - max_position_ = stod(info_.parameters["max_position"]); - min_position_ = stod(info_.parameters["min_position"]); return return_type::OK; } - -private: - double max_position_, min_position_; }; class DummyMultiJoint : public components::Joint @@ -83,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 @@ -108,12 +104,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 +169,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); } @@ -294,7 +303,7 @@ class DummySystemHardware : public SystemHardwareInterface for (uint i = 0; i < joints.size(); i++) { joint_values.clear(); joint_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(joint_values, interfaces); if (ret != return_type::OK) { break; @@ -308,7 +317,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; @@ -351,17 +360,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"; } }; @@ -369,11 +378,11 @@ 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], 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); + EXPECT_EQ(joint.get_state_interface_names()[0], hardware_interface::HW_IF_POSITION); // Command getters and setters std::vector interfaces; @@ -385,7 +394,7 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) 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_interface_names()[0]); input.clear(); input.push_back(1.2); EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); @@ -393,7 +402,7 @@ 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_interface_names()[0]); EXPECT_EQ(joint.get_command(output, interfaces), return_type::OK); ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 1.2); @@ -428,7 +437,7 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) 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_interface_names()[0]); EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 1.2); @@ -446,37 +455,45 @@ 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); - EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); + // 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); } 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_.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(hardware_interface::HW_IF_POSITION); - joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + // 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); + 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); + 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], 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; @@ -523,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); @@ -552,10 +571,10 @@ 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_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; @@ -577,7 +596,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(); @@ -597,8 +616,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) @@ -611,14 +630,14 @@ 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(), status::CONFIGURED); EXPECT_EQ(actuator_hw.start(), return_type::OK); EXPECT_EQ(actuator_hw.get_status(), 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)); @@ -638,7 +657,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(), status::CONFIGURED); @@ -648,7 +667,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)); @@ -671,9 +690,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"; @@ -688,25 +707,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_interface_names(); + 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_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], hardware_interface::HW_IF_POSITION); + } output.clear(); - interfaces.clear(); - 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 35dac0768b..4f563886af 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -172,7 +172,8 @@ 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) +// Note, joint parameters can be any string valid_urdf_ros2_control_system_multi_interface_ = R"( @@ -183,27 +184,39 @@ 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 + + A42B1 ros2_control_components/MultiInterfaceJoint - position + + -1 + 1 + + position velocity effort - -1 - 1 + + A42B2 )"; @@ -305,15 +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 - velocity - -1 - 1 + + -1 + 1 + + + D transmission_interface/SimpleTansmission @@ -322,15 +338,16 @@ 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 - velocity - -1 - 1 + + -1 + 1 + @@ -422,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 @@ -697,20 +714,20 @@ 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"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(6)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("min_effort_value"), "-0.5"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, "velocity"); + 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"); 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"); - ASSERT_THAT(hardware_info.joints[1].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[1].parameters.at("min_position_value"), "-1"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, "effort"); + 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) @@ -854,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"); @@ -863,9 +880,9 @@ 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"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("max_velocity_value"), "1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "velocity"); + 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"); @@ -880,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"); @@ -889,9 +906,9 @@ 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"); - 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].name, "velocity"); + 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); @@ -910,7 +927,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 +949,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 +1015,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 +1025,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"); @@ -1026,7 +1043,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"); 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));