From 5ae5cc3d5477a9c837f60d1524eeca975663ea3e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 28 Mar 2024 20:24:20 +0100 Subject: [PATCH] Component parser: Get mimic information from URDF (#1256) --- controller_manager/doc/userdoc.rst | 17 +- doc/migration/Foxy.rst | 51 ++ doc/migration/Iron.rst | 50 ++ hardware_interface/CMakeLists.txt | 1 + .../doc/hardware_components_userdoc.rst | 50 -- .../doc/hardware_interface_types_userdoc.rst | 2 + .../doc/mock_components_userdoc.rst | 19 +- .../hardware_interface/hardware_info.hpp | 32 +- .../mock_components/generic_system.hpp | 8 - hardware_interface/package.xml | 1 + hardware_interface/src/component_parser.cpp | 122 +++- .../src/mock_components/generic_system.cpp | 34 +- .../mock_components/test_generic_system.cpp | 26 +- .../test/test_component_parser.cpp | 126 ++++ .../test/test_components/test_actuator.cpp | 25 +- .../test/test_components/test_components.xml | 12 +- .../test/test_components/test_sensor.cpp | 4 +- .../test/test_components/test_system.cpp | 40 +- .../test/test_resource_manager.cpp | 42 +- .../test/test_resource_manager.hpp | 2 +- .../components_urdfs.hpp | 4 +- .../ros2_control_test_assets/descriptions.hpp | 602 +++++++++++++++++- transmission_interface/CMakeLists.txt | 4 + transmission_interface/package.xml | 1 + .../differential_transmission_loader_test.cpp | 25 +- ...r_bar_linkage_transmission_loader_test.cpp | 25 +- .../test/simple_transmission_loader_test.cpp | 127 +--- 27 files changed, 1080 insertions(+), 372 deletions(-) create mode 100644 doc/migration/Foxy.rst create mode 100644 doc/migration/Iron.rst diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 46781d461c..c8aa68d774 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -45,6 +45,15 @@ Alternatives to the standard kernel include Though installing a realtime-kernel will definitely get the best results when it comes to low jitter, using a lowlatency kernel can improve things a lot with being really easy to install. +Subscribers +----------- + +~/robot_description [std_msgs::msg::String] + String with the URDF xml, e.g., from ``robot_state_publisher``. + Reloading of the URDF is not supported yet. + All joints defined in the ````-tag have to be present in the URDF. + + Parameters ----------- @@ -74,18 +83,10 @@ update_rate (mandatory; integer) The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controller and writes commands to hardware. - .type Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. -Subscribers ------------ - -robot_description (std_msgs/msg/String) - The URDF string as robot description. - This is usually published by the ``robot_state_publisher`` node. - Handling Multiple Controller Managers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/doc/migration/Foxy.rst b/doc/migration/Foxy.rst new file mode 100644 index 0000000000..a1afeddc70 --- /dev/null +++ b/doc/migration/Foxy.rst @@ -0,0 +1,51 @@ +Foxy to Galactic +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +hardware_interface +************************************** +Between Foxy and Galactic we made substantial changes to the interface of hardware components to enable management of their lifecycle. +The following list shows a set of quick changes to port existing hardware components to Galactic: + +1. Rename ``configure`` to ``on_init`` and change return type to ``CallbackReturn`` + +2. If using BaseInterface as base class then you should remove it. Specifically, change: + + .. code-block:: c++ + + hardware_interface::BaseInterface + + to + + .. code-block:: c++ + + hardware_interface::[Actuator|Sensor|System]Interface + +3. Remove include of headers ``base_interface.hpp`` and ``hardware_interface_status_values.hpp`` + +4. Add include of header ``rclcpp_lifecycle/state.hpp`` although this may not be strictly necessary + +5. replace first three lines in ``on_init`` to + + .. code-block:: c++ + + if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + +6. Change last return of ``on_init`` to ``return CallbackReturn::SUCCESS;`` + +7. Remove all lines with ``status_ = ...`` or ``status::...`` + +8. Rename ``start()`` to ``on_activate(const rclcpp_lifecycle::State & previous_state)`` and + ``stop()`` to ``on_deactivate(const rclcpp_lifecycle::State & previous_state)`` + +9. Change return type of ``on_activate`` and ``on_deactivate`` to ``CallbackReturn`` + +10. Change last return of ``on_activate`` and ``on_deactivate`` to ``return CallbackReturn::SUCCESS;`` + +11. If you have any ``return_type::ERROR`` in ``on_init``, ``on_activate``, or ``in_deactivate`` change to ``CallbackReturn::ERROR`` + +12. If you get link errors with undefined refernences to symbols in ``rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface``, then add + ``rclcpp_lifecyle`` package dependency to ``CMakeLists.txt`` and ``package.xml`` diff --git a/doc/migration/Iron.rst b/doc/migration/Iron.rst new file mode 100644 index 0000000000..21e28fc43f --- /dev/null +++ b/doc/migration/Iron.rst @@ -0,0 +1,50 @@ +Iron to Jazzy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +component parser +***************** +Changes from `(PR #1256) `__ + +* All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise a ``std::runtime_error`` is thrown. This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` tags instead. +* The syntax for mimic joints is changed to the `official URDF specification `__. The parameters within the ``ros2_control`` tag are not supported any more. Instead of + + .. code-block:: xml + + + + + + 0.15 + + + + + + right_finger_joint + 1 + + + + + + + + define your mimic joints directly in the joint definitions: + + .. code-block:: xml + + + + + + + + + + + + + + + + diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index f6a4890cba..4ea7fdc2f7 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rcutils TinyXML2 tinyxml2_vendor + urdf ) find_package(ament_cmake REQUIRED) diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst index 404e90ee03..0a980cad4e 100644 --- a/hardware_interface/doc/hardware_components_userdoc.rst +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -27,53 +27,3 @@ If ``hardware_interface::return_type::ERROR`` is returned from ``read()`` or ``w Error handling follows the `node lifecycle `_. If successful ``CallbackReturn::SUCCESS`` is returned and hardware is again in ``UNCONFIGURED`` state, if any ``ERROR`` or ``FAILURE`` happens the hardware ends in ``FINALIZED`` state and can not be recovered. The only option is to reload the complete plugin, but there is currently no service for this in the Controller Manager. - -Migration from Foxy to newer versions -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Between Foxy and Galactic we made substantial changes to the interface of hardware components to enable management of their lifecycle. -The following list shows a set of quick changes to port existing hardware components to Galactic: - -1. Rename ``configure`` to ``on_init`` and change return type to ``CallbackReturn`` - -2. If using BaseInterface as base class then you should remove it. Specifically, change: - - .. code-block:: c++ - - hardware_interface::BaseInterface - - to - - .. code-block:: c++ - - hardware_interface::[Actuator|Sensor|System]Interface - -3. Remove include of headers ``base_interface.hpp`` and ``hardware_interface_status_values.hpp`` - -4. Add include of header ``rclcpp_lifecycle/state.hpp`` although this may not be strictly necessary - -5. replace first three lines in ``on_init`` to - - .. code-block:: c++ - - if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS) - { - return CallbackReturn::ERROR; - } - - -6. Change last return of ``on_init`` to ``return CallbackReturn::SUCCESS;`` - -7. Remove all lines with ``status_ = ...`` or ``status::...`` - -8. Rename ``start()`` to ``on_activate(const rclcpp_lifecycle::State & previous_state)`` and - ``stop()`` to ``on_deactivate(const rclcpp_lifecycle::State & previous_state)`` - -9. Change return type of ``on_activate`` and ``on_deactivate`` to ``CallbackReturn`` - -10. Change last return of ``on_activate`` and ``on_deactivate`` to ``return CallbackReturn::SUCCESS;`` - -11. If you have any ``return_type::ERROR`` in ``on_init``, ``on_activate``, or ``in_deactivate`` change to ``CallbackReturn::ERROR`` - -12. If you get link errors with undefined refernences to symbols in ``rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface``, then add - ``rclcpp_lifecyle`` package dependency to ``CMakeLists.txt`` and ``package.xml`` diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index cd1e475b20..ecf852cb94 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -14,6 +14,8 @@ Joints ````-tag groups the interfaces associated with the joints of physical robots and actuators. They have command and state interfaces to set the goal values for hardware and read its current state. +All joints defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `. + State interfaces of joints can be published as a ROS topic by means of the :ref:`joint_state_broadcaster ` Sensors diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index c075fdeafc..782d3e01ea 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -19,9 +19,10 @@ For more information about hardware components check :ref:`detailed documentatio Features: - - support for mimic joints + - support for mimic joints, which is parsed from the URDF (see the `URDF wiki `__) - mirroring commands to states with and without offset - fake command interfaces for setting sensor data from an external node (combined with a :ref:`forward controller `) + - fake gpio interfaces for setting sensor data from an external node (combined with a :ref:`forward controller `) Parameters @@ -36,24 +37,16 @@ mock_sensor_commands (optional; boolean; default: false) Creates fake command interfaces for faking sensor measurements with an external command. Those interfaces are usually used by a :ref:`forward controller ` to provide access from ROS-world. +mock_gpio_commands (optional; boolean; default: false) + Creates fake command interfaces for faking GPIO states with an external command. + Those interfaces are usually used by a :ref:`forward controller ` to provide access from ROS-world. + position_state_following_offset (optional; double; default: 0.0) Following offset added to the commanded values when mirrored to states. - custom_interface_with_following_offset (optional; string; default: "") Mapping of offsetted commands to a custom interface. - -Per-joint Parameters -,,,,,,,,,,,,,,,,,,,, - -mimic (optional; string) - Defined name of the joint to mimic. This is often used concept with parallel grippers. Example: ``joint1``. - - -multiplier (optional; double; default: 1; used if mimic joint is defined) - Multiplier of values for mimicking joint defined in ``mimic`` parameter. Example: ``-2``. - Per-interface Parameters ,,,,,,,,,,,,,,,,,,,,,,,, diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index eb6b63cfc3..97abc00438 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -44,6 +44,23 @@ struct InterfaceInfo int size; }; +/// @brief This structure stores information about a joint that is mimicking another joint +struct MimicJoint +{ + std::size_t joint_index; + std::size_t mimicked_joint_index; + double multiplier = 1.0; + double offset = 0.0; +}; + +/// @brief This enum is used to store the mimic attribute of a joint +enum class MimicAttribute +{ + NOT_SET, + TRUE, + FALSE +}; + /** * This structure stores information about components defined for a specific hardware * in robot's URDF. @@ -55,6 +72,9 @@ struct ComponentInfo /// Type of the component: sensor, joint, or GPIO. std::string type; + /// Hold the value of the mimic attribute if given, NOT_SET otherwise + MimicAttribute is_mimic = MimicAttribute::NOT_SET; + /** * Name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints and GPIOs. @@ -116,22 +136,26 @@ struct HardwareInfo /// (Optional) Key-value pairs for hardware parameters. std::unordered_map hardware_parameters; /** - * Map of joints provided by the hardware where the key is the joint name. + * Vector of joints provided by the hardware. * Required for Actuator and System Hardware. */ std::vector joints; /** - * Map of sensors provided by the hardware where the key is the joint or link name. + * Vector of mimic joints. + */ + std::vector mimic_joints; + /** + * Vector of sensors provided by the hardware. * Required for Sensor and optional for System Hardware. */ std::vector sensors; /** - * Map of GPIO provided by the hardware where the key is a descriptive name of the GPIO. + * Vector of GPIOs provided by the hardware. * Optional for any hardware components. */ std::vector gpios; /** - * Map of transmissions to calculate ration between joints and physical actuators. + * Vector of transmissions to calculate ratio between joints and physical actuators. * Optional for Actuator and System Hardware. */ std::vector transmissions; diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index e9b38de65d..fbb8547ab1 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -72,14 +72,6 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; - struct MimicJoint - { - std::size_t joint_index; - std::size_t mimicked_joint_index; - double multiplier = 1.0; - }; - std::vector mimic_joints_; - /// The size of this vector is (standard_interfaces_.size() x nr_joints) std::vector> joint_commands_; std::vector> joint_states_; diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 73b5f4af7f..41daac50cd 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -16,6 +16,7 @@ rclcpp_lifecycle rcpputils tinyxml2_vendor + urdf rcutils rcutils diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 4f67d3e8b6..0841265e81 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -21,6 +21,8 @@ #include #include +#include "urdf/model.h" + #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/lexical_casts.hpp" @@ -42,6 +44,7 @@ constexpr const auto kStateInterfaceTag = "state_interface"; constexpr const auto kMinTag = "min"; constexpr const auto kMaxTag = "max"; constexpr const auto kInitialValueTag = "initial_value"; +constexpr const auto kMimicAttribute = "mimic"; constexpr const auto kDataTypeAttribute = "data_type"; constexpr const auto kSizeAttribute = "size"; constexpr const auto kNameAttribute = "name"; @@ -315,6 +318,21 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it component.type = component_it->Name(); component.name = get_attribute_value(component_it, kNameAttribute, component.type); + if (std::string(kJointTag) == component.type) + { + try + { + component.is_mimic = parse_bool(get_attribute_value(component_it, kMimicAttribute, kJointTag)) + ? MimicAttribute::TRUE + : MimicAttribute::FALSE; + } + catch (const std::runtime_error & e) + { + // mimic attribute not set + component.is_mimic = MimicAttribute::NOT_SET; + } + } + // Parse all command interfaces const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag); while (command_interfaces_it) @@ -347,7 +365,7 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it * and the interface may be an array of a fixed size of the data type. * * \param[in] component_it pointer to the iterator where component - * info should befound + * info should be found * \throws std::runtime_error if a required component attribute or tag is not found. */ ComponentInfo parse_complex_component_from_xml(const tinyxml2::XMLElement * component_it) @@ -530,7 +548,7 @@ HardwareInfo parse_resource_from_xml( 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())) + if (std::string(kHardwareTag) == ros2_control_child_it->Name()) { const auto * type_it = ros2_control_child_it->FirstChildElement(kPluginNameTag); hardware.hardware_plugin_name = @@ -541,19 +559,19 @@ HardwareInfo parse_resource_from_xml( hardware.hardware_parameters = parse_parameters_from_xml(params_it); } } - else if (!std::string(kJointTag).compare(ros2_control_child_it->Name())) + else if (std::string(kJointTag) == ros2_control_child_it->Name()) { hardware.joints.push_back(parse_component_from_xml(ros2_control_child_it)); } - else if (!std::string(kSensorTag).compare(ros2_control_child_it->Name())) + else if (std::string(kSensorTag) == ros2_control_child_it->Name()) { hardware.sensors.push_back(parse_component_from_xml(ros2_control_child_it)); } - else if (!std::string(kGPIOTag).compare(ros2_control_child_it->Name())) + else if (std::string(kGPIOTag) == ros2_control_child_it->Name()) { hardware.gpios.push_back(parse_complex_component_from_xml(ros2_control_child_it)); } - else if (!std::string(kTransmissionTag).compare(ros2_control_child_it->Name())) + else if (std::string(kTransmissionTag) == ros2_control_child_it->Name()) { hardware.transmissions.push_back(parse_transmission_from_xml(ros2_control_child_it)); } @@ -593,7 +611,7 @@ std::vector parse_control_resources_from_urdf(const std::string & // Find robot tag const tinyxml2::XMLElement * robot_it = doc.RootElement(); - if (std::string(kRobotTag).compare(robot_it->Name())) + if (std::string(kRobotTag) != robot_it->Name()) { throw std::runtime_error("the robot tag is not root element in URDF"); } @@ -611,6 +629,96 @@ std::vector parse_control_resources_from_urdf(const std::string & ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag); } + // parse full URDF for mimic options + urdf::Model model; + if (!model.initString(urdf)) + { + throw std::runtime_error("Failed to parse URDF file"); + } + for (auto & hw_info : hardware_info) + { + for (auto i = 0u; i < hw_info.joints.size(); ++i) + { + const auto & joint = hw_info.joints.at(i); + + // Search for mimic joints defined in ros2_control tag (deprecated) + // TODO(christophfroehlich) delete deprecated config with ROS-J + if (joint.parameters.find("mimic") != joint.parameters.cend()) + { + std::cerr << "Warning: Mimic joints defined in ros2_control tag are deprecated. " + << "Please define mimic joints in URDF." << std::endl; + const auto mimicked_joint_it = std::find_if( + hw_info.joints.begin(), hw_info.joints.end(), + [&mimicked_joint = + joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info) + { return joint_info.name == mimicked_joint; }); + if (mimicked_joint_it == hw_info.joints.cend()) + { + throw std::runtime_error( + "Mimicked joint '" + joint.parameters.at("mimic") + "' not found"); + } + hardware_interface::MimicJoint mimic_joint; + mimic_joint.joint_index = i; + mimic_joint.multiplier = 1.0; + mimic_joint.offset = 0.0; + mimic_joint.mimicked_joint_index = std::distance(hw_info.joints.begin(), mimicked_joint_it); + auto param_it = joint.parameters.find("multiplier"); + if (param_it != joint.parameters.end()) + { + mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier")); + } + param_it = joint.parameters.find("offset"); + if (param_it != joint.parameters.end()) + { + mimic_joint.offset = hardware_interface::stod(joint.parameters.at("offset")); + } + hw_info.mimic_joints.push_back(mimic_joint); + } + else + { + auto urdf_joint = model.getJoint(joint.name); + if (!urdf_joint) + { + throw std::runtime_error("Joint " + joint.name + " not found in URDF"); + } + if (!urdf_joint->mimic && joint.is_mimic == MimicAttribute::TRUE) + { + throw std::runtime_error( + "Joint '" + joint.name + "' has no mimic information in the URDF."); + } + if (urdf_joint->mimic && joint.is_mimic != MimicAttribute::FALSE) + { + if (joint.command_interfaces.size() > 0) + { + throw std::runtime_error( + "Joint '" + joint.name + + "' has mimic attribute not set to false: Activated mimic joints cannot have command " + "interfaces."); + } + auto find_joint = [&hw_info](const std::string & name) + { + auto it = std::find_if( + hw_info.joints.begin(), hw_info.joints.end(), + [&name](const auto & j) { return j.name == name; }); + if (it == hw_info.joints.end()) + { + throw std::runtime_error( + "Mimic joint '" + name + "' not found in tag"); + } + return std::distance(hw_info.joints.begin(), it); + }; + + MimicJoint mimic_joint; + mimic_joint.joint_index = i; + mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name); + mimic_joint.multiplier = urdf_joint->mimic->multiplier; + mimic_joint.offset = urdf_joint->mimic->offset; + hw_info.mimic_joints.push_back(mimic_joint); + } + } + } + } + return hardware_info; } diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 22d8aa573c..2d8a01a34f 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -17,7 +17,6 @@ #include "mock_components/generic_system.hpp" #include -#include #include #include #include @@ -137,34 +136,6 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } - // Search for mimic joints - for (auto i = 0u; i < info_.joints.size(); ++i) - { - const auto & joint = info_.joints.at(i); - if (joint.parameters.find("mimic") != joint.parameters.cend()) - { - const auto mimicked_joint_it = std::find_if( - info_.joints.begin(), info_.joints.end(), - [&mimicked_joint = - joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info) - { return joint_info.name == mimicked_joint; }); - if (mimicked_joint_it == info_.joints.cend()) - { - throw std::runtime_error( - std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); - } - MimicJoint mimic_joint; - mimic_joint.joint_index = i; - mimic_joint.mimicked_joint_index = std::distance(info_.joints.begin(), mimicked_joint_it); - auto param_it = joint.parameters.find("multiplier"); - if (param_it != joint.parameters.end()) - { - mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier")); - } - mimic_joints_.push_back(mimic_joint); - } - } - // search for non-standard joint interfaces for (const auto & joint : info_.joints) { @@ -594,11 +565,12 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur mirror_command_to_state(joint_states_, joint_commands_, 1); } - for (const auto & mimic_joint : mimic_joints_) + for (const auto & mimic_joint : info_.mimic_joints) { for (auto i = 0u; i < joint_states_.size(); ++i) { joint_states_[i][mimic_joint.joint_index] = + mimic_joint.offset + mimic_joint.multiplier * joint_states_[i][mimic_joint.mimicked_joint_index]; } } @@ -626,13 +598,13 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur mirror_command_to_state(sensor_states_, sensor_mock_commands_); } - // do loopback on all gpio interfaces if (use_mock_gpio_command_interfaces_) { mirror_command_to_state(gpio_states_, gpio_mock_commands_); } else { + // do loopback on all gpio interfaces mirror_command_to_state(gpio_states_, gpio_commands_); } diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 0641cfda57..ac89dc1553 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -142,12 +142,12 @@ class TestGenericSystem : public ::testing::Test 0.2 - + 0.5 - + )"; @@ -251,11 +251,7 @@ class TestGenericSystem : public ::testing::Test - - joint1 - -2 - - + @@ -1207,11 +1203,9 @@ void TestGenericSystem::test_generic_system_with_mimic_joint( EXPECT_TRUE(rm.state_interface_exists("joint2/position")); EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); - ASSERT_EQ(4u, rm.command_interface_keys().size()); + ASSERT_EQ(2u, rm.command_interface_keys().size()); EXPECT_TRUE(rm.command_interface_exists("joint1/position")); EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); - EXPECT_TRUE(rm.command_interface_exists("joint2/position")); - EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); @@ -1261,7 +1255,7 @@ void TestGenericSystem::test_generic_system_with_mimic_joint( TEST_F(TestGenericSystem, hardware_system_2dof_with_mimic_joint) { - auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_mimic_joint_ + + auto urdf = ros2_control_test_assets::urdf_head_mimic + hardware_system_2dof_with_mimic_joint_ + ros2_control_test_assets::urdf_tail; test_generic_system_with_mimic_joint(urdf, "MockHardwareSystem"); @@ -1918,10 +1912,11 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tags) { - auto check_prepare_command_mode_switch = [&](const std::string & urdf) + auto check_prepare_command_mode_switch = + [&]( + const std::string & urdf, const std::string & urdf_head = ros2_control_test_assets::urdf_head) { - TestableResourceManager rm( - ros2_control_test_assets::urdf_head + urdf + ros2_control_test_assets::urdf_tail); + TestableResourceManager rm(urdf_head + urdf + ros2_control_test_assets::urdf_tail); rclcpp_lifecycle::State state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); rm.set_component_state("MockHardwareSystem", state); auto start_interfaces = rm.command_interface_keys(); @@ -1937,7 +1932,8 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_)); ASSERT_TRUE( check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_True_)); - ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_mimic_joint_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + hardware_system_2dof_with_mimic_joint_, ros2_control_test_assets::urdf_head_mimic)); ASSERT_TRUE( check_prepare_command_mode_switch(hardware_system_2dof_standard_interfaces_with_offset_)); ASSERT_TRUE(check_prepare_command_mode_switch( diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index b0c7c5a16d..968d41ed97 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -564,6 +564,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio EXPECT_THAT(hardware_info.gpios[1].state_interfaces, SizeIs(1)); EXPECT_THAT(hardware_info.gpios[1].command_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.gpios[1].state_interfaces[0].name, "vacuum"); + EXPECT_EQ(hardware_info.gpios[1].state_interfaces[0].initial_value, "1.0"); EXPECT_EQ(hardware_info.gpios[1].command_interfaces[0].name, "vacuum"); EXPECT_THAT(hardware_info.transmissions, IsEmpty()); @@ -674,3 +675,128 @@ TEST_F(TestComponentParser, transmission_given_too_many_joints_throws_error) ros2_control_test_assets::urdf_tail; ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } + +TEST_F(TestComponentParser, gripper_mimic_true_valid_config) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); + ASSERT_THAT(hw_info, SizeIs(1)); + ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1)); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 2.0); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0); + EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); + EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); +} + +TEST_F(TestComponentParser, gripper_no_mimic_valid_config) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); + ASSERT_THAT(hw_info, SizeIs(1)); + ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1)); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 2.0); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0); + EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); + EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); +} + +// TODO(christophfroehlich) delete deprecated config test +TEST_F(TestComponentParser, gripper_mimic_deprecated_valid_config) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_deprecated) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); + ASSERT_THAT(hw_info, SizeIs(1)); + ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1)); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 2.0); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0); + EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); + EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); +} + +TEST_F(TestComponentParser, gripper_mimic_deprecated_unknown_joint_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string( + ros2_control_test_assets::gripper_hardware_resources_mimic_deprecated_unknown_joint) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} +// end delete deprecated config test + +TEST_F(TestComponentParser, gripper_mimic_with_unknown_joint_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head_unknown_joint) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, gripper_mimic_true_without_mimic_info_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head_no_mimic) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, gripper_mimic_true_invalid_config_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, gripper_mimic_false_valid_config) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_false_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); + ASSERT_THAT(hw_info, SizeIs(1)); + ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(0)); +} + +/** + * @brief Test that the parser throws an error if the URDF contains a link with no parent. + */ +TEST_F(TestComponentParser, urdf_two_root_links_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head_invalid_two_root_links) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +/** + * @brief Test that the parser throws an error if a joint defined in the ros2_control tag is missing + * in the URDF + */ +TEST_F(TestComponentParser, urdf_incomplete_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head_incomplete) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 41f27e201b..a01ccd879f 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -38,9 +38,11 @@ class TestActuator : public ActuatorInterface * // can only control one joint * if (info_.joints.size() != 1) {return CallbackReturn::ERROR;} * // can only control in position - * if (info_.joints[0].command_interfaces.size() != 1) {return CallbackReturn::ERROR;} + * if (info_.joints[0].command_interfaces.size() != 1) {return + * CallbackReturn::ERROR;} * // can only give feedback state for position and velocity - * if (info_.joints[0].state_interfaces.size() != 2) {return CallbackReturn::ERROR;} + * if (info_.joints[0].state_interfaces.size() != 2) {return + * CallbackReturn::ERROR;} */ return CallbackReturn::SUCCESS; @@ -95,7 +97,8 @@ class TestActuator : public ActuatorInterface // simulate error on read if (velocity_command_ == test_constants::READ_FAIL_VALUE) { - // reset value to get out from error on the next call - simplifies CM tests + // reset value to get out from error on the next call - simplifies CM + // tests velocity_command_ = 0.0; return return_type::ERROR; } @@ -104,10 +107,11 @@ class TestActuator : public ActuatorInterface { return return_type::DEACTIVATE; } - // The next line is for the testing purposes. We need value to be changed to be sure that - // the feedback from hardware to controllers in the chain is working as it should. - // This makes value checks clearer and confirms there is no "state = command" line or some - // other mixture of interfaces somewhere in the test stack. + // The next line is for the testing purposes. We need value to be changed to + // be sure that the feedback from hardware to controllers in the chain is + // working as it should. This makes value checks clearer and confirms there + // is no "state = command" line or some other mixture of interfaces + // somewhere in the test stack. velocity_state_ = velocity_command_ / 2; return return_type::OK; } @@ -117,7 +121,8 @@ class TestActuator : public ActuatorInterface // simulate error on write if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) { - // reset value to get out from error on the next call - simplifies CM tests + // reset value to get out from error on the next call - simplifies CM + // tests velocity_command_ = 0.0; return return_type::ERROR; } @@ -136,7 +141,7 @@ class TestActuator : public ActuatorInterface double max_velocity_command_ = 0.0; }; -class TestUnitilizableActuator : public TestActuator +class TestUninitializableActuator : public TestActuator { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { @@ -147,4 +152,4 @@ class TestUnitilizableActuator : public TestActuator #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::ActuatorInterface) -PLUGINLIB_EXPORT_CLASS(TestUnitilizableActuator, hardware_interface::ActuatorInterface) +PLUGINLIB_EXPORT_CLASS(TestUninitializableActuator, hardware_interface::ActuatorInterface) diff --git a/hardware_interface_testing/test/test_components/test_components.xml b/hardware_interface_testing/test/test_components/test_components.xml index f029d3dd37..e24ee28317 100644 --- a/hardware_interface_testing/test/test_components/test_components.xml +++ b/hardware_interface_testing/test/test_components/test_components.xml @@ -18,21 +18,21 @@ - + - Test Unitilizable Actuator + Test Uninitializable Actuator - + - Test Unitilizable Sensor + Test Uninitializable Sensor - + - Test Unitilizable System + Test Uninitializable System diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 4811244138..2ea36ac5c1 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -55,7 +55,7 @@ class TestSensor : public SensorInterface double velocity_state_ = 0.0; }; -class TestUnitilizableSensor : public TestSensor +class TestUninitializableSensor : public TestSensor { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { @@ -66,4 +66,4 @@ class TestUnitilizableSensor : public TestSensor #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::SensorInterface) -PLUGINLIB_EXPORT_CLASS(TestUnitilizableSensor, hardware_interface::SensorInterface) +PLUGINLIB_EXPORT_CLASS(TestUninitializableSensor, hardware_interface::SensorInterface) diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 6eed347299..20e606ee6d 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -32,22 +32,19 @@ class TestSystem : public SystemInterface std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { - if (info_.joints[i].name != "configuration") - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); - } + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); } - if (info_.joints.size() > 2) + if (info_.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[2].name, info_.joints[2].state_interfaces[0].name, &configuration_state_)); + info_.gpios[0].name, info_.gpios[0].state_interfaces[0].name, &configuration_state_)); } return state_interfaces; @@ -58,22 +55,19 @@ class TestSystem : public SystemInterface std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { - if (info_.joints[i].name != "configuration") - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); - } + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); } // Add max_acceleration command interface command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[0].name, info_.joints[0].command_interfaces[1].name, &max_acceleration_command_)); - if (info_.joints.size() > 2) + if (info_.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[2].name, info_.joints[2].command_interfaces[0].name, &configuration_command_)); + info_.gpios[0].name, info_.gpios[0].command_interfaces[0].name, &configuration_command_)); } return command_interfaces; @@ -84,7 +78,8 @@ class TestSystem : public SystemInterface // simulate error on read if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) { - // reset value to get out from error on the next call - simplifies CM tests + // reset value to get out from error on the next call - simplifies CM + // tests velocity_command_[0] = 0.0; return return_type::ERROR; } @@ -101,7 +96,8 @@ class TestSystem : public SystemInterface // simulate error on write if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) { - // reset value to get out from error on the next call - simplifies CM tests + // reset value to get out from error on the next call - simplifies CM + // tests velocity_command_[0] = 0.0; return return_type::ERROR; } @@ -123,7 +119,7 @@ class TestSystem : public SystemInterface double configuration_command_ = 0.0; }; -class TestUnitilizableSystem : public TestSystem +class TestUninitializableSystem : public TestSystem { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { @@ -134,4 +130,4 @@ class TestUnitilizableSystem : public TestSystem #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::SystemInterface) -PLUGINLIB_EXPORT_CLASS(TestUnitilizableSystem, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(TestUninitializableSystem, hardware_interface::SystemInterface) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 4bb9e0c5fe..c8b077229e 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -44,6 +44,7 @@ using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_PLUGIN_NAME; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE; +using testing::SizeIs; auto configure_components = [](TestableResourceManager & rm, const std::vector & components = {}) @@ -101,22 +102,23 @@ TEST_F(ResourceManagerTest, post_initialization_with_urdf) ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf)); } -TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation) +TEST_F(ResourceManagerTest, test_uninitializable_hardware_validation) { - // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a - // runtime exception is thrown + // If the the hardware can not be initialized and load_urdf tried to validate + // the interfaces a runtime exception is thrown TestableResourceManager rm; ASSERT_THROW( - rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true), + rm.load_urdf(ros2_control_test_assets::minimal_uninitializable_robot_urdf, true), std::runtime_error); } -TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) +TEST_F(ResourceManagerTest, test_uninitializable_hardware_no_validation) { - // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces, - // the interface should not show up + // If the the hardware can not be initialized and load_urdf didn't try to + // validate the interfaces, the interface should not show up TestableResourceManager rm; - EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + EXPECT_NO_THROW( + rm.load_urdf(ros2_control_test_assets::minimal_uninitializable_robot_urdf, false)); // test actuator EXPECT_FALSE(rm.state_interface_exists("joint1/position")); @@ -150,7 +152,7 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) EXPECT_EQ(1u, rm.system_components_size()); auto state_interface_keys = rm.state_interface_keys(); - ASSERT_EQ(11u, state_interface_keys.size()); + ASSERT_THAT(state_interface_keys, SizeIs(11)); EXPECT_TRUE(rm.state_interface_exists("joint1/position")); EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); EXPECT_TRUE(rm.state_interface_exists("sensor1/velocity")); @@ -158,7 +160,7 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) EXPECT_TRUE(rm.state_interface_exists("joint3/position")); auto command_interface_keys = rm.command_interface_keys(); - ASSERT_EQ(6u, command_interface_keys.size()); + ASSERT_THAT(command_interface_keys, SizeIs(6)); EXPECT_TRUE(rm.command_interface_exists("joint1/position")); EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); @@ -351,8 +353,8 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) EXPECT_EQ(1u, rm.sensor_components_size()); EXPECT_EQ(1u, rm.system_components_size()); - ASSERT_EQ(11u, rm.state_interface_keys().size()); - ASSERT_EQ(6u, rm.command_interface_keys().size()); + ASSERT_THAT(rm.state_interface_keys(), SizeIs(11)); + ASSERT_THAT(rm.command_interface_keys(), SizeIs(6)); hardware_interface::HardwareInfo external_component_hw_info; external_component_hw_info.name = "ExternalComponent"; @@ -361,9 +363,9 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) rm.import_component(std::make_unique(), external_component_hw_info); EXPECT_EQ(2u, rm.actuator_components_size()); - ASSERT_EQ(12u, rm.state_interface_keys().size()); + ASSERT_THAT(rm.state_interface_keys(), SizeIs(12)); EXPECT_TRUE(rm.state_interface_exists("external_joint/external_state_interface")); - ASSERT_EQ(7u, rm.command_interface_keys().size()); + ASSERT_THAT(rm.command_interface_keys(), SizeIs(7)); EXPECT_TRUE(rm.command_interface_exists("external_joint/external_command_interface")); auto status_map = rm.get_components_status(); @@ -874,7 +876,8 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) std::bind(&TestableResourceManager::command_interface_is_claimed, &rm, _1), expected_result); }; - // All resources start as UNCONFIGURED - All interfaces are imported but not available + // All resources start as UNCONFIGURED - All interfaces are imported but not + // available { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, @@ -954,7 +957,8 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) TEST_SYSTEM_HARDWARE_STATE_INTERFACES, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, false); } - // When actuator is activated all state- and command- interfaces become available + // When actuator is activated all state- and command- interfaces become + // available activate_components(rm, {TEST_ACTUATOR_HARDWARE_NAME}); { check_interfaces( @@ -1430,7 +1434,8 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest check_if_interface_available(true, true); } - // read failure for both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + // read failure for both, TEST_ACTUATOR_HARDWARE_NAME and + // TEST_SYSTEM_HARDWARE_NAME claimed_itfs[0].set_value(fail_value); claimed_itfs[1].set_value(fail_value); { @@ -1547,7 +1552,8 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest check_if_interface_available(true, true); } - // deactivate both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + // deactivate both, TEST_ACTUATOR_HARDWARE_NAME and + // TEST_SYSTEM_HARDWARE_NAME claimed_itfs[0].set_value(deactivate_value); claimed_itfs[1].set_value(deactivate_value); { diff --git a/hardware_interface_testing/test/test_resource_manager.hpp b/hardware_interface_testing/test/test_resource_manager.hpp index 46a487f2ef..554dfe2c86 100644 --- a/hardware_interface_testing/test/test_resource_manager.hpp +++ b/hardware_interface_testing/test/test_resource_manager.hpp @@ -48,7 +48,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); - FRIEND_TEST(ResourceManagerTest, test_unitilizable_hardware_no_validation); + FRIEND_TEST(ResourceManagerTest, test_uninitializable_hardware_no_validation); TestableResourceManager() : hardware_interface::ResourceManager() {} diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index a42d39a241..7b2dd46e7a 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -372,7 +372,9 @@ const auto valid_urdf_ros2_control_system_robot_with_gpio = - + + 1.0 + )"; diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index a2f6195c67..308751e0d8 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -113,6 +113,136 @@ const auto urdf_head = + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; +const auto urdf_head_mimic = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -169,18 +299,18 @@ const auto hardware_resources = - + - + )"; -const auto unitilizable_hardware_resources = +const auto uninitializable_hardware_resources = R"( - + - test_unitilizable_actuator + test_uninitializable_actuator @@ -189,9 +319,9 @@ const auto unitilizable_hardware_resources = - + - test_unitilizable_sensor + test_uninitializable_sensor 2 2 @@ -199,9 +329,9 @@ const auto unitilizable_hardware_resources = - + - test_unitilizable_system + test_uninitializable_system 2 2 @@ -218,10 +348,6 @@ const auto unitilizable_hardware_resources = - - - - )"; @@ -372,16 +498,16 @@ const auto diffbot_urdf = - + - - - + + + 1 1 @@ -407,16 +533,16 @@ const auto diffbot_urdf = - + - - - + + + 1 1 @@ -453,10 +579,440 @@ const auto diffbot_urdf = )"; +const auto gripper_urdf_head = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_urdf_head_no_mimic = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_urdf_head_unknown_joint = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_urdf_head_incomplete = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_urdf_head_invalid_two_root_links = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_hardware_resources_no_command_if = + R"( + + + + + + + + + + + + + )"; + +const auto gripper_hardware_resources_mimic_true_no_command_if = + R"( + + + + + + + + + + + + + )"; + +// TODO(christophfroehlich) delete deprecated config test +const auto gripper_hardware_resources_mimic_deprecated = + R"( + + + + + + + + + right_finger_joint + 2 + 1 + + + + + + )"; + +const auto gripper_hardware_resources_mimic_deprecated_unknown_joint = + R"( + + + + + + + + + middle_finger_joint + 1 + + + + + + )"; +// end delete deprecated config test + +const auto gripper_hardware_resources_mimic_true_command_if = + R"( + + + + + + + + + + + + + + )"; + +const auto gripper_hardware_resources_mimic_false_command_if = + R"( + + + + + + + + + + + + + + )"; + const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); -const auto minimal_unitilizable_robot_urdf = - std::string(urdf_head) + std::string(unitilizable_hardware_resources) + std::string(urdf_tail); +const auto minimal_uninitializable_robot_urdf = + std::string(urdf_head) + std::string(uninitializable_hardware_resources) + std::string(urdf_tail); const auto minimal_robot_missing_state_keys_urdf = std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) + diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt index 60d2075956..98dcdcd192 100644 --- a/transmission_interface/CMakeLists.txt +++ b/transmission_interface/CMakeLists.txt @@ -31,6 +31,7 @@ pluginlib_export_plugin_description_file(transmission_interface ros2_control_plu if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) + find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_simple_transmission test/simple_transmission_test.cpp @@ -51,16 +52,19 @@ if(BUILD_TESTING) test/simple_transmission_loader_test.cpp ) target_link_libraries(test_simple_transmission_loader transmission_interface) + ament_target_dependencies(test_simple_transmission_loader ros2_control_test_assets) ament_add_gmock(test_four_bar_linkage_transmission_loader test/four_bar_linkage_transmission_loader_test.cpp ) target_link_libraries(test_four_bar_linkage_transmission_loader transmission_interface) + ament_target_dependencies(test_four_bar_linkage_transmission_loader ros2_control_test_assets) ament_add_gmock(test_differential_transmission_loader test/differential_transmission_loader_test.cpp ) target_link_libraries(test_differential_transmission_loader transmission_interface) + ament_target_dependencies(test_differential_transmission_loader ros2_control_test_assets) ament_add_gmock( test_utils diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 2af89c96e1..2ca9d43928 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -15,6 +15,7 @@ pluginlib ament_cmake_gmock + ros2_control_test_assets ament_cmake diff --git a/transmission_interface/test/differential_transmission_loader_test.cpp b/transmission_interface/test/differential_transmission_loader_test.cpp index 70d0adf2cd..946c4703b5 100644 --- a/transmission_interface/test/differential_transmission_loader_test.cpp +++ b/transmission_interface/test/differential_transmission_loader_test.cpp @@ -22,6 +22,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_loader.hpp" +#include "ros2_control_test_assets/descriptions.hpp" #include "transmission_interface/differential_transmission.hpp" #include "transmission_interface/differential_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" @@ -57,9 +58,7 @@ class TransmissionPluginLoader TEST(DifferentialTransmissionLoaderTest, FullSpec) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -132,9 +131,7 @@ TEST(DifferentialTransmissionLoaderTest, FullSpec) TEST(DifferentialTransmissionLoaderTest, only_mech_red_specified) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -203,9 +200,7 @@ TEST(DifferentialTransmissionLoaderTest, only_mech_red_specified) TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -266,9 +261,7 @@ TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) TEST(DifferentialTransmissionLoaderTest, mechanical_reduction_not_a_number) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -338,9 +331,7 @@ TEST(DifferentialTransmissionLoaderTest, mechanical_reduction_not_a_number) TEST(DifferentialTransmissionLoaderTest, offset_ill_defined) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -413,9 +404,7 @@ TEST(DifferentialTransmissionLoaderTest, offset_ill_defined) TEST(DifferentialTransmissionLoaderTest, mech_red_invalid_value) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( diff --git a/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp index 53b584b7b5..720cad68b4 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp @@ -22,6 +22,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_loader.hpp" +#include "ros2_control_test_assets/descriptions.hpp" #include "transmission_interface/four_bar_linkage_transmission.hpp" #include "transmission_interface/four_bar_linkage_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" @@ -57,9 +58,7 @@ class TransmissionPluginLoader TEST(FourBarLinkageTransmissionLoaderTest, FullSpec) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -133,9 +132,7 @@ TEST(FourBarLinkageTransmissionLoaderTest, FullSpec) TEST(FourBarLinkageTransmissionLoaderTest, only_mech_red_specified) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -205,9 +202,7 @@ TEST(FourBarLinkageTransmissionLoaderTest, only_mech_red_specified) TEST(DifferentialTransmissionLoaderTest, offset_and_mech_red_not_specified) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -269,9 +264,7 @@ TEST(DifferentialTransmissionLoaderTest, offset_and_mech_red_not_specified) TEST(FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -342,9 +335,7 @@ TEST(FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number) TEST(FourBarLinkageTransmissionLoaderTest, offset_ill_defined) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -418,9 +409,7 @@ TEST(FourBarLinkageTransmissionLoaderTest, offset_ill_defined) TEST(FourBarLinkageTransmissionLoaderTest, mech_red_invalid_value) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( diff --git a/transmission_interface/test/simple_transmission_loader_test.cpp b/transmission_interface/test/simple_transmission_loader_test.cpp index 4623d8c409..968f64c6e8 100644 --- a/transmission_interface/test/simple_transmission_loader_test.cpp +++ b/transmission_interface/test/simple_transmission_loader_test.cpp @@ -22,6 +22,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_loader.hpp" +#include "ros2_control_test_assets/descriptions.hpp" #include "transmission_interface/simple_transmission.hpp" #include "transmission_interface/simple_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" @@ -58,106 +59,8 @@ TEST(SimpleTransmissionLoaderTest, FullSpec) { // Parse transmission info - std::string urdf_to_test = - R"( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + R"( ros2_control_demo_hardware/VelocityActuatorHardware @@ -239,9 +142,7 @@ TEST(SimpleTransmissionLoaderTest, FullSpec) TEST(SimpleTransmissionLoaderTest, only_mech_red_specified) { - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -285,9 +186,7 @@ TEST(SimpleTransmissionLoaderTest, only_mech_red_specified) TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) { - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -327,9 +226,7 @@ TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) TEST(SimpleTransmissionLoaderTest, mechanical_reduction_not_a_number) { - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -369,9 +266,7 @@ TEST(SimpleTransmissionLoaderTest, mechanical_reduction_not_a_number) TEST(SimpleTransmissionLoaderTest, offset_ill_defined) { - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -413,11 +308,9 @@ TEST(SimpleTransmissionLoaderTest, offset_ill_defined) TEST(SimpleTransmissionLoaderTest, mech_red_invalid_value) { - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( - + -1 1 @@ -426,7 +319,7 @@ TEST(SimpleTransmissionLoaderTest, mech_red_invalid_value) transmission_interface/SimpleTransmission - + 0