From fdca293ae24bb8d88a56a9c072de371b772b8d40 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 22 Mar 2024 17:21:53 +0100 Subject: [PATCH 01/18] added the group element to HardwareInfo and added its parsing --- .../include/hardware_interface/hardware_info.hpp | 2 ++ hardware_interface/src/component_parser.cpp | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index d94573bf5e..2bd2099e69 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -133,6 +133,8 @@ struct HardwareInfo std::string name; /// Type of the hardware: actuator, sensor or system. std::string type; + /// Hardware group to which the hardware belongs. + std::string group; /// Component is async bool is_async; /// Name of the pluginlib plugin of the hardware that will be loaded. diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 14e016df21..aae029bcef 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -36,6 +36,7 @@ constexpr const auto kROS2ControlTag = "ros2_control"; constexpr const auto kHardwareTag = "hardware"; constexpr const auto kPluginNameTag = "plugin"; constexpr const auto kParamTag = "param"; +constexpr const auto kGroupTag = "group"; constexpr const auto kActuatorTag = "actuator"; constexpr const auto kJointTag = "joint"; constexpr const auto kSensorTag = "sensor"; @@ -578,6 +579,11 @@ HardwareInfo parse_resource_from_xml( const auto * type_it = ros2_control_child_it->FirstChildElement(kPluginNameTag); hardware.hardware_plugin_name = get_text_for_element(type_it, std::string("hardware ") + kPluginNameTag); + const auto * group_it = ros2_control_child_it->FirstChildElement(kGroupTag); + if (group_it) + { + hardware.group = get_text_for_element(type_it, std::string("hardware.") + kGroupTag); + } const auto * params_it = ros2_control_child_it->FirstChildElement(kParamTag); if (params_it) { From bc742fab35f7496fe4b44d0d11e62b45eb6c8694 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 22 Mar 2024 17:37:06 +0100 Subject: [PATCH 02/18] Add group to the hardware component info --- .../include/hardware_interface/hardware_component_info.hpp | 3 +++ hardware_interface/src/resource_manager.cpp | 1 + 2 files changed, 4 insertions(+) diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index 45afebdb34..e7d47bcaa4 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -39,6 +39,9 @@ struct HardwareComponentInfo /// Component "classification": "actuator", "sensor" or "system" std::string type; + /// Component group + std::string group; + /// Component pluginlib plugin name. std::string plugin_name; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 92b6d01519..cf254502ef 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -133,6 +133,7 @@ class ResourceStorage HardwareComponentInfo component_info; component_info.name = hardware_info.name; component_info.type = hardware_info.type; + component_info.group = hardware_info.group; component_info.plugin_name = hardware_info.hardware_plugin_name; component_info.is_async = hardware_info.is_async; From 35f2caea8b56238d41c58b6e33cdabdd370a621f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 22 Mar 2024 17:49:59 +0100 Subject: [PATCH 03/18] Added the get_group_name method to different components and their interface files --- hardware_interface/include/hardware_interface/actuator.hpp | 3 +++ .../include/hardware_interface/actuator_interface.hpp | 6 ++++++ hardware_interface/include/hardware_interface/sensor.hpp | 3 +++ .../include/hardware_interface/sensor_interface.hpp | 6 ++++++ hardware_interface/include/hardware_interface/system.hpp | 3 +++ .../include/hardware_interface/system_interface.hpp | 6 ++++++ hardware_interface/src/actuator.cpp | 2 ++ hardware_interface/src/sensor.cpp | 2 ++ hardware_interface/src/system.cpp | 2 ++ 9 files changed, 33 insertions(+) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 4082863370..b23b913d75 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -83,6 +83,9 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC std::string get_name() const; + HARDWARE_INTERFACE_PUBLIC + std::string get_group_name() const; + HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_state() const; diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index abfd8eb45a..556d7a2047 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -190,6 +190,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual std::string get_name() const { return info_.name; } + /// Get name of the actuator hardware group to which it belongs to. + /** + * \return group name. + */ + virtual std::string get_group_name() const { return info_.group; } + /// Get life-cycle state of the actuator hardware. /** * \return state. diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 5d0677c587..4c267bef77 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -71,6 +71,9 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC std::string get_name() const; + HARDWARE_INTERFACE_PUBLIC + std::string get_group_name() const; + HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_state() const; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 14a59e4588..5a3601afa8 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -129,6 +129,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual std::string get_name() const { return info_.name; } + /// Get name of the actuator hardware group to which it belongs to. + /** + * \return group name. + */ + virtual std::string get_group_name() const { return info_.group; } + /// Get life-cycle state of the actuator hardware. /** * \return state. diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index ece14f814d..fb28929948 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -84,6 +84,9 @@ class System final HARDWARE_INTERFACE_PUBLIC std::string get_name() const; + HARDWARE_INTERFACE_PUBLIC + std::string get_group_name() const; + HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_state() const; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index e5c6f2f542..0a7421531a 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -191,6 +191,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual std::string get_name() const { return info_.name; } + /// Get name of the actuator hardware group to which it belongs to. + /** + * \return group name. + */ + virtual std::string get_group_name() const { return info_.group; } + /// Get life-cycle state of the actuator hardware. /** * \return state. diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 6b58e365dc..b80f76ebf5 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -214,6 +214,8 @@ return_type Actuator::perform_command_mode_switch( std::string Actuator::get_name() const { return impl_->get_name(); } +std::string Actuator::get_group_name() const { return impl_->get_group_name(); } + const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_state(); } return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 2e53e447b9..2da627f892 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -191,6 +191,8 @@ std::vector Sensor::export_state_interfaces() std::string Sensor::get_name() const { return impl_->get_name(); } +std::string Sensor::get_group_name() const { return impl_->get_group_name(); } + const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_state(); } return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index ee942d6581..8e950faa89 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -210,6 +210,8 @@ return_type System::perform_command_mode_switch( std::string System::get_name() const { return impl_->get_name(); } +std::string System::get_group_name() const { return impl_->get_group_name(); } + const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_state(); } return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) From 8197452b84df3df6bebc93e5fccd7bfb5ce0f3f8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 22 Mar 2024 18:24:08 +0100 Subject: [PATCH 04/18] Added a method to update the component group return type state --- hardware_interface/src/resource_manager.cpp | 23 +++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index cf254502ef..e1b9e69f8a 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -138,6 +138,7 @@ class ResourceStorage component_info.is_async = hardware_info.is_async; hardware_info_map_.insert(std::make_pair(component_info.name, component_info)); + hw_group_state_.insert(std::make_pair(component_info.group, return_type:OK)); hardware_used_by_controllers_.insert( std::make_pair(component_info.name, std::vector())); is_loaded = true; @@ -885,6 +886,27 @@ class ResourceStorage } } + /** + * Returns the return type of the hardware component group state, if the return type is other + * than OK, then updates the return type of the group to the respective one + */ + return_type update_hardware_component_group_state(const std::string &group_name, + const return_type &value) + { + // This is for the components that has no configured group + if(group_name.empty()) + { + return value; + } + // If it is anything other than OK, change the return type of the hardware group state + // to the respective return type + if(value > hw_group_state_.at(group_name)) + { + hw_group_state_.at(group_name) = value; + } + return hw_group_state_.at(group_name); + } + // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; @@ -899,6 +921,7 @@ class ResourceStorage std::vector async_systems_; std::unordered_map hardware_info_map_; + std::unordered_map hw_group_state_; /// Mapping between hardware and controllers that are using it (accessing data from it) std::unordered_map> hardware_used_by_controllers_; From d7956f5b25308c0053104bda61e6b0ef3d199514 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 22 Mar 2024 18:25:21 +0100 Subject: [PATCH 05/18] use the hardware grouping logic in the read and write methods --- hardware_interface/src/resource_manager.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e1b9e69f8a..26710d51e7 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1599,6 +1599,8 @@ HardwareReadWriteStatus ResourceManager::read( try { ret_val = component.read(time, period); + const auto component_group = component.get_group_name(); + ret_val = resource_storage_->update_hardware_component_group_state(component_group, ret_val); } catch (const std::exception & e) { @@ -1657,6 +1659,8 @@ HardwareReadWriteStatus ResourceManager::write( try { ret_val = component.write(time, period); + const auto component_group = component.get_group_name(); + ret_val = resource_storage_->update_hardware_component_group_state(component_group, ret_val); } catch (const std::exception & e) { From 26cc638fc5aedff46d5467a16b3acbed28c2dac1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 22 Mar 2024 18:25:49 +0100 Subject: [PATCH 06/18] reset the hardware group state on configure, cleanup and shutdown of the hardware --- hardware_interface/src/resource_manager.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 26710d51e7..48b7baa6d5 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -300,6 +300,10 @@ class ResourceStorage async_component_threads_.at(hardware.get_name()).register_component(&hardware); } } + if(!hardware.get_group_name().empty()) + { + hw_group_state_[hardware.get_group_name()] = return_type::OK; + } return result; } @@ -382,6 +386,10 @@ class ResourceStorage { remove_all_hardware_interfaces_from_available_list(hardware.get_name()); } + if(!hardware.get_group_name().empty()) + { + hw_group_state_[hardware.get_group_name()] = return_type::OK; + } return result; } @@ -416,6 +424,10 @@ class ResourceStorage // deimport_non_movement_command_interfaces(hardware); // deimport_state_interfaces(hardware); // use remove_command_interfaces(hardware); + if(!hardware.get_group_name().empty()) + { + hw_group_state_[hardware.get_group_name()] = return_type::OK; + } } return result; } From 99617898b91af014242ba0cf3379124b6763bfea Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 22 Mar 2024 18:33:35 +0100 Subject: [PATCH 07/18] Fix a typo in the component parser --- hardware_interface/src/component_parser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index aae029bcef..ef585c971b 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -582,7 +582,7 @@ HardwareInfo parse_resource_from_xml( const auto * group_it = ros2_control_child_it->FirstChildElement(kGroupTag); if (group_it) { - hardware.group = get_text_for_element(type_it, std::string("hardware.") + kGroupTag); + hardware.group = get_text_for_element(group_it, std::string("hardware.") + kGroupTag); } const auto * params_it = ros2_control_child_it->FirstChildElement(kParamTag); if (params_it) From 874475f4cf9bea8e09e04538e1747f15d1fbcc4d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 22 Mar 2024 18:35:33 +0100 Subject: [PATCH 08/18] Add the hardware component group tests to the component parser --- hardware_interface/test/test_component_parser.cpp | 12 ++++++++++++ .../ros2_control_test_assets/components_urdfs.hpp | 6 ++++++ 2 files changed, 18 insertions(+) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 6a0c11cf72..bbaf89f578 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -113,6 +113,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface) EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnly"); EXPECT_EQ(hardware_info.type, "system"); + EXPECT_TRUE(hardware_info.group.empty()); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware"); @@ -176,6 +177,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface EXPECT_EQ(hardware_info.name, "RRBotSystemMultiInterface"); EXPECT_EQ(hardware_info.type, "system"); + EXPECT_TRUE(hardware_info.group.empty()); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware"); @@ -238,6 +240,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens EXPECT_EQ(hardware_info.name, "RRBotSystemWithSensor"); EXPECT_EQ(hardware_info.type, "system"); + EXPECT_TRUE(hardware_info.group.empty()); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithSensorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); @@ -306,6 +309,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnlyWithExternalSensor"); EXPECT_EQ(hardware_info.type, "system"); + EXPECT_TRUE(hardware_info.group.empty()); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware"); @@ -372,6 +376,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot auto hardware_info = control_hardware.at(0); EXPECT_EQ(hardware_info.name, "RRBotModularJoint1"); + EXPECT_EQ(hardware_info.group, "Hardware Group"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionActuatorHardware"); @@ -400,6 +405,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot hardware_info = control_hardware.at(1); EXPECT_EQ(hardware_info.name, "RRBotModularJoint2"); + EXPECT_EQ(hardware_info.group, "Hardware Group"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionActuatorHardware"); @@ -445,6 +451,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot auto hardware_info = control_hardware.at(0); EXPECT_EQ(hardware_info.name, "RRBotModularJoint1"); + EXPECT_EQ(hardware_info.group, "Hardware Group 1"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/VelocityActuatorHardware"); @@ -484,6 +491,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot hardware_info = control_hardware.at(1); EXPECT_EQ(hardware_info.name, "RRBotModularJoint2"); + EXPECT_EQ(hardware_info.group, "Hardware Group 2"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/VelocityActuatorHardware"); @@ -523,6 +531,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot hardware_info = control_hardware.at(2); EXPECT_EQ(hardware_info.name, "RRBotModularPositionSensorJoint1"); + EXPECT_EQ(hardware_info.group, "Hardware Group 1"); EXPECT_EQ(hardware_info.type, "sensor"); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionSensorHardware"); @@ -554,6 +563,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot hardware_info = control_hardware.at(3); EXPECT_EQ(hardware_info.name, "RRBotModularPositionSensorJoint2"); + EXPECT_EQ(hardware_info.group, "Hardware Group 2"); EXPECT_EQ(hardware_info.type, "sensor"); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionSensorHardware"); @@ -602,6 +612,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_tr EXPECT_EQ(hardware_info.name, "RRBotModularWrist"); EXPECT_EQ(hardware_info.type, "system"); + EXPECT_TRUE(hardware_info.group.empty()); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/ActuatorHardwareMultiDOF"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); @@ -644,6 +655,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only) EXPECT_EQ(hardware_info.name, "CameraWithIMU"); EXPECT_EQ(hardware_info.type, "sensor"); + EXPECT_TRUE(hardware_info.group.empty()); EXPECT_EQ(hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/CameraWithIMUSensor"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); 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 7b46eda9c0..eba16c1e71 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 @@ -172,6 +172,7 @@ const auto valid_urdf_ros2_control_actuator_modular_robot = ros2_control_demo_hardware/PositionActuatorHardware + Hardware Group 1.23 3 @@ -186,6 +187,7 @@ const auto valid_urdf_ros2_control_actuator_modular_robot = ros2_control_demo_hardware/PositionActuatorHardware + Hardware Group 1.23 3 @@ -206,6 +208,7 @@ const auto valid_urdf_ros2_control_actuator_modular_robot_sensors = ros2_control_demo_hardware/VelocityActuatorHardware + Hardware Group 1 1.23 3 @@ -226,6 +229,7 @@ const auto valid_urdf_ros2_control_actuator_modular_robot_sensors = ros2_control_demo_hardware/VelocityActuatorHardware + Hardware Group 2 1.23 3 @@ -240,6 +244,7 @@ const auto valid_urdf_ros2_control_actuator_modular_robot_sensors = ros2_control_demo_hardware/PositionSensorHardware + Hardware Group 1 2 @@ -249,6 +254,7 @@ const auto valid_urdf_ros2_control_actuator_modular_robot_sensors = ros2_control_demo_hardware/PositionSensorHardware + Hardware Group 2 2 From 9d6e7119fb173d23c567eccf07942fd0b291b5dd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 22 Mar 2024 18:58:53 +0100 Subject: [PATCH 09/18] update the tests of the generic system --- .../test/mock_components/test_generic_system.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index ac89dc1553..3cdc6672f1 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -96,6 +96,7 @@ class TestGenericSystem : public ::testing::Test mock_components/GenericSystem + Hardware Group @@ -121,6 +122,7 @@ class TestGenericSystem : public ::testing::Test mock_components/GenericSystem + Hardware Group @@ -289,6 +291,7 @@ class TestGenericSystem : public ::testing::Test mock_components/GenericSystem + Hardware Group -3 actual_position @@ -351,6 +354,7 @@ class TestGenericSystem : public ::testing::Test mock_components/GenericSystem + Hardware Group 2 2 @@ -814,7 +818,7 @@ void generic_system_functional_test( ASSERT_EQ(0.44, j2v_c.get_value()); // write() does not change values - rm.write(TIME, PERIOD); + ASSERT_TRUE(rm.write(TIME, PERIOD).ok); ASSERT_EQ(3.45, j1p_s.get_value()); ASSERT_EQ(0.0, j1v_s.get_value()); ASSERT_EQ(2.78, j2p_s.get_value()); @@ -825,7 +829,7 @@ void generic_system_functional_test( ASSERT_EQ(0.44, j2v_c.get_value()); // read() mirrors commands + offset to states - rm.read(TIME, PERIOD); + ASSERT_TRUE(rm.read(TIME, PERIOD).ok); ASSERT_EQ(0.11 + offset, j1p_s.get_value()); ASSERT_EQ(0.22, j1v_s.get_value()); ASSERT_EQ(0.33 + offset, j2p_s.get_value()); From 9c8cf01c7b5795bef8561f3f10d1991054f81cdd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 24 Mar 2024 14:57:52 +0100 Subject: [PATCH 10/18] if infinity is set to the command interfaces then return ERROR in mock generic system --- .../src/mock_components/generic_system.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 2d8a01a34f..01283b4ea4 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -459,7 +459,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur return return_type::OK; } - auto mirror_command_to_state = [](auto & states_, auto commands_, size_t start_index = 0) + auto mirror_command_to_state = [](auto & states_, auto commands_, size_t start_index = 0) -> return_type { for (size_t i = start_index; i < states_.size(); ++i) { @@ -469,8 +469,13 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur { states_[i][j] = commands_[i][j]; } + if (std::isinf(commands_[i][j])) + { + return return_type::ERROR; + } } } + return return_type::OK; }; for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) @@ -556,13 +561,9 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position, // velocity, and acceleration interface - if (calculate_dynamics_) - { - mirror_command_to_state(joint_states_, joint_commands_, 3); - } - else + if(mirror_command_to_state(joint_states_, joint_commands_, calculate_dynamics_ ? 3 : 1) != return_type::OK) { - mirror_command_to_state(joint_states_, joint_commands_, 1); + return return_type::ERROR; } for (const auto & mimic_joint : info_.mimic_joints) From c61ebab4612d7eb48973881ff76cce55ff32df34 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 24 Mar 2024 14:59:18 +0100 Subject: [PATCH 11/18] Add testing for the same group hardware error propagation --- .../mock_components/test_generic_system.cpp | 226 ++++++++++++++++++ 1 file changed, 226 insertions(+) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 3cdc6672f1..b7eca5046e 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -583,6 +583,70 @@ class TestGenericSystem : public ::testing::Test )"; + + hardware_system_2dof_standard_interfaces_with_same_hardware_group_ = + R"( + + + mock_components/GenericSystem + Hardware Group + + + + + + 3.45 + + + + + + + mock_components/GenericSystem + Hardware Group + + + + + + 2.78 + + + + +)"; + + hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_ = + R"( + + + mock_components/GenericSystem + Hardware Group 1 + + + + + + 3.45 + + + + + + + mock_components/GenericSystem + Hardware Group 2 + + + + + + 2.78 + + + + +)"; } std::string hardware_system_2dof_; @@ -604,6 +668,8 @@ class TestGenericSystem : public ::testing::Test std::string hardware_system_2dof_standard_interfaces_with_different_control_modes_; std::string valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_; std::string disabled_commands_; + std::string hardware_system_2dof_standard_interfaces_with_same_hardware_group_; + std::string hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_; }; // Forward declaration @@ -861,6 +927,150 @@ void generic_system_functional_test( status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); } +void generic_system_error_group_test( + const std::string & urdf, const std::string component_prefix, + bool validate_same_group) +{ + TestableResourceManager rm(urdf); + const std::string component1 = component_prefix + "1"; + const std::string component2 = component_prefix + "2"; + // check is hardware is configured + auto status_map = rm.get_components_status(); + for(auto component: {component1, component2}) + { + EXPECT_EQ( + status_map[component].state.label(), + hardware_interface::lifecycle_state_names::UNCONFIGURED); + configure_components(rm, {component}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[component].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + activate_components(rm, {component}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[component].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); + } + + // Check initial values + hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface("joint2/position"); + hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); + + // State interfaces without initial value are set to 0 + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + + // set some new values in commands + j1p_c.set_value(0.11); + j1v_c.set_value(0.22); + j2p_c.set_value(0.33); + j2v_c.set_value(0.44); + + // State values should not be changed + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); + + // write() does not change values + ASSERT_TRUE(rm.write(TIME, PERIOD).ok); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); + + // read() mirrors commands to states + ASSERT_TRUE(rm.read(TIME, PERIOD).ok); + ASSERT_EQ(0.11, j1p_s.get_value()); + ASSERT_EQ(0.22, j1v_s.get_value()); + ASSERT_EQ(0.33, j2p_s.get_value()); + ASSERT_EQ(0.44, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); + + // set some new values in commands + j1p_c.set_value(0.55); + j1v_c.set_value(0.66); + j2p_c.set_value(0.77); + j2v_c.set_value(0.88); + + // state values should not be changed + ASSERT_EQ(0.11, j1p_s.get_value()); + ASSERT_EQ(0.22, j1v_s.get_value()); + ASSERT_EQ(0.33, j2p_s.get_value()); + ASSERT_EQ(0.44, j2v_s.get_value()); + ASSERT_EQ(0.55, j1p_c.get_value()); + ASSERT_EQ(0.66, j1v_c.get_value()); + ASSERT_EQ(0.77, j2p_c.get_value()); + ASSERT_EQ(0.88, j2v_c.get_value()); + + // Error testing + j1p_c.set_value(std::numeric_limits::infinity()); + j1v_c.set_value(std::numeric_limits::infinity()); + // read() should now bring error in the first component + auto read_result = rm.read(TIME, PERIOD); + ASSERT_FALSE(read_result.ok); + if(validate_same_group) + { + // If they belong to the same group, show the error in all hardware components of smae group + EXPECT_THAT(read_result.failed_hardware_names, ::testing::ElementsAre(component1, component2)); + } + else + { + // If they don't belong to the same group, show the error in only that hardware component + EXPECT_THAT(read_result.failed_hardware_names, ::testing::ElementsAre(component1)); + } + + // Check initial values + ASSERT_FALSE(rm.state_interface_is_available("joint1/position")); + ASSERT_FALSE(rm.state_interface_is_available("joint1/velocity")); + ASSERT_FALSE(rm.command_interface_is_available("joint1/position")); + ASSERT_FALSE(rm.command_interface_is_available("joint1/velocity")); + + if(validate_same_group) + { + ASSERT_FALSE(rm.state_interface_is_available("joint2/position")); + ASSERT_FALSE(rm.state_interface_is_available("joint2/velocity")); + ASSERT_FALSE(rm.command_interface_is_available("joint2/position")); + ASSERT_FALSE(rm.command_interface_is_available("joint2/velocity")); + } + else + { + ASSERT_TRUE(rm.state_interface_is_available("joint2/position")); + ASSERT_TRUE(rm.state_interface_is_available("joint2/velocity")); + ASSERT_TRUE(rm.command_interface_is_available("joint2/position")); + ASSERT_TRUE(rm.command_interface_is_available("joint2/velocity")); + } + + deactivate_components(rm, {component1, component2}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[component1].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + EXPECT_EQ( + status_map[component2].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); +} + TEST_F(TestGenericSystem, generic_system_2dof_functionality) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_ + @@ -869,6 +1079,22 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality) generic_system_functional_test(urdf, {"MockHardwareSystem"}); } +TEST_F(TestGenericSystem, generic_system_2dof_error_propagation_different_group) +{ + auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_ + + ros2_control_test_assets::urdf_tail; + + generic_system_error_group_test(urdf, {"MockHardwareSystem"}, false); +} + +TEST_F(TestGenericSystem, generic_system_2dof_error_propagation_same_group) +{ + auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_with_same_hardware_group_ + + ros2_control_test_assets::urdf_tail; + + generic_system_error_group_test(urdf, {"MockHardwareSystem"}, true); +} + TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_other_interface_ + From a6da5995f5a9c235a08d5c7f8f4a74883e34f2c8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 24 Mar 2024 15:08:21 +0100 Subject: [PATCH 12/18] added testing for the recoverable phases of error propagation --- .../test/mock_components/test_generic_system.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index b7eca5046e..3b5bbb7a5c 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -1063,6 +1063,16 @@ void generic_system_error_group_test( ASSERT_TRUE(rm.command_interface_is_available("joint2/velocity")); } + // Error should be recoverable only after reactivating the hardware component + j1p_c.set_value(0.0); + j1v_c.set_value(0.0); + ASSERT_FALSE(rm.read(TIME, PERIOD).ok); + + // Now it should be recoverable + deactivate_components(rm, {component1}); + activate_components(rm, {component1}); + ASSERT_TRUE(rm.read(TIME, PERIOD).ok); + deactivate_components(rm, {component1, component2}); status_map = rm.get_components_status(); EXPECT_EQ( From fffcdfdf517805b1f5ce56cf47ffc2ae7d5dd0f8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 24 Mar 2024 20:23:26 +0100 Subject: [PATCH 13/18] Apply formatting changes --- .../src/mock_components/generic_system.cpp | 7 ++++-- hardware_interface/src/resource_manager.cpp | 18 +++++++------- .../mock_components/test_generic_system.cpp | 24 +++++++++---------- 3 files changed, 26 insertions(+), 23 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 01283b4ea4..162c3aa60d 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -459,7 +459,8 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur return return_type::OK; } - auto mirror_command_to_state = [](auto & states_, auto commands_, size_t start_index = 0) -> return_type + auto mirror_command_to_state = + [](auto & states_, auto commands_, size_t start_index = 0) -> return_type { for (size_t i = start_index; i < states_.size(); ++i) { @@ -561,7 +562,9 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position, // velocity, and acceleration interface - if(mirror_command_to_state(joint_states_, joint_commands_, calculate_dynamics_ ? 3 : 1) != return_type::OK) + if ( + mirror_command_to_state(joint_states_, joint_commands_, calculate_dynamics_ ? 3 : 1) != + return_type::OK) { return return_type::ERROR; } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 48b7baa6d5..11f7d17e77 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -300,7 +300,7 @@ class ResourceStorage async_component_threads_.at(hardware.get_name()).register_component(&hardware); } } - if(!hardware.get_group_name().empty()) + if (!hardware.get_group_name().empty()) { hw_group_state_[hardware.get_group_name()] = return_type::OK; } @@ -386,7 +386,7 @@ class ResourceStorage { remove_all_hardware_interfaces_from_available_list(hardware.get_name()); } - if(!hardware.get_group_name().empty()) + if (!hardware.get_group_name().empty()) { hw_group_state_[hardware.get_group_name()] = return_type::OK; } @@ -424,7 +424,7 @@ class ResourceStorage // deimport_non_movement_command_interfaces(hardware); // deimport_state_interfaces(hardware); // use remove_command_interfaces(hardware); - if(!hardware.get_group_name().empty()) + if (!hardware.get_group_name().empty()) { hw_group_state_[hardware.get_group_name()] = return_type::OK; } @@ -899,20 +899,20 @@ class ResourceStorage } /** - * Returns the return type of the hardware component group state, if the return type is other + * Returns the return type of the hardware component group state, if the return type is other * than OK, then updates the return type of the group to the respective one - */ - return_type update_hardware_component_group_state(const std::string &group_name, - const return_type &value) + */ + return_type update_hardware_component_group_state( + const std::string & group_name, const return_type & value) { // This is for the components that has no configured group - if(group_name.empty()) + if (group_name.empty()) { return value; } // If it is anything other than OK, change the return type of the hardware group state // to the respective return type - if(value > hw_group_state_.at(group_name)) + if (value > hw_group_state_.at(group_name)) { hw_group_state_.at(group_name) = value; } diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 3b5bbb7a5c..c7777b3e21 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -928,19 +928,17 @@ void generic_system_functional_test( } void generic_system_error_group_test( - const std::string & urdf, const std::string component_prefix, - bool validate_same_group) + const std::string & urdf, const std::string component_prefix, bool validate_same_group) { TestableResourceManager rm(urdf); const std::string component1 = component_prefix + "1"; const std::string component2 = component_prefix + "2"; // check is hardware is configured auto status_map = rm.get_components_status(); - for(auto component: {component1, component2}) + for (auto component : {component1, component2}) { EXPECT_EQ( - status_map[component].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); + status_map[component].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); configure_components(rm, {component}); status_map = rm.get_components_status(); EXPECT_EQ( @@ -1031,9 +1029,9 @@ void generic_system_error_group_test( // read() should now bring error in the first component auto read_result = rm.read(TIME, PERIOD); ASSERT_FALSE(read_result.ok); - if(validate_same_group) + if (validate_same_group) { - // If they belong to the same group, show the error in all hardware components of smae group + // If they belong to the same group, show the error in all hardware components of same group EXPECT_THAT(read_result.failed_hardware_names, ::testing::ElementsAre(component1, component2)); } else @@ -1048,7 +1046,7 @@ void generic_system_error_group_test( ASSERT_FALSE(rm.command_interface_is_available("joint1/position")); ASSERT_FALSE(rm.command_interface_is_available("joint1/velocity")); - if(validate_same_group) + if (validate_same_group) { ASSERT_FALSE(rm.state_interface_is_available("joint2/position")); ASSERT_FALSE(rm.state_interface_is_available("joint2/velocity")); @@ -1060,10 +1058,10 @@ void generic_system_error_group_test( ASSERT_TRUE(rm.state_interface_is_available("joint2/position")); ASSERT_TRUE(rm.state_interface_is_available("joint2/velocity")); ASSERT_TRUE(rm.command_interface_is_available("joint2/position")); - ASSERT_TRUE(rm.command_interface_is_available("joint2/velocity")); + ASSERT_TRUE(rm.command_interface_is_available("joint2/velocity")); } - // Error should be recoverable only after reactivating the hardware component + // Error should be recoverable only after reactivating the hardware component j1p_c.set_value(0.0); j1v_c.set_value(0.0); ASSERT_FALSE(rm.read(TIME, PERIOD).ok); @@ -1091,7 +1089,8 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality) TEST_F(TestGenericSystem, generic_system_2dof_error_propagation_different_group) { - auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_ + + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_ + ros2_control_test_assets::urdf_tail; generic_system_error_group_test(urdf, {"MockHardwareSystem"}, false); @@ -1099,7 +1098,8 @@ TEST_F(TestGenericSystem, generic_system_2dof_error_propagation_different_group) TEST_F(TestGenericSystem, generic_system_2dof_error_propagation_same_group) { - auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_with_same_hardware_group_ + + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_2dof_standard_interfaces_with_same_hardware_group_ + ros2_control_test_assets::urdf_tail; generic_system_error_group_test(urdf, {"MockHardwareSystem"}, true); From f5f20c81838b058da858cf4c8f1c6bf4aaf4878b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 24 Mar 2024 20:26:04 +0100 Subject: [PATCH 14/18] Add documentation on hardware grouping --- hardware_interface/doc/hardware_interface_types_userdoc.rst | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index ecf852cb94..5480f9bb2f 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -41,6 +41,12 @@ The ```` tag can be used as a child of all three types of hardware compone Because ports implemented as ````-tag are typically very application-specific, there exists no generic publisher within the ros2_control framework. A custom gpio-controller has to be implemented for each application. As an example, see :ref:`the GPIO controller example ` as part of the demo repository. +Hardware Groups +***************************** +Hardware Component Groups serve as a critical organizational mechanism within complex systems, facilitating error handling and fault tolerance. By grouping related hardware components together, such as actuators within a manipulator, users can establish a unified framework for error detection and response. + +Hardware Component Groups play a vital role in propagating errors across interconnected hardware components. For instance, in a manipulator system, grouping actuators together allows for error propagation. If one actuator fails within the group, the error can propagate to the other actuators, signaling a potential issue across the system. By default, the actuator errors are isolated to their own hardware component, allowing the rest to continue operation unaffected. In the provided ROS2 control configuration, the ```` tag within each ```` block signifies the grouping of hardware components, enabling error propagation mechanisms within the system. + Examples ***************************** The following examples show how to use the different hardware interface types in a ``ros2_control`` URDF. From 65664c32ce2d715bf2d729ae29ecfe387c0d3072 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 24 Mar 2024 22:37:33 +0100 Subject: [PATCH 15/18] Added hardware component grouping example --- .../doc/hardware_interface_types_userdoc.rst | 63 +++++++++++++++++++ 1 file changed, 63 insertions(+) diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index 5480f9bb2f..478742bc15 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -158,3 +158,66 @@ They can be combined together within the different hardware component types (sys + +4. Robot with multiple hardware components belonging to same group : ``Group1`` + + - RRBot System 1 and 2 + - Digital: Total 4 inputs and 2 outputs + - Analog: Total 2 inputs and 1 output + - Vacuum valve at the flange (on/off) + - Group: Group1 + + .. code:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + Group1 + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + + 3.1 + + + + + + + + + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + Group1 + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + + + + + + + From d3e2e868a6f27a39c9cbf9102507334c00cb3503 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 26 Mar 2024 10:13:15 +0100 Subject: [PATCH 16/18] update tests to use IsEmpty from gmock --- hardware_interface/test/test_component_parser.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index bbaf89f578..2e2cae9807 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -113,7 +113,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface) EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnly"); EXPECT_EQ(hardware_info.type, "system"); - EXPECT_TRUE(hardware_info.group.empty()); + ASSERT_THAT(hardware_info.group, IsEmpty()); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware"); @@ -177,7 +177,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface EXPECT_EQ(hardware_info.name, "RRBotSystemMultiInterface"); EXPECT_EQ(hardware_info.type, "system"); - EXPECT_TRUE(hardware_info.group.empty()); + ASSERT_THAT(hardware_info.group, IsEmpty()); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware"); @@ -240,7 +240,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens EXPECT_EQ(hardware_info.name, "RRBotSystemWithSensor"); EXPECT_EQ(hardware_info.type, "system"); - EXPECT_TRUE(hardware_info.group.empty()); + ASSERT_THAT(hardware_info.group, IsEmpty()); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithSensorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); @@ -309,7 +309,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnlyWithExternalSensor"); EXPECT_EQ(hardware_info.type, "system"); - EXPECT_TRUE(hardware_info.group.empty()); + ASSERT_THAT(hardware_info.group, IsEmpty()); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware"); @@ -612,7 +612,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_tr EXPECT_EQ(hardware_info.name, "RRBotModularWrist"); EXPECT_EQ(hardware_info.type, "system"); - EXPECT_TRUE(hardware_info.group.empty()); + ASSERT_THAT(hardware_info.group, IsEmpty()); EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/ActuatorHardwareMultiDOF"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); @@ -655,7 +655,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only) EXPECT_EQ(hardware_info.name, "CameraWithIMU"); EXPECT_EQ(hardware_info.type, "sensor"); - EXPECT_TRUE(hardware_info.group.empty()); + ASSERT_THAT(hardware_info.group, IsEmpty()); EXPECT_EQ(hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/CameraWithIMUSensor"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); From c357aeb037975923ade42c293c0fc28102869923 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 11 May 2024 21:07:09 +0200 Subject: [PATCH 17/18] add pre-commit changes after the rebase --- hardware_interface/src/resource_manager.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 11f7d17e77..fa9053be93 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -138,7 +138,7 @@ class ResourceStorage component_info.is_async = hardware_info.is_async; hardware_info_map_.insert(std::make_pair(component_info.name, component_info)); - hw_group_state_.insert(std::make_pair(component_info.group, return_type:OK)); + hw_group_state_.insert(std::make_pair(component_info.group, return_type::OK)); hardware_used_by_controllers_.insert( std::make_pair(component_info.name, std::vector())); is_loaded = true; @@ -1612,7 +1612,8 @@ HardwareReadWriteStatus ResourceManager::read( { ret_val = component.read(time, period); const auto component_group = component.get_group_name(); - ret_val = resource_storage_->update_hardware_component_group_state(component_group, ret_val); + ret_val = + resource_storage_->update_hardware_component_group_state(component_group, ret_val); } catch (const std::exception & e) { @@ -1672,7 +1673,8 @@ HardwareReadWriteStatus ResourceManager::write( { ret_val = component.write(time, period); const auto component_group = component.get_group_name(); - ret_val = resource_storage_->update_hardware_component_group_state(component_group, ret_val); + ret_val = + resource_storage_->update_hardware_component_group_state(component_group, ret_val); } catch (const std::exception & e) { From 63e40620bac29d880212f01d1ea5b645eccb19a9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 21 May 2024 07:52:54 +0200 Subject: [PATCH 18/18] Apply suggestions from Bence's code review Co-authored-by: Bence Magyar --- hardware_interface/doc/hardware_interface_types_userdoc.rst | 2 +- hardware_interface/src/resource_manager.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index 478742bc15..d8338bf7a6 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -45,7 +45,7 @@ Hardware Groups ***************************** Hardware Component Groups serve as a critical organizational mechanism within complex systems, facilitating error handling and fault tolerance. By grouping related hardware components together, such as actuators within a manipulator, users can establish a unified framework for error detection and response. -Hardware Component Groups play a vital role in propagating errors across interconnected hardware components. For instance, in a manipulator system, grouping actuators together allows for error propagation. If one actuator fails within the group, the error can propagate to the other actuators, signaling a potential issue across the system. By default, the actuator errors are isolated to their own hardware component, allowing the rest to continue operation unaffected. In the provided ROS2 control configuration, the ```` tag within each ```` block signifies the grouping of hardware components, enabling error propagation mechanisms within the system. +Hardware Component Groups play a vital role in propagating errors across interconnected hardware components. For instance, in a manipulator system, grouping actuators together allows for error propagation. If one actuator fails within the group, the error can propagate to the other actuators, signaling a potential issue across the system. By default, the actuator errors are isolated to their own hardware component, allowing the rest to continue operation unaffected. In the provided ros2_control configuration, the ```` tag within each ```` block signifies the grouping of hardware components, enabling error propagation mechanisms within the system. Examples ***************************** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index fa9053be93..d77f915eee 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -912,7 +912,7 @@ class ResourceStorage } // If it is anything other than OK, change the return type of the hardware group state // to the respective return type - if (value > hw_group_state_.at(group_name)) + if (value != return_type::OK) { hw_group_state_.at(group_name) = value; }