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; diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 9c996d5aca..e0094b7e01 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -50,7 +50,6 @@ int main(int argc, char ** argv) // A simple way to reject non ros args continue; } - RCLCPP_INFO(rclcpp::get_logger("CM args"), "Adding argument: %s", argv[i]); node_arguments.push_back(argv[i]); } cm_node_options.arguments(node_arguments); diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 4ded911bf4..6fce17a74d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -141,6 +141,7 @@ hardware_interface * 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 349e3ee54d..f62329ee62 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/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 9eaadbb3a6..91bb618c44 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 @@ const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_c return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - 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())) { last_read_cycle_time_ = time; return return_type::OK; @@ -270,9 +269,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) { - 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())) { last_write_cycle_time_ = time; return return_type::OK; diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 2a90260ac6..2788c8f645 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -350,6 +350,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/src/sensor.cpp b/hardware_interface/src/sensor.cpp index fe5db54e01..595bfcbac3 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" @@ -220,9 +221,7 @@ const rclcpp::Time & Sensor::get_last_read_time() const { return last_read_cycle return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - 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())) { last_read_cycle_time_ = time; return return_type::OK; diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index be3a1125be..1151e08aa1 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 @@ const rclcpp::Time & System::get_last_write_time() const { return last_write_cyc return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - 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())) { last_read_cycle_time_ = time; return return_type::OK; @@ -266,9 +265,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) { - 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())) { last_write_cycle_time_ = time; return return_type::OK; diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index fc41b8d0ce..473beceeab 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"( diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index 3daf752b8c..b4ebff94cd 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -98,6 +98,10 @@ def main(self, *, args): with NodeStrategy(args) as node: response = list_controllers(node, args.controller_manager) + if not response.controller: + print("No controllers are currently loaded!") + return 0 + # Structure data as table for nicer output col_width_name = max(len(ctrl.name) for ctrl in response.controller) col_width_type = max(len(ctrl.type) for ctrl in response.controller)