From f743bf401c6ae17fc83928ab2b7296710872703e Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Wed, 28 Aug 2024 19:08:27 +0200 Subject: [PATCH 1/4] [HWItfs] Add key-value-storage to the InterfaceInfo (#1421) * add key-value storage to the IntefaceInfo, parameters can now be defined per Command-/StateInterface --------- Co-authored-by: Daniel Azanov <31107191+muritane@users.noreply.github.com> Co-authored-by: Dr. Denis Co-authored-by: Bence Magyar Co-authored-by: Sai Kishor Kothakota --- doc/release_notes.rst | 1 + .../doc/hardware_interface_types_userdoc.rst | 32 ++++++ .../hardware_interface/hardware_info.hpp | 4 + hardware_interface/src/component_parser.cpp | 7 ++ .../test/test_component_parser.cpp | 102 ++++++++++++++++++ .../components_urdfs.hpp | 66 +++++++++++- 6 files changed, 211 insertions(+), 1 deletion(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 50583b3bf4..097ab208b0 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -104,6 +104,7 @@ hardware_interface * Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) * Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. +* With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file. joint_limits ************ diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index d8338bf7a6..45141e5479 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -9,6 +9,38 @@ The ``ros2_control`` framework provides a set of hardware interface types that c a hardware component for a specific robot or device. The following sections describe the different hardware interface types and their usage. +Overview +***************************** +Hardware in ros2_control is described as URDF and internally parsed and encapsulated as ``HardwareInfo``. +The definition can be found in the `ros2_control repository `_. +You can check the structs defined there to see what attributes are available for each of the xml tags. +A generic example which shows the structure is provided below. More specific examples can be found in the Example part below. + +.. code:: xml + + + + library_name/ClassName + + value + + + + + -1 + 1 + 0.0 + + 5 + + some_value + other_value + + + + + + Joints ***************************** ````-tag groups the interfaces associated with the joints of physical robots and actuators. diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index b4e47c610e..eea8b6ca8a 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -46,6 +46,10 @@ struct InterfaceInfo int size; /// (Optional) enable or disable the limits for the command interfaces bool enable_limits; + /// (Optional) Key-value pairs of command/stateInterface parameters. This is + /// useful for drivers that operate on protocols like modbus, where each + /// interface needs own address(register), datatype, etc. + std::unordered_map parameters; }; /// @brief This structure stores information about a joint that is mimicking another joint diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 0ef6c084f9..d2ec0f9d53 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -313,6 +313,13 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( interface.data_type = "double"; interface.size = 1; + // Parse parameters + const auto * params_it = interfaces_it->FirstChildElement(kParamTag); + if (params_it) + { + interface.parameters = parse_parameters_from_xml(params_it); + } + return interface; } diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 6a78b0807d..20e098b62e 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -297,6 +297,108 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens } } +TEST_F( + TestComponentParser, + successfully_parse_valid_urdf_system_multi_interface_custom_interface_parameters) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets:: + valid_urdf_ros2_control_system_multi_interface_and_custom_interface_parameters + + ros2_control_test_assets::urdf_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, "RRBotSystemMultiInterface"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, + "ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware"); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); + + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + EXPECT_EQ(hardware_info.joints[0].parameters.size(), 3); + EXPECT_EQ(hardware_info.joints[0].parameters.at("modbus_server_ip"), "1.1.1.1"); + EXPECT_EQ(hardware_info.joints[0].parameters.at("modbus_server_port"), "1234"); + EXPECT_EQ(hardware_info.joints[0].parameters.at("use_persistent_connection"), "true"); + ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(3)); + ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(3)); + // CommandInterfaces of joints + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].initial_value, "1.2"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.size(), 5); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.at("register"), "1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.at("register_size"), "2"); + + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].initial_value, "3.4"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].min, "-1.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].max, "1.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.size(), 5); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.at("register"), "2"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.at("register_size"), "4"); + + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].min, "-0.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].max, "0.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].initial_value, ""); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].parameters.size(), 2); + + // StateInterfaces of joints + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.at("register"), "3"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.at("register_size"), "2"); + + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.at("register"), "4"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.at("register_size"), "4"); + + EXPECT_EQ(hardware_info.joints[0].state_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[2].parameters.size(), 0); + + // Second Joint + EXPECT_EQ(hardware_info.joints[1].name, "joint2"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_EQ(hardware_info.joints[1].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[1].parameters.at("modbus_server_ip"), "192.168.178.123"); + EXPECT_EQ(hardware_info.joints[1].parameters.at("modbus_server_port"), "4321"); + ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); + ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3)); + // CommandInterfaces + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].initial_value, ""); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].min, "-1"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "1"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.size(), 4); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.at("register"), "20"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.at("data_type"), "int32_t"); + // StateInterfaces of joints + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.at("register"), "21"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.at("data_type"), "int32_t"); + + EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].parameters.size(), 0); + + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.at("register"), "21"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.at("data_type"), "int32_t"); +} + TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_external_sensor) { std::string urdf_to_test = 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 b0076b859b..a8c1f02e77 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 @@ -43,7 +43,7 @@ const auto valid_urdf_ros2_control_system_one_interface = )"; -// 2. Industrial Robots with multiple interfaces (cannot be written at the same time) +// 2.1 Industrial Robots with multiple interfaces (cannot be written at the same time) // Note, joint parameters can be any string const auto valid_urdf_ros2_control_system_multi_interface = R"( @@ -84,6 +84,70 @@ const auto valid_urdf_ros2_control_system_multi_interface = )"; +// 2.2 Industrial Robots with multiple interfaces (cannot be written at the same time) +// Additionally some of the Command-/StateInterfaces have parameters defined per interface +const auto valid_urdf_ros2_control_system_multi_interface_and_custom_interface_parameters = + R"( + + + ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware + 2 + 2 + + + + -1 + 1 + 1.2 + 1 + 2 + + + -1.5 + 1.5 + 3.4 + 2 + 4 + + + -0.5 + 0.5 + + + 3 + 2 + + + 4 + 4 + + + 1.1.1.1 + 1234 + true + + + + -1 + 1 + 20 + int32_t + + + 21 + int32_t + + + + 21 + int32_t + + 192.168.178.123 + 4321 + + +)"; + // 3. Industrial Robots with integrated sensor const auto valid_urdf_ros2_control_system_robot_with_sensor = R"( From 045b0602a767a467ca6a9f19d89d3225c0176b97 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 30 Aug 2024 17:49:30 +0200 Subject: [PATCH 2/4] [CM] Handle other exceptions while loading the controller plugin (#1731) --- controller_manager/src/controller_manager.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 275f12f542..e769ea06ce 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -467,13 +467,21 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller = chainable_loader_->createSharedInstance(controller_type); } } - catch (const pluginlib::CreateClassException & e) + catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "Error happened during creation of controller '%s' with type '%s':\n%s", + get_logger(), "Caught exception while loading the controller '%s' of plugin type '%s':\n%s", controller_name.c_str(), controller_type.c_str(), e.what()); return nullptr; } + catch (...) + { + RCLCPP_ERROR( + get_logger(), + "Caught unknown exception while loading the controller '%s' of plugin type '%s'", + controller_name.c_str(), controller_type.c_str()); + throw; + } ControllerSpec controller_spec; controller_spec.c = controller; From 8552dcfbc3934c006d09dd5950a75506a58c45b0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 30 Aug 2024 17:55:42 +0200 Subject: [PATCH 3/4] Fix spam of logs on failed hardware component initialization (#1719) --- .../hardware_interface/lifecycle_helpers.hpp | 30 +++++++++++++++++++ hardware_interface/src/actuator.cpp | 9 ++---- hardware_interface/src/sensor.cpp | 5 ++-- hardware_interface/src/system.cpp | 9 ++---- 4 files changed, 38 insertions(+), 15 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/lifecycle_helpers.hpp diff --git a/hardware_interface/include/hardware_interface/lifecycle_helpers.hpp b/hardware_interface/include/hardware_interface/lifecycle_helpers.hpp new file mode 100644 index 0000000000..1358da3058 --- /dev/null +++ b/hardware_interface/include/hardware_interface/lifecycle_helpers.hpp @@ -0,0 +1,30 @@ +// Copyright 2024 PAL Robotics S.L. +// +// 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__LIFECYCLE_HELPERS_HPP_ +#define HARDWARE_INTERFACE__LIFECYCLE_HELPERS_HPP_ + +#include + +namespace hardware_interface +{ +constexpr bool lifecycleStateThatRequiresNoAction(const lifecycle_msgs::msg::State::_id_type state) +{ + return state == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN || + state == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + state == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED; +} +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__LIFECYCLE_HELPERS_HPP_ diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index d586b463af..13f2090d1f 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -22,6 +22,7 @@ #include "hardware_interface/actuator_interface.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/lifecycle_helpers.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -246,9 +247,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p impl_->get_name().c_str()); return return_type::OK; } - if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { return return_type::OK; } @@ -276,9 +275,7 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & impl_->get_name().c_str()); return return_type::OK; } - if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { return return_type::OK; } diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 2cffa649fd..83b4c9face 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -21,6 +21,7 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/lifecycle_helpers.hpp" #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" @@ -224,9 +225,7 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_name().c_str()); return return_type::OK; } - if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { return return_type::OK; } diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 8f455a7bd5..6e96814c24 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -21,6 +21,7 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/lifecycle_helpers.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" @@ -242,9 +243,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_name().c_str()); return return_type::OK; } - if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { return return_type::OK; } @@ -272,9 +271,7 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe impl_->get_name().c_str()); return return_type::OK; } - if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { return return_type::OK; } From ce58faea325335206ab3be356c64a7868b72a338 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 1 Sep 2024 22:10:35 +0200 Subject: [PATCH 4/4] Bump version of pre-commit hooks (#1737) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6fadbbdace..43bb778260 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -50,13 +50,13 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 24.4.2 + rev: 24.8.0 hooks: - id: black args: ["--line-length=99"] - repo: https://github.com/pycqa/flake8 - rev: 7.1.0 + rev: 7.1.1 hooks: - id: flake8 args: ["--extend-ignore=E501"] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.1 + rev: 0.29.2 hooks: - id: check-github-workflows args: ["--verbose"]