diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index ac7cd5a5f9..d5c05c7388 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -14,6 +14,8 @@ find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rcpputils REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) add_library( hardware_interface @@ -41,6 +43,21 @@ ament_target_dependencies( # which is appropriate when building the dll but not consuming it. target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") +add_library( + component_parser + src/component_parser.cpp +) +target_include_directories( + component_parser + PUBLIC + include +) +ament_target_dependencies( + component_parser + TinyXML2 +) +target_compile_definitions(component_parser PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") + add_library( components SHARED @@ -52,8 +69,6 @@ target_include_directories( PUBLIC include ) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. target_compile_definitions(components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") install( @@ -63,6 +78,7 @@ install( install( TARGETS + component_parser components hardware_interface RUNTIME DESTINATION bin @@ -93,6 +109,11 @@ if(BUILD_TESTING) target_link_libraries(test_actuator_handle hardware_interface) ament_target_dependencies(test_actuator_handle rcpputils) + ament_add_gmock(test_component_parser test/test_component_parser.cpp) + target_include_directories(test_component_parser PRIVATE include) + target_link_libraries(test_component_parser component_parser) + ament_target_dependencies(test_component_parser TinyXML2) + ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) target_include_directories(test_component_interfaces PRIVATE include) target_link_libraries(test_component_interfaces components hardware_interface) @@ -102,6 +123,7 @@ ament_export_include_directories( include ) ament_export_libraries( + component_parser components hardware_interface ) diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp new file mode 100644 index 0000000000..a21cc651ca --- /dev/null +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -0,0 +1,127 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ +#define HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +/** + * \brief Search XML snippet from URDF for information about a control component. + * + * \param urdf string with robot's URDF + * \return vector filled with information about robot's control resources + * \throws std::runtime_error if a robot attribute or tag is not found + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_control_resources_from_urdf(const std::string & urdf); + +/** + * \brief Parse a control resource from an "ros2_control" tag. + * + * \param ros2_control_it pointer to ros2_control element with informtions about resource. + * \return robot_control_components::ComponentInfo filled with information about the robot + * \throws std::runtime_error if a attributes or tag are not found + */ +HARDWARE_INTERFACE_PUBLIC +HardwareInfo parse_resource_from_xml(const tinyxml2::XMLElement * ros2_control_it); + +/** + * \brief Gets value of the attribute on an XMLelement. + * 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 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 + */ +HARDWARE_INTERFACE_PUBLIC +std::string get_attribute_value( + const tinyxml2::XMLElement * element_it, + const char * attribute_name, const char * tag_name); + +/** + * \brief Gets value of the text between tags. + * + * \param element_it XMLElement iterator to search for the text. + * \param tag_name parent tag name where text is searched for (used for error output) + * \return text of for the tag + * \throws std::runtime_error if text is not found + */ +HARDWARE_INTERFACE_PUBLIC +std::string get_text_for_element( + const tinyxml2::XMLElement * element_it, + const std::string & tag_name); + +/** + * \brief Gets value of the attribute on an XMLelement. + * 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 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 + */ +HARDWARE_INTERFACE_PUBLIC +std::string get_attribute_value( + const tinyxml2::XMLElement * element_it, + const char * attribute_name, std::string tag_name); + +/** + * \brief Search XML snippet from URDF for information about a control component. + * + * \param component_it pointer to the iterator where component info should be found + * \return robot_control_components::ComponentInfo filled with information about component + * \throws std::runtime_error if a component attribute or tag is not found + */ +HARDWARE_INTERFACE_PUBLIC +components::ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it); + +/** + * \brief Search XML snippet for definition of interfaceTypes. + * + * \param interfaces_it pointer to the interator 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 + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_interfaces_from_xml( + const tinyxml2::XMLElement * interfaces_it, const char * interfaceTag); + +/** + * \brief Search XML snippet from URDF for parameters. + * + * \param params_it pointer to the iterator where parameters info should be found + * \return std::map< std::__cxx11::string, std::__cxx11::string > key-value map with parameters + * \throws std::runtime_error if a component attribute or tag is not found + */ +HARDWARE_INTERFACE_PUBLIC +std::unordered_map parse_parameters_from_xml( + const tinyxml2::XMLElement * params_it); + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp b/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp new file mode 100644 index 0000000000..bde5d325b6 --- /dev/null +++ b/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp @@ -0,0 +1,226 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ +#define HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ + +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +namespace helpers +{ + +/** + * \brief Get values for queried_interfaces from the int_values. int_values data structure matches + * int_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. + * \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 + * 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) +{ + 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; + } + } + return return_type::OK; +} + +/** + * \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. + * \return return_type::OK always. + */ +inline return_type get_internal_values( + std::vector & values, const std::vector & int_values) +{ + values.clear(); + for (const auto & int_value : int_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. + * + * \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. + * \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. + * + * \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. + * (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) +{ + if (queried_interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + if (values.size() != queried_interfaces.size()) { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + + 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)] = + values[std::distance(queried_interfaces.begin(), q_it)]; + } else { + return return_type::INTERFACE_NOT_FOUND; + } + } + return return_type::OK; +} + +/** + * \brief set values for queried_interfaces to the int_values considering the value limits. + * int_values, lower_limits and upper_limits 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 lower_limits list of lower limits. + * \param upper_limits list of upper limits. + * \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::VALUE_OUT_OF_LIMITS if a + * value is not in the limits; return_type::INTERFACE_NOT_FOUND if + * one of queried_interfaces is not defined in int_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. + * (see: https://github.com/ros-controls/ros2_control/issues/129) + */ +inline return_type set_internal_values_with_limits( + const std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, std::vector & int_values, + const std::vector & lower_limits, const std::vector & upper_limits) +{ + if (queried_interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + if (values.size() != queried_interfaces.size()) { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + + 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()) { + if (values[std::distance(queried_interfaces.begin(), q_it)] >= + lower_limits[std::distance(int_interfaces.begin(), it)] && + values[std::distance(queried_interfaces.begin(), q_it)] <= + upper_limits[std::distance(int_interfaces.begin(), it)]) + { + int_values[std::distance(int_interfaces.begin(), it)] = + values[std::distance(queried_interfaces.begin(), q_it)]; + } else { + return return_type::VALUE_OUT_OF_LIMITS; + } + } else { + return return_type::INTERFACE_NOT_FOUND; + } + } + return return_type::OK; +} + +/** + * \brief set all values to compoenents internal values. + * + * \param values values to set. + * \param int_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) +{ + if (values.size() == int_values.size()) { + for (uint i = 0; i < int_values.size(); i++) { + int_values[i] = values[i]; + } + } else { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return return_type::OK; +} + +/** + * \brief set all values to compoenents internal values considering limits. + * int_values, lower_limits and upper_limits have the same data structure. + * + * \param values values to set. + * \param int_values internal values of a component. + * \param lower_limits list of lower limits. + * \param upper_limits list of upper limits. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if the size of the arguments is not equal; + * return_type::VALUE_OUT_OF_LIMITS if a value is not in the limits; return_type::OK otherwise. + */ +inline return_type set_internal_values_with_limits( + const std::vector & values, std::vector & int_values, + const std::vector & lower_limits, const std::vector & upper_limits) +{ + if (values.size() == int_values.size()) { + for (uint i = 0; i < int_values.size(); i++) { + if (values[i] >= lower_limits[i] && values[i] <= upper_limits[i]) { + int_values[i] = values[i]; + } else { + return return_type::VALUE_OUT_OF_LIMITS; + } + } + } else { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return return_type::OK; +} + +} // namespace helpers +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp new file mode 100644 index 0000000000..9184d906c7 --- /dev/null +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -0,0 +1,195 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__JOINT_HPP_ +#define HARDWARE_INTERFACE__JOINT_HPP_ + +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +/** + * \brief Base Class for the "Joint" component used as a basic building block for a robot. + * A joint is always 1-DoF and can have one or more interfaces (e.g., position, velocity, etc.) + * A joint has to be able to receive command(s) and optionally can provide its state(s). + */ +class Joint +{ +public: + HARDWARE_INTERFACE_PUBLIC + Joint() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~Joint() = default; + + /** + * \brief Configure base joint class based on the description in the robot's URDF file. + * + * \param joint_info structure with data from URDF. + * \return return_type::OK if required data are provided and is successfully parsed, + * return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + return_type configure(const ComponentInfo & joint_info); + + /** + * \brief Provide the list of command interfaces configured for the joint. + * + * \return string list with command interfaces. + */ + HARDWARE_INTERFACE_PUBLIC + std::vector get_command_interfaces() const; + + /** + * \brief Provide the list of state interfaces configured for the joint. + * + * \return string list with state interfaces. + */ + HARDWARE_INTERFACE_PUBLIC + std::vector get_state_interfaces() const; + + /** + * \brief Get command list from the joint. This function is used in the write function of the + * actuator or system hardware. The parameters command and interfaces have the same order, + * and number of elements. Using the interfaces list, the hardware can choose which values to + * provide. + * + * \param command list of doubles with commands for the hardware. + * \param interfaces list of interfaces on which commands have to set. + * \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 + return_type get_command( + 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 + * complete command for it. The hardware valus have the same order as interfaces which + * can be recived by get_hardware_interfaces() function. Return value is used for API consistency. + * + * \param command list of doubles with commands for the hardware. + * \return return_type::OK always. + */ + HARDWARE_INTERFACE_EXPORT + return_type get_command(std::vector & command) const; + + /** + * \brief Set command list for the joint. This function is used by the controller to set the goal + * values for the hardware. The parameters command, and interfaces have the same order and number + * 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. + * \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 + * defined for the joint; 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. + * (see: https://github.com/ros-controls/ros2_control/issues/129) + */ + HARDWARE_INTERFACE_EXPORT + return_type set_command( + 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 + * complete command for it. The hardware valus have the same order as interfaces which + * can be recived by get_hardware_interfaces() function. + * + * \param command list of doubles with commands for the hardware. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is command size is not equal to number of + * joint's command interfaces; return_type::COMMAND_OUT_OF_LIMITS if one of the command values is out + * of limits; return_type::OK otherwise. + */ + 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. + * \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 + return_type get_state( + std::vector & state, + const std::vector & interfaces) const; + + /** + * \brief Get complete state list from the joint. This function is used by the controller to get + * complete actual state of the hardware. The state values have the same order as interfaces which + * can be recived by get_state_interfaces() function. Return value is used for API consistency. + * + * \param state list of doubles with states of the hardware. + * \return return_type::OK always. + */ + HARDWARE_INTERFACE_EXPORT + return_type get_state(std::vector & state) const; + + /** + * \brief Set state list for the joint. This function is used by the hardware to set its actual + * state. The parameters state, and interfaces have the same order and number of elements. Using + * 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. + * \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 + return_type set_state( + 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 + * complete actual state. The state values have the same order as interfaces which can be recived + * by get_state_interfaces() function. + * + * \param state list of doubles with states from the hardware. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is command size is not equal to number of + * joint's state interfaces, return_type::OK otherwise. + */ + HARDWARE_INTERFACE_EXPORT + return_type set_state(const std::vector & state); + +protected: + ComponentInfo info_; + std::vector commands_; + std::vector states_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__JOINT_HPP_ diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp new file mode 100644 index 0000000000..d68343ea1a --- /dev/null +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -0,0 +1,123 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__SENSOR_HPP_ +#define HARDWARE_INTERFACE__SENSOR_HPP_ + +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +/** + * \brief Base Class for "Sensor" component used as a basic build block for the devices which + * provide data. A sensor can have one or more interfaces (e.g., force, acceleration, etc.) to + * provide states for. The list of state interfaces defines this. + */ +class Sensor +{ +public: + HARDWARE_INTERFACE_PUBLIC + Sensor() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~Sensor() = default; + + /** + * \brief Configure base sensor class based on the description in the robot's URDF file. + * + * \param joint_info structure with data from URDF. + * \return return_type::OK if required data are provided and is successfully parsed, + * return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + return_type configure(const ComponentInfo & joint_info); + + /** + * \brief Provide the list of state interfaces configured for the sensor. + * + * \return string list with state interfaces. + */ + HARDWARE_INTERFACE_PUBLIC + std::vector get_state_interfaces(); + + /** + * \brief Get state list from the sensor. 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. + * \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 + return_type get_state( + 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 + * complete actual state of the hardware. The state values have the same order as interfaces which + * can be recived by get_state_interfaces() function. Return value is used for API consistency. + * + * \param state list of doubles with states of the hardware. + * \return return_type::OK always. + */ + HARDWARE_INTERFACE_EXPORT + return_type get_state(std::vector & state) const; + + /** + * \brief Set state list for the sensor. This function is used by the hardware to set its actual + * state. The parameters state, and interfaces have the same order and number of elements. Using + * 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. + * \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 + return_type set_state( + 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 + * complete actual state. The state values have the same order as interfaces which can be recived + * by get_state_interfaces() function. + * + * \param state list of doubles with states from the hardware. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is state size is not equal to number of + * sensor's state interfaces, return_type::OK otherwise. + */ + HARDWARE_INTERFACE_EXPORT + return_type set_state(const std::vector & state); + +protected: + ComponentInfo info_; + std::vector states_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__SENSOR_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp index d09fea6e1a..6d3e6a12af 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp @@ -36,7 +36,12 @@ enum class return_type : std::uint8_t INTERFACE_VALUE_SIZE_NOT_EQUAL = 31, INTERFACE_NOT_PROVIDED = 32, - COMMAND_OUT_OF_LIMITS = 40, + VALUE_OUT_OF_LIMITS = 40, + + COMPONENT_TOO_MANY_INTERFACES = 51, + COMPONENT_WRONG_INTERFACE = 52, + COMPONENT_ONLY_STATE_DEFINED = 53, + COMPONENT_MISSING_PARAMETER = 54, }; using hardware_interface_ret_t = return_type; diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 74b50f4098..9c6426aa6d 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -9,9 +9,10 @@ ament_cmake + control_msgs rclcpp rcpputils - control_msgs + tinyxml2_vendor ament_cmake_gmock ament_lint_auto diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp new file mode 100644 index 0000000000..27a07945fa --- /dev/null +++ b/hardware_interface/src/component_parser.cpp @@ -0,0 +1,218 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/component_parser.hpp" + +namespace +{ +constexpr const auto kRobotTag = "robot"; +constexpr const auto kROS2ControlTag = "ros2_control"; +constexpr const auto kHardwareTag = "hardware"; +constexpr const auto kClassTypeTag = "classType"; +constexpr const auto kParamTag = "param"; +constexpr const auto kJointTag = "joint"; +constexpr const auto kSensorTag = "sensor"; +constexpr const auto kTransmissionTag = "transmission"; +constexpr const auto kCommandInterfaceTypeTag = "commandInterfaceType"; +constexpr const auto kStateInterfaceTypeTag = "stateInterfaceType"; +} // namespace + +namespace hardware_interface +{ + +std::vector parse_control_resources_from_urdf(const std::string & urdf) +{ + // Check if everything OK with URDF string + if (urdf.empty()) { + throw std::runtime_error("empty URDF passed to robot"); + } + tinyxml2::XMLDocument doc; + if (!doc.Parse(urdf.c_str()) && doc.Error()) { + throw std::runtime_error("invalid URDF passed in to robot parser"); + } + if (doc.Error()) { + throw std::runtime_error("invalid URDF passed in to robot parser"); + } + + // Find robot tag + const tinyxml2::XMLElement * robot_it = doc.RootElement(); + + if (std::string(kRobotTag).compare(robot_it->Name())) { + throw std::runtime_error("the robot tag is not root element in URDF"); + } + + const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag); + if (!ros2_control_it) { + throw std::runtime_error("no " + std::string(kROS2ControlTag) + " tag"); + } + + std::vector hardware_info; + while (ros2_control_it) { + hardware_info.push_back(parse_resource_from_xml(ros2_control_it)); + ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag); + } + + return hardware_info; +} + +HardwareInfo parse_resource_from_xml(const tinyxml2::XMLElement * ros2_control_it) +{ + HardwareInfo hardware; + hardware.name = get_attribute_value(ros2_control_it, "name", kROS2ControlTag); + hardware.type = get_attribute_value(ros2_control_it, "type", kROS2ControlTag); + + // Parse everything under ros2_control tag + hardware.hardware_class_type = ""; + const auto * ros2_control_child_it = ros2_control_it->FirstChildElement(); + while (ros2_control_child_it) { + if (!std::string(kHardwareTag).compare(ros2_control_child_it->Name())) { + const auto * type_it = ros2_control_child_it->FirstChildElement(kClassTypeTag); + hardware.hardware_class_type = get_text_for_element( + type_it, std::string("hardware ") + kClassTypeTag); + const auto * params_it = ros2_control_child_it->FirstChildElement(kParamTag); + if (params_it) { + hardware.hardware_parameters = parse_parameters_from_xml(params_it); + } + } else if (!std::string(kJointTag).compare(ros2_control_child_it->Name())) { + hardware.joints.insert( + {get_attribute_value(ros2_control_child_it, "name", kJointTag), + parse_component_from_xml(ros2_control_child_it)}); + } else if (!std::string(kSensorTag).compare(ros2_control_child_it->Name())) { + hardware.sensors.insert( + {get_attribute_value(ros2_control_child_it, "name", kSensorTag), + parse_component_from_xml(ros2_control_child_it)}); + } else if (!std::string(kTransmissionTag).compare(ros2_control_child_it->Name())) { + hardware.transmissions.insert( + {get_attribute_value(ros2_control_child_it, "name", kTransmissionTag), + parse_component_from_xml(ros2_control_child_it)}); + } else { + throw std::runtime_error("invalid tag name " + std::string(ros2_control_child_it->Name())); + } + ros2_control_child_it = ros2_control_child_it->NextSiblingElement(); + } + + return hardware; +} + +std::string get_attribute_value( + const tinyxml2::XMLElement * element_it, const char * attribute_name, + const char * tag_name) +{ + return get_attribute_value(element_it, attribute_name, std::string(tag_name)); +} + +std::string get_attribute_value( + const tinyxml2::XMLElement * element_it, const char * attribute_name, + std::string tag_name) +{ + const tinyxml2::XMLAttribute * attr; + attr = element_it->FindAttribute(attribute_name); + if (!attr) { + throw std::runtime_error( + "no attribute " + std::string(attribute_name) + " in " + tag_name + " tag"); + } + return element_it->Attribute(attribute_name); +} + +std::string get_text_for_element( + const tinyxml2::XMLElement * element_it, const std::string & tag_name) +{ + const auto get_text_output = element_it->GetText(); + if (!get_text_output) { + throw std::runtime_error("text not specified in the " + tag_name + " tag"); + } + return get_text_output; +} + +components::ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it) +{ + components::ComponentInfo component; + + // Find name, type and class of a component + component.type = component_it->Name(); + component.name = get_attribute_value(component_it, "name", component.type); + + const auto * classType_it = component_it->FirstChildElement(kClassTypeTag); + if (!classType_it) { + throw std::runtime_error("no class type tag found in " + component.name); + } + 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) { + component.state_interfaces = parse_interfaces_from_xml( + state_interfaces_it, kStateInterfaceTypeTag); + } + + // Parse paramter tags + const auto * params_it = component_it->FirstChildElement(kParamTag); + if (params_it) { + component.parameters = parse_parameters_from_xml(params_it); + } + + return component; +} + +std::vector parse_interfaces_from_xml( + const tinyxml2::XMLElement * interfaces_it, const char * interfaceTag) +{ + 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); + interfaces_it = interfaces_it->NextSiblingElement(interfaceTag); + } + return interfaces; +} + +std::unordered_map parse_parameters_from_xml( + const tinyxml2::XMLElement * params_it) +{ + std::unordered_map parameters; + const tinyxml2::XMLAttribute * attr; + + while (params_it) { + // Fill the map with parameters + attr = params_it->FindAttribute("name"); + if (!attr) { + throw std::runtime_error("no parameter name attribute set in param tag"); + } + const std::string parameter_name = params_it->Attribute("name"); + const std::string parameter_value = get_text_for_element(params_it, parameter_name); + parameters[parameter_name] = parameter_value; + + params_it = params_it->NextSiblingElement(kParamTag); + } + return parameters; +} + +} // namespace hardware_interface diff --git a/hardware_interface/src/joint.cpp b/hardware_interface/src/joint.cpp new file mode 100644 index 0000000000..e5d5ef35c6 --- /dev/null +++ b/hardware_interface/src/joint.cpp @@ -0,0 +1,93 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/joint.hpp" +#include "hardware_interface/helpers/component_lists_management.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace hardware_interface +{ + +return_type Joint::configure(const ComponentInfo & joint_info) +{ + info_ = joint_info; + if (info_.command_interfaces.size() > 0) { + commands_.resize(info_.command_interfaces.size()); + } + if (info_.state_interfaces.size() > 0) { + states_.resize(info_.state_interfaces.size()); + } + return return_type::OK; +} + +std::vector Joint::get_command_interfaces() const +{ + return info_.command_interfaces; +} + +std::vector Joint::get_state_interfaces() const +{ + return info_.state_interfaces; +} + +return_type Joint::get_command( + std::vector & command, const std::vector & interfaces) const +{ + return helpers::get_internal_values(command, interfaces, info_.command_interfaces, commands_); +} + +return_type Joint::get_command(std::vector & command) const +{ + return helpers::get_internal_values(command, commands_); +} + +return_type Joint::set_command( + const std::vector & command, + const std::vector & interfaces) +{ + return helpers::set_internal_values(command, interfaces, info_.command_interfaces, commands_); +} + +return_type Joint::set_command(const std::vector & command) +{ + return helpers::set_internal_values(command, commands_); +} + +return_type Joint::get_state( + std::vector & state, const std::vector & interfaces) const +{ + return helpers::get_internal_values(state, interfaces, info_.state_interfaces, states_); +} + +return_type Joint::get_state(std::vector & state) const +{ + return helpers::get_internal_values(state, states_); +} + +return_type Joint::set_state( + const std::vector & state, const std::vector & interfaces) +{ + return helpers::set_internal_values(state, interfaces, info_.state_interfaces, states_); +} + +return_type Joint::set_state(const std::vector & state) +{ + return helpers::set_internal_values(state, states_); +} + +} // namespace hardware_interface diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp new file mode 100644 index 0000000000..6723e709ce --- /dev/null +++ b/hardware_interface/src/sensor.cpp @@ -0,0 +1,62 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/helpers/component_lists_management.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace hardware_interface +{ + +return_type Sensor::configure(const ComponentInfo & joint_info) +{ + info_ = joint_info; + if (info_.state_interfaces.size() > 0) { + states_.resize(info_.state_interfaces.size()); + } + return return_type::OK; +} + +std::vector Sensor::get_state_interfaces() +{ + return info_.state_interfaces; +} + +return_type Sensor::get_state( + std::vector & state, const std::vector & interfaces) const +{ + return helpers::get_internal_values(state, interfaces, info_.state_interfaces, states_); +} + +return_type Sensor::get_state(std::vector & state) const +{ + return helpers::get_internal_values(state, states_); +} + +return_type Sensor::set_state( + const std::vector & state, const std::vector & interfaces) +{ + return helpers::set_internal_values(state, interfaces, info_.state_interfaces, states_); +} + +return_type Sensor::set_state(const std::vector & state) +{ + return helpers::set_internal_values(state, states_); +} + +} // namespace hardware_interface diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp new file mode 100644 index 0000000000..7f41ff271e --- /dev/null +++ b/hardware_interface/test/test_component_parser.cpp @@ -0,0 +1,1099 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "hardware_interface/component_parser.hpp" + +using namespace ::testing; // NOLINT + +class TestComponentParser : public Test +{ +protected: + void SetUp() override + { + urdf_xml_head_ = + R"( + + + + + + + + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Gray + + + Gazebo/Gray + + + Gazebo/Gray + + + + + +)"; + + urdf_xml_tail_ = + R"( + +)"; + +// 1. Industrial Robots with only one interface + valid_urdf_ros2_control_system_one_interface_ = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + 2 + 2 + + + ros2_control_components/PositionJoint + -1 + 1 + + + ros2_control_components/PositionJoint + -1 + 1 + + +)"; + +// 2. Industrial Robots with multiple interfaces (can not be written at the same time) + valid_urdf_ros2_control_system_multi_interface_ = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_MultiInterface + 2.2 + 2.3 + + + ros2_control_components/MultiInterfaceJoint + position + velocity + effort + position + velocity + effort + -1 + 1 + -1 + 1 + -0.5 + 0.5 + + + ros2_control_components/MultiInterfaceJoint + position + position + velocity + effort + -1 + 1 + -1 + 1 + -0.5 + 0.5 + + +)"; + +// 3. Industrial Robots with integrated sensor + valid_urdf_ros2_control_system_robot_with_sensor_ = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Sensor + 2 + 2 + + + ros2_control_components/PositionJoint + -1 + 1 + + + ros2_control_components/PositionJoint + -1 + 1 + + + ros2_control_components/ForceTorqueSensor + kuka_tcp + -100 + 100 + + +)"; + +// 4. Industrial Robots with externally connected sensor + valid_urdf_ros2_control_system_robot_with_external_sensor_ = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + 2 + 2 + + + ros2_control_components/PositionJoint + -1 + 1 + + + ros2_control_components/PositionJoint + -1 + 1 + + + + + ros2_control_demo_hardware/2D_Sensor_Force_Torque + 0.43 + + + ros2_control_components/ForceTorqueSensor + kuka_tcp + -100 + 100 + + +)"; + +// 5. Modular Robots with separate communication to each actuator + valid_urdf_ros2_control_actuator_modular_robot_ = + R"( + + + ros2_control_demo_hardware/Position_Actuator_Hadware + 1.23 + 3 + + + ros2_control_components/PositionJoint + -1 + 1 + + + + + ros2_control_demo_hardware/Position_Actuator_Hadware + 1.23 + 2.12 + + + ros2_control_components/PositionJoint + -1 + 1 + + +)"; + +// 6. Modular Robots with actuators not providing states and with additional sensors +// Example for simple transmission + valid_urdf_ros2_control_actuator_modular_robot_sensors_ = + R"( + + + ros2_control_demo_hardware/Velocity_Actuator_Hadware + 1.23 + 3 + + + ros2_control_components/VelocityJoint + velocity + -1 + 1 + + + transmission_interface/SimpleTansmission + ${1024/PI} + + + + + ros2_control_demo_hardware/Velocity_Actuator_Hadware + 1.23 + 3 + + + ros2_control_components/VelocityJoint + velocity + -1 + 1 + + + + + ros2_control_demo_hardware/Position_Sensor_Hardware + 2 + + + ros2_control_components/PositionJoint + position + ${-PI} + ${PI} + + + + + ros2_control_demo_hardware/Position_Sensor_Hardware + 2 + + + ros2_control_components/PositionJoint + position + ${-PI} + ${PI} + + +)"; + +// 7. Modular Robots with separate communication to each "actuator" with multi joints +// Example for complex transmission (multi-joint/multi-actuator) - (system component is used) + valid_urdf_ros2_control_system_muti_joints_transmission_ = + R"( + + + ros2_control_demo_hardware/Actuator_Hadware_MultiDOF + 1.23 + 3 + + + ros2_control_components/PositionJoint + -1 + 1 + + + ros2_control_components/PositionJoint + -1 + 1 + + + transmission_interface/SomeComplex_2x2_Transmission + {joint1, joint2} + {output2, output2} + 1.5 + 3.2 + 3.1 + 1.4 + + +)"; + +// 8. Sensor only + valid_urdf_ros2_control_sensor_only_ = + R"( + + + ros2_control_demo_hardware/CameraWithIMU_Sensor + 2 + + + ros2_control_components/IMUSensor + velocity + acceleration + -54 + 23 + -10 + 10 + + + ros2_control_components/2DImageSensor + image + 0 + 255 + + +)"; + +// 9. Actuator Only + valid_urdf_ros2_control_actuator_only_ = + R"( + + + ros2_control_demo_hardware/Velocity_Actuator_Hadware + 1.13 + 3 + + + ros2_control_components/VelocityJoint + -1 + 1 + + + transmission_interface/RotationToLinearTansmission + ${1024/PI} + + +)"; + +// Errors + invalid_urdf_ros2_control_invalid_child_ = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + 2 + 2 + + + )"; + + invalid_urdf_ros2_control_missing_attribute_ = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + 2 + 2 + + + )"; + + invalid_urdf_ros2_control_component_missing_class_type_ = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + 2 + 2 + + + -1 + 1 + + +)"; + + invalid_urdf_ros2_control_parameter_missing_name_ = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + 2 + 2 + + + ros2_control_components/PositionJoint + -1 + 1 + + +)"; + + + invalid_urdf_ros2_control_component_class_type_empty_ = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + 2 + 2 + + + + -1 + 1 + + +)"; + + invalid_urdf_ros2_control_component_interface_type_empty_ = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + 2 + 2 + + + ros2_control_components/PositionJoint + + -1 + 1 + + +)"; + + invalid_urdf_ros2_control_parameter_empty_ = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + + 2 + + + ros2_control_components/PositionJoint + -1 + 1 + + +)"; + } + + std::string urdf_xml_head_, urdf_xml_tail_; + std::string valid_urdf_ros2_control_system_one_interface_; + std::string valid_urdf_ros2_control_system_multi_interface_; + std::string valid_urdf_ros2_control_system_robot_with_sensor_; + std::string valid_urdf_ros2_control_system_robot_with_external_sensor_; + std::string valid_urdf_ros2_control_actuator_modular_robot_; + std::string valid_urdf_ros2_control_actuator_modular_robot_sensors_; + std::string valid_urdf_ros2_control_system_muti_joints_transmission_; + std::string valid_urdf_ros2_control_sensor_only_; + std::string valid_urdf_ros2_control_actuator_only_; + + std::string invalid_urdf_ros2_control_invalid_child_; + std::string invalid_urdf_ros2_control_missing_attribute_; + std::string invalid_urdf_ros2_control_component_missing_class_type_; + std::string invalid_urdf_ros2_control_component_class_type_empty_; + std::string invalid_urdf_ros2_control_component_interface_type_empty_; + std::string invalid_urdf_ros2_control_parameter_missing_name_; + std::string invalid_urdf_ros2_control_parameter_empty_; +}; + +using hardware_interface::parse_control_resources_from_urdf; + +TEST_F(TestComponentParser, empty_string_throws_error) +{ + ASSERT_THROW(parse_control_resources_from_urdf(""), std::runtime_error); +} + +TEST_F(TestComponentParser, empty_urdf_throws_error) +{ + const std::string empty_urdf = + ""; + + ASSERT_THROW(parse_control_resources_from_urdf(empty_urdf), std::runtime_error); +} + +TEST_F(TestComponentParser, string_robot_not_root_throws_error) +{ + const std::string broken_xml_string = + R"( + > + )"; + + ASSERT_THROW(parse_control_resources_from_urdf(broken_xml_string), std::runtime_error); +} + +TEST_F(TestComponentParser, invalid_child_throws_error) +{ + const std::string broken_urdf_string = urdf_xml_head_ + invalid_urdf_ros2_control_invalid_child_ + + urdf_xml_tail_; + + ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); +} + +TEST_F(TestComponentParser, component_missing_class_type_throws_error) +{ + const std::string broken_urdf_string = urdf_xml_head_ + + invalid_urdf_ros2_control_component_missing_class_type_ + urdf_xml_tail_; + + ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); +} + +TEST_F(TestComponentParser, missing_attribute_throws_error) +{ + const std::string broken_urdf_string = urdf_xml_head_ + + invalid_urdf_ros2_control_missing_attribute_ + urdf_xml_tail_; + + ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); +} + +TEST_F(TestComponentParser, parameter_missing_name_throws_error) +{ + const std::string broken_urdf_string = urdf_xml_head_ + + invalid_urdf_ros2_control_parameter_missing_name_ + urdf_xml_tail_; + + ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); +} + +TEST_F(TestComponentParser, component_class_type_empty_throws_error) +{ + const std::string broken_urdf_string = urdf_xml_head_ + + invalid_urdf_ros2_control_component_class_type_empty_ + urdf_xml_tail_; + + ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); +} + +TEST_F(TestComponentParser, component_interface_type_empty_throws_error) +{ + const std::string broken_urdf_string = urdf_xml_head_ + + invalid_urdf_ros2_control_component_interface_type_empty_ + urdf_xml_tail_; + + ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); +} + +TEST_F(TestComponentParser, parameter_empty_throws_error) +{ + const std::string broken_urdf_string = urdf_xml_head_ + + invalid_urdf_ros2_control_parameter_empty_ + urdf_xml_tail_; + + ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); +} + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface) +{ + std::string urdf_to_test = urdf_xml_head_ + valid_urdf_ros2_control_system_one_interface_ + + urdf_xml_tail_; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + const auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_Position_Only"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_class_type, + "ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only"); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); + + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + + const auto & joint1_component = hardware_info.joints.at("joint1"); + EXPECT_EQ(joint1_component.name, "joint1"); + EXPECT_EQ(joint1_component.type, "joint"); + EXPECT_EQ(joint1_component.class_type, "ros2_control_components/PositionJoint"); + ASSERT_THAT(joint1_component.parameters, SizeIs(2)); + EXPECT_EQ(joint1_component.parameters.at("max_position_value"), "1"); + + const auto & joint2_component = hardware_info.joints.at("joint2"); + EXPECT_EQ(joint2_component.name, "joint2"); + EXPECT_EQ(joint2_component.type, "joint"); + EXPECT_EQ(joint2_component.class_type, "ros2_control_components/PositionJoint"); + ASSERT_THAT(joint2_component.parameters, SizeIs(2)); + EXPECT_EQ(joint2_component.parameters.at("min_position_value"), "-1"); +} + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface) +{ + std::string urdf_to_test = urdf_xml_head_ + valid_urdf_ros2_control_system_multi_interface_ + + urdf_xml_tail_; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + const auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_MultiInterface"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_class_type, + "ros2_control_demo_hardware/2DOF_System_Hardware_MultiInterface"); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2.2"); + + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + + const auto & joint1_component = hardware_info.joints.at("joint1"); + EXPECT_EQ(joint1_component.name, "joint1"); + EXPECT_EQ(joint1_component.type, "joint"); + EXPECT_EQ(joint1_component.class_type, "ros2_control_components/MultiInterfaceJoint"); + ASSERT_THAT(joint1_component.command_interfaces, SizeIs(3)); + ASSERT_THAT(joint1_component.state_interfaces, SizeIs(3)); + ASSERT_THAT(joint1_component.parameters, SizeIs(6)); + EXPECT_EQ(joint1_component.command_interfaces[0], "position"); + EXPECT_EQ(joint1_component.state_interfaces[1], "velocity"); + EXPECT_EQ(joint1_component.parameters.at("min_effort_value"), "-0.5"); + + const auto & joint2_component = hardware_info.joints.at("joint2"); + EXPECT_EQ(joint2_component.name, "joint2"); + EXPECT_EQ(joint2_component.type, "joint"); + EXPECT_EQ(joint2_component.class_type, "ros2_control_components/MultiInterfaceJoint"); + ASSERT_THAT(joint2_component.command_interfaces, SizeIs(1)); + ASSERT_THAT(joint2_component.state_interfaces, SizeIs(3)); + ASSERT_THAT(joint2_component.parameters, SizeIs(6)); + EXPECT_EQ(joint2_component.state_interfaces[2], "effort"); + EXPECT_EQ(joint2_component.parameters.at("min_position_value"), "-1"); +} + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sensor) +{ + std::string urdf_to_test = urdf_xml_head_ + valid_urdf_ros2_control_system_robot_with_sensor_ + + urdf_xml_tail_; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + const auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_with_Sensor"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_class_type, + "ros2_control_demo_hardware/2DOF_System_Hardware_Sensor"); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); + + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + + const auto & joint1_component = hardware_info.joints.at("joint1"); + EXPECT_EQ(joint1_component.name, "joint1"); + EXPECT_EQ(joint1_component.type, "joint"); + EXPECT_EQ(joint1_component.class_type, "ros2_control_components/PositionJoint"); + ASSERT_THAT(joint1_component.parameters, SizeIs(2)); + EXPECT_EQ(joint1_component.parameters.at("max_position_value"), "1"); + + const auto & joint2_component = hardware_info.joints.at("joint2"); + EXPECT_EQ(joint2_component.name, "joint2"); + EXPECT_EQ(joint2_component.type, "joint"); + EXPECT_EQ(joint2_component.class_type, "ros2_control_components/PositionJoint"); + ASSERT_THAT(joint2_component.parameters, SizeIs(2)); + EXPECT_EQ(joint2_component.parameters.at("min_position_value"), "-1"); + + ASSERT_THAT(hardware_info.sensors, SizeIs(1)); + + const auto & sensor1_component = hardware_info.sensors.at("tcp_fts_sensor"); + EXPECT_EQ(sensor1_component.name, "tcp_fts_sensor"); + EXPECT_EQ(sensor1_component.type, "sensor"); + EXPECT_EQ(sensor1_component.class_type, "ros2_control_components/ForceTorqueSensor"); + ASSERT_THAT(sensor1_component.parameters, SizeIs(3)); + EXPECT_EQ(sensor1_component.parameters.at("frame_id"), "kuka_tcp"); +} + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_external_sensor) +{ + std::string urdf_to_test = urdf_xml_head_ + + valid_urdf_ros2_control_system_robot_with_external_sensor_ + urdf_xml_tail_; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(2)); + const auto hardware1_info = control_hardware.at(0); + + EXPECT_EQ(hardware1_info.name, "2DOF_System_Robot_Position_Only_External_Sensor"); + EXPECT_EQ(hardware1_info.type, "system"); + EXPECT_EQ( + hardware1_info.hardware_class_type, + "ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only"); + ASSERT_THAT(hardware1_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware1_info.hardware_parameters.at("example_param_write_for_sec"), "2"); + + ASSERT_THAT(hardware1_info.joints, SizeIs(2)); + + const auto & joint1_component = hardware1_info.joints.at("joint1"); + EXPECT_EQ(joint1_component.name, "joint1"); + EXPECT_EQ(joint1_component.type, "joint"); + EXPECT_EQ(joint1_component.class_type, "ros2_control_components/PositionJoint"); + ASSERT_THAT(joint1_component.parameters, SizeIs(2)); + EXPECT_EQ(joint1_component.parameters.at("max_position_value"), "1"); + + const auto & joint2_component = hardware1_info.joints.at("joint2"); + EXPECT_EQ(joint2_component.name, "joint2"); + EXPECT_EQ(joint2_component.type, "joint"); + EXPECT_EQ(joint2_component.class_type, "ros2_control_components/PositionJoint"); + ASSERT_THAT(joint2_component.parameters, SizeIs(2)); + EXPECT_EQ(joint2_component.parameters.at("min_position_value"), "-1"); + + ASSERT_THAT(hardware1_info.sensors, SizeIs(0)); + + const auto hardware2_info = control_hardware.at(1); + + EXPECT_EQ(hardware2_info.name, "2DOF_System_Robot_ForceTorqueSensor"); + EXPECT_EQ(hardware2_info.type, "sensor"); + EXPECT_EQ( + hardware2_info.hardware_class_type, + "ros2_control_demo_hardware/2D_Sensor_Force_Torque"); + ASSERT_THAT(hardware2_info.hardware_parameters, SizeIs(1)); + EXPECT_EQ(hardware2_info.hardware_parameters.at("example_param_read_for_sec"), "0.43"); + + ASSERT_THAT(hardware2_info.sensors, SizeIs(1)); + + const auto & sensor1_component = hardware2_info.sensors.at("tcp_fts_sensor"); + EXPECT_EQ(sensor1_component.name, "tcp_fts_sensor"); + EXPECT_EQ(sensor1_component.type, "sensor"); + EXPECT_EQ(sensor1_component.class_type, "ros2_control_components/ForceTorqueSensor"); + ASSERT_THAT(sensor1_component.parameters, SizeIs(3)); + EXPECT_EQ(sensor1_component.parameters.at("frame_id"), "kuka_tcp"); + EXPECT_EQ(sensor1_component.parameters.at("lower_limits"), "-100"); +} + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot) +{ + std::string urdf_to_test = urdf_xml_head_ + valid_urdf_ros2_control_actuator_modular_robot_ + + urdf_xml_tail_; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(2)); + auto hardware1_info = control_hardware.at(0); + + EXPECT_EQ(hardware1_info.name, "2DOF_Modular_Robot_joint1"); + EXPECT_EQ(hardware1_info.type, "actuator"); + EXPECT_EQ( + hardware1_info.hardware_class_type, + "ros2_control_demo_hardware/Position_Actuator_Hadware"); + ASSERT_THAT(hardware1_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware1_info.hardware_parameters.at("example_param_write_for_sec"), "1.23"); + + ASSERT_THAT(hardware1_info.joints, SizeIs(1)); + + { + const auto & joint1_component = hardware1_info.joints.at("joint1"); + EXPECT_EQ(joint1_component.name, "joint1"); + EXPECT_EQ(joint1_component.type, "joint"); + EXPECT_EQ(joint1_component.class_type, "ros2_control_components/PositionJoint"); + ASSERT_THAT(joint1_component.parameters, SizeIs(2)); + EXPECT_EQ(joint1_component.parameters.at("max_position_value"), "1"); + } + const auto hardware2_info = control_hardware.at(1); + + EXPECT_EQ(hardware2_info.name, "2DOF_Modular_Robot_joint2"); + EXPECT_EQ(hardware2_info.type, "actuator"); + EXPECT_EQ( + hardware2_info.hardware_class_type, + "ros2_control_demo_hardware/Position_Actuator_Hadware"); + ASSERT_THAT(hardware2_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware2_info.hardware_parameters.at("example_param_read_for_sec"), "2.12"); + + ASSERT_THAT(hardware2_info.joints, SizeIs(1)); + + { + const auto & joint2_component = hardware2_info.joints.at("joint2"); + EXPECT_EQ(joint2_component.name, "joint2"); + EXPECT_EQ(joint2_component.type, "joint"); + EXPECT_EQ(joint2_component.class_type, "ros2_control_components/PositionJoint"); + ASSERT_THAT(joint2_component.parameters, SizeIs(2)); + EXPECT_EQ(joint2_component.parameters.at("min_position_value"), "-1"); + } +} + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot_with_sensors) +{ + std::string urdf_to_test = urdf_xml_head_ + + valid_urdf_ros2_control_actuator_modular_robot_sensors_ + urdf_xml_tail_; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(4)); + const auto hardware1_info = control_hardware.at(0); + + EXPECT_EQ(hardware1_info.name, "2DOF_Modular_Robot_joint1"); + EXPECT_EQ(hardware1_info.type, "actuator"); + EXPECT_EQ( + hardware1_info.hardware_class_type, + "ros2_control_demo_hardware/Velocity_Actuator_Hadware"); + ASSERT_THAT(hardware1_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware1_info.hardware_parameters.at("example_param_write_for_sec"), "1.23"); + + ASSERT_THAT(hardware1_info.joints, SizeIs(1)); + { + const auto & joint1_component = hardware1_info.joints.at("joint1"); + EXPECT_EQ(joint1_component.name, "joint1"); + EXPECT_EQ(joint1_component.type, "joint"); + EXPECT_EQ(joint1_component.class_type, "ros2_control_components/VelocityJoint"); + ASSERT_THAT(joint1_component.command_interfaces, SizeIs(1)); + EXPECT_EQ(joint1_component.command_interfaces[0], "velocity"); + ASSERT_THAT(joint1_component.parameters, SizeIs(2)); + EXPECT_EQ(joint1_component.parameters.at("max_velocity_value"), "1"); + } + + ASSERT_THAT(hardware1_info.transmissions, SizeIs(1)); + { + const auto & transmission1_component = hardware1_info.transmissions.at("transmission1"); + EXPECT_EQ(transmission1_component.name, "transmission1"); + EXPECT_EQ(transmission1_component.type, "transmission"); + EXPECT_EQ(transmission1_component.class_type, "transmission_interface/SimpleTansmission"); + ASSERT_THAT(transmission1_component.parameters, SizeIs(1)); + EXPECT_EQ(transmission1_component.parameters.at("joint_to_actuator"), "${1024/PI}"); + } + + const auto hardware2_info = control_hardware.at(1); + + EXPECT_EQ(hardware2_info.name, "2DOF_Modular_Robot_joint2"); + EXPECT_EQ(hardware2_info.type, "actuator"); + EXPECT_EQ( + hardware2_info.hardware_class_type, + "ros2_control_demo_hardware/Velocity_Actuator_Hadware"); + ASSERT_THAT(hardware2_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware2_info.hardware_parameters.at("example_param_read_for_sec"), "3"); + + ASSERT_THAT(hardware2_info.joints, SizeIs(1)); + { + const auto & joint2_component = hardware2_info.joints.at("joint2"); + EXPECT_EQ(joint2_component.name, "joint2"); + EXPECT_EQ(joint2_component.type, "joint"); + EXPECT_EQ(joint2_component.class_type, "ros2_control_components/VelocityJoint"); + ASSERT_THAT(joint2_component.command_interfaces, SizeIs(1)); + EXPECT_EQ(joint2_component.command_interfaces[0], "velocity"); + ASSERT_THAT(joint2_component.parameters, SizeIs(2)); + EXPECT_EQ(joint2_component.parameters.at("min_velocity_value"), "-1"); + } + + const auto hardware3_info = control_hardware.at(2); + + EXPECT_EQ(hardware3_info.name, "2DOF_System_Robot_Position_Sensor_joint1"); + EXPECT_EQ(hardware3_info.type, "sensor"); + EXPECT_EQ( + hardware3_info.hardware_class_type, + "ros2_control_demo_hardware/Position_Sensor_Hardware"); + ASSERT_THAT(hardware3_info.hardware_parameters, SizeIs(1)); + EXPECT_EQ(hardware3_info.hardware_parameters.at("example_param_read_for_sec"), "2"); + + ASSERT_THAT(hardware3_info.joints, SizeIs(1)); + { + const auto & joint1_component = hardware3_info.joints.at("joint1"); + EXPECT_EQ(joint1_component.name, "joint1"); + EXPECT_EQ(joint1_component.type, "joint"); + EXPECT_EQ(joint1_component.class_type, "ros2_control_components/PositionJoint"); + ASSERT_THAT(joint1_component.command_interfaces, SizeIs(0)); + ASSERT_THAT(joint1_component.state_interfaces, SizeIs(1)); + EXPECT_EQ(joint1_component.state_interfaces[0], "position"); + ASSERT_THAT(joint1_component.parameters, SizeIs(2)); + EXPECT_EQ(joint1_component.parameters.at("max_position_value"), "${PI}"); + EXPECT_EQ(joint1_component.parameters.at("min_position_value"), "${-PI}"); + } + + ASSERT_THAT(hardware3_info.sensors, SizeIs(0)); + + const auto hardware4_info = control_hardware.at(3); + + EXPECT_EQ(hardware4_info.name, "2DOF_System_Robot_Position_Sensor_joint2"); + EXPECT_EQ(hardware4_info.type, "sensor"); + EXPECT_EQ( + hardware4_info.hardware_class_type, + "ros2_control_demo_hardware/Position_Sensor_Hardware"); + ASSERT_THAT(hardware4_info.hardware_parameters, SizeIs(1)); + EXPECT_EQ(hardware4_info.hardware_parameters.at("example_param_read_for_sec"), "2"); + + ASSERT_THAT(hardware4_info.joints, SizeIs(1)); + { + const auto & joint2_component = hardware4_info.joints.at("joint2"); + EXPECT_EQ(joint2_component.name, "joint2"); + EXPECT_EQ(joint2_component.type, "joint"); + EXPECT_EQ(joint2_component.class_type, "ros2_control_components/PositionJoint"); + ASSERT_THAT(joint2_component.command_interfaces, SizeIs(0)); + ASSERT_THAT(joint2_component.state_interfaces, SizeIs(1)); + EXPECT_EQ(joint2_component.state_interfaces[0], "position"); + ASSERT_THAT(joint2_component.parameters, SizeIs(2)); + EXPECT_EQ(joint2_component.parameters.at("max_position_value"), "${PI}"); + EXPECT_EQ(joint2_component.parameters.at("min_position_value"), "${-PI}"); + } + + ASSERT_THAT(hardware4_info.sensors, SizeIs(0)); +} + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_transmission) +{ + std::string urdf_to_test = urdf_xml_head_ + + valid_urdf_ros2_control_system_muti_joints_transmission_ + urdf_xml_tail_; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.at(0); + + EXPECT_EQ(hardware_info.name, "2DOF_Modular_Robot_Wrist"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_class_type, + "ros2_control_demo_hardware/Actuator_Hadware_MultiDOF"); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23"); + + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + + const auto & joint1_component = hardware_info.joints.at("joint1"); + EXPECT_EQ(joint1_component.name, "joint1"); + EXPECT_EQ(joint1_component.type, "joint"); + EXPECT_EQ(joint1_component.class_type, "ros2_control_components/PositionJoint"); + ASSERT_THAT(joint1_component.parameters, SizeIs(2)); + EXPECT_EQ(joint1_component.parameters.at("max_position_value"), "1"); + + const auto & joint2_component = hardware_info.joints.at("joint2"); + EXPECT_EQ(joint2_component.name, "joint2"); + EXPECT_EQ(joint2_component.type, "joint"); + EXPECT_EQ(joint2_component.class_type, "ros2_control_components/PositionJoint"); + ASSERT_THAT(joint2_component.parameters, SizeIs(2)); + EXPECT_EQ(joint2_component.parameters.at("min_position_value"), "-1"); + + ASSERT_THAT(hardware_info.transmissions, SizeIs(1)); + const auto & transmission1_component = hardware_info.transmissions["transmission1"]; + EXPECT_EQ(transmission1_component.name, "transmission1"); + EXPECT_EQ(transmission1_component.type, "transmission"); + EXPECT_EQ( + transmission1_component.class_type, + "transmission_interface/SomeComplex_2x2_Transmission"); + ASSERT_THAT(transmission1_component.parameters, SizeIs(6)); + EXPECT_EQ(transmission1_component.parameters.at("joint1_output1"), "1.5"); + EXPECT_EQ(transmission1_component.parameters.at("joint1_output2"), "3.2"); + EXPECT_EQ(transmission1_component.parameters.at("joint2_output1"), "3.1"); + EXPECT_EQ(transmission1_component.parameters.at("joint2_output2"), "1.4"); +} + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only) +{ + std::string urdf_to_test = urdf_xml_head_ + valid_urdf_ros2_control_sensor_only_ + urdf_xml_tail_; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.at(0); + + EXPECT_EQ(hardware_info.name, "Camera_with_IMU"); + EXPECT_EQ(hardware_info.type, "sensor"); + EXPECT_EQ(hardware_info.hardware_class_type, "ros2_control_demo_hardware/CameraWithIMU_Sensor"); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); + + ASSERT_THAT(hardware_info.sensors, SizeIs(2)); + + const auto & sensor1_component = hardware_info.sensors.at("sensor1"); + EXPECT_EQ(sensor1_component.name, "sensor1"); + EXPECT_EQ(sensor1_component.type, "sensor"); + EXPECT_EQ(sensor1_component.class_type, "ros2_control_components/IMUSensor"); + ASSERT_THAT(sensor1_component.state_interfaces, SizeIs(2)); + EXPECT_EQ(sensor1_component.state_interfaces[0], "velocity"); + EXPECT_EQ(sensor1_component.state_interfaces[1], "acceleration"); + ASSERT_THAT(sensor1_component.parameters, SizeIs(4)); + EXPECT_EQ(sensor1_component.parameters.at("min_acceleration_value"), "-10"); + EXPECT_EQ(sensor1_component.parameters.at("max_velocity_value"), "23"); + + const auto & sensor2_component = hardware_info.sensors.at("sensor2"); + EXPECT_EQ(sensor2_component.name, "sensor2"); + EXPECT_EQ(sensor2_component.type, "sensor"); + EXPECT_EQ(sensor2_component.class_type, "ros2_control_components/2DImageSensor"); + ASSERT_THAT(sensor2_component.state_interfaces, SizeIs(1)); + EXPECT_EQ(sensor2_component.state_interfaces[0], "image"); + ASSERT_THAT(sensor2_component.parameters, SizeIs(2)); + EXPECT_EQ(sensor2_component.parameters.at("min_image_value"), "0"); + EXPECT_EQ(sensor2_component.parameters.at("max_image_value"), "255"); +} + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) +{ + std::string urdf_to_test = urdf_xml_head_ + + valid_urdf_ros2_control_actuator_only_ + urdf_xml_tail_; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.at(0); + + EXPECT_EQ(hardware_info.name, "2DOF_Modular_Robot_joint1"); + EXPECT_EQ(hardware_info.type, "actuator"); + EXPECT_EQ( + hardware_info.hardware_class_type, + "ros2_control_demo_hardware/Velocity_Actuator_Hadware"); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.13"); + + ASSERT_THAT(hardware_info.joints, SizeIs(1)); + + const auto & joint1_component = hardware_info.joints.at("joint1"); + EXPECT_EQ(joint1_component.name, "joint1"); + EXPECT_EQ(joint1_component.type, "joint"); + EXPECT_EQ(joint1_component.class_type, "ros2_control_components/VelocityJoint"); + ASSERT_THAT(joint1_component.parameters, SizeIs(2)); + EXPECT_EQ(joint1_component.parameters.at("max_velocity_value"), "1"); + + ASSERT_THAT(hardware_info.transmissions, SizeIs(1)); + const auto & transmission1_component = hardware_info.transmissions["transmission1"]; + EXPECT_EQ(transmission1_component.name, "transmission1"); + EXPECT_EQ(transmission1_component.type, "transmission"); + EXPECT_EQ( + transmission1_component.class_type, + "transmission_interface/RotationToLinearTansmission"); + ASSERT_THAT(transmission1_component.parameters, SizeIs(1)); + EXPECT_EQ(transmission1_component.parameters.at("joint_to_actuator"), "${1024/PI}"); +} diff --git a/ros2_control/package.xml b/ros2_control/package.xml index e4673a0bbe..bb4b9953e7 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -14,6 +14,7 @@ controller_interface controller_manager hardware_interface + ros2_control_components transmission_interface test_robot_hardware diff --git a/ros2_control_components/CMakeLists.txt b/ros2_control_components/CMakeLists.txt new file mode 100644 index 0000000000..5cacfa6dd7 --- /dev/null +++ b/ros2_control_components/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.5) +project(ros2_control_components) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) + +add_library( + ${PROJECT_NAME} + SHARED + # joints + src/position_joint.cpp + # sensors +) +ament_target_dependencies( + ${PROJECT_NAME} + hardware_interface + pluginlib +) + +pluginlib_export_plugin_description_file( + ros2_control_components ros2_control_components_plugins.xml) + +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gmock REQUIRED) + + ament_add_gmock(test_joints test/test_joints.cpp) + ament_target_dependencies(test_joints hardware_interface pluginlib) +endif() + +ament_export_libraries( + ros2_control_components +) +ament_export_dependencies( + control_components + hardware_interface + pluginlib +) +ament_package() diff --git a/ros2_control_components/package.xml b/ros2_control_components/package.xml new file mode 100644 index 0000000000..ec15267d55 --- /dev/null +++ b/ros2_control_components/package.xml @@ -0,0 +1,20 @@ + + + + ros2_control_components + 0.0.0 + The package implements control components, i.e., joints and sensors, the logical building blocks of ros2_control system. + + Denis Štogl + Apache License 2.0 + + hardware_interface + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2_control_components/ros2_control_components_plugins.xml b/ros2_control_components/ros2_control_components_plugins.xml new file mode 100644 index 0000000000..ccf85bfc0a --- /dev/null +++ b/ros2_control_components/ros2_control_components_plugins.xml @@ -0,0 +1,7 @@ + + + + The position joint provides functionality of position-only joint with minimum and maximum limits. + + + diff --git a/ros2_control_components/src/position_joint.cpp b/ros2_control_components/src/position_joint.cpp new file mode 100644 index 0000000000..a87700e161 --- /dev/null +++ b/ros2_control_components/src/position_joint.cpp @@ -0,0 +1,104 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "hardware_interface/joint.hpp" +#include "hardware_interface/helpers/component_lists_management.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using namespace hardware_interface; + +namespace ros2_control_components +{ + +class PositionJoint : public Joint +{ +public: + return_type configure(const ComponentInfo & joint_info) + { + if (Joint::configure(joint_info) != return_type::OK) { + return return_type::ERROR; + } + + // has to provide exactly one command interface if defined + if (info_.command_interfaces.size() > 1 || info_.state_interfaces.size() > 1) { + return return_type::COMPONENT_TOO_MANY_INTERFACES; + } + + if (info_.command_interfaces.size() == 1 && + !info_.command_interfaces[0].compare(HW_IF_POSITION)) { + return return_type::COMPONENT_WRONG_INTERFACE; + } + + if (info_.command_interfaces.size() == 1 && + !info_.command_interfaces[0].compare(HW_IF_POSITION)) { + return return_type::COMPONENT_WRONG_INTERFACE; + } + + // set default values is not interface defined in URDF. + // Set state interface to default only if command interface is also not defined, otherwise + // return error code. + if (info_.command_interfaces.size() == 0) { + info_.command_interfaces = {HW_IF_POSITION}; + commands_.resize(1); + if (info_.state_interfaces.size() == 0) { + info_.state_interfaces = {HW_IF_POSITION}; + states_.resize(1); + } else { + return return_type::COMPONENT_ONLY_STATE_DEFINED; + } + } + + auto it = info_.parameters.find("min_position_value"); + if (it == info_.parameters.end()) { + return return_type::COMPONENT_MISSING_PARAMETER; + } + it = info_.parameters.find("max_position_value"); + if (it == info_.parameters.end()) { + return return_type::COMPONENT_MISSING_PARAMETER; + } + lower_limits_.resize(1); + lower_limits_.push_back(stod(info_.parameters["min_position_value"])); + upper_limits_.resize(1); + upper_limits_.push_back(stod(info_.parameters["max_position_value"])); + + return return_type::OK; + } + + return_type set_command( + const std::vector & command, + const std::vector & interfaces) + { + return hardware_interface::helpers::set_internal_values_with_limits( + command, interfaces, info_.command_interfaces, commands_, lower_limits_, upper_limits_); + } + + return_type set_command(const std::vector & command) + { + return hardware_interface::helpers::set_internal_values_with_limits( + command, commands_, lower_limits_, upper_limits_); + } + +private: + std::vector lower_limits_, upper_limits_; +}; + +} // namespace ros2_control_components + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + ros2_control_components::PositionJoint, hardware_interface::Joint) diff --git a/ros2_control_components/test/test_joints.cpp b/ros2_control_components/test/test_joints.cpp new file mode 100644 index 0000000000..de194b49ff --- /dev/null +++ b/ros2_control_components/test/test_joints.cpp @@ -0,0 +1,151 @@ +// Copyright 2020 ros2_control development team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/joint.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_loader.hpp" + +using namespace ::testing; // NOLINT + +using hardware_interface::ComponentInfo; +using hardware_interface::Joint; +using hardware_interface::hardware_interface_status; +using hardware_interface::return_type; + +TEST(PositionJointTest, position_joint_ok_test) +{ + ComponentInfo position_joint_info; + position_joint_info.name = "joint1"; + position_joint_info.parameters["min_position_value"] = "-3.14"; + position_joint_info.parameters["max_position_value"] = "3.14"; + + pluginlib::ClassLoader joint_component_loader( + "ros2_control_components", "hardware_interface::Joint"); + + std::shared_ptr joint = joint_component_loader.createSharedInstance( + "ros2_control_components/PositionJoint"); + + EXPECT_EQ(joint->configure(position_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); + 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 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.clear(); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + input.clear(); + input.push_back(1.2); + EXPECT_EQ(joint->set_command(input, interfaces), return_type::OK); +// +// 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); +// 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); +// +// input.clear(); +// EXPECT_EQ(joint->set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); +// input.push_back(2.1); +// EXPECT_EQ(joint->set_command(input), return_type::OK); +// +// EXPECT_EQ(joint->get_command(output), return_type::OK); +// 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); +// +// output.clear(); +// interfaces.clear(); +// EXPECT_EQ(joint->get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); +// interfaces.push_back(hardware_interface::HW_IF_POSITION); +// EXPECT_EQ(joint->get_state(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_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); +// +// input.clear(); +// EXPECT_EQ(joint->set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); +// input.push_back(2.1); +// EXPECT_EQ(joint->set_state(input), return_type::OK); +// +// EXPECT_EQ(joint->get_state(output), return_type::OK); +// ASSERT_THAT(output, SizeIs(1)); +// 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); +} + +TEST(PositionJointTest, position_joint_failure_test) +{ + pluginlib::ClassLoader joint_component_loader( + "ros2_control_components", "hardware_interface::Joint"); + + std::shared_ptr joint = joint_component_loader.createSharedInstance( + "ros2_control_components/PositionJoint"); + + ComponentInfo position_joint_info; + position_joint_info.name = "joint1"; + position_joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); + position_joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); + + EXPECT_EQ(joint->configure(position_joint_info), return_type::COMPONENT_TOO_MANY_INTERFACES); + + position_joint_info.command_interfaces.clear(); + position_joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); + position_joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); + + EXPECT_EQ(joint->configure(position_joint_info), return_type::COMPONENT_TOO_MANY_INTERFACES); + +}