diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index ecf852cb94..d8338bf7a6 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. @@ -152,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 + + + + + + + + + + + + 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/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/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/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/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 14e016df21..ef585c971b 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(group_it, std::string("hardware.") + kGroupTag); + } const auto * params_it = ros2_control_child_it->FirstChildElement(kParamTag); if (params_it) { diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 2d8a01a34f..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) + 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 +470,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 +562,11 @@ 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) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 92b6d01519..d77f915eee 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -133,10 +133,12 @@ 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; 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; @@ -298,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; } @@ -380,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; } @@ -414,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; } @@ -884,6 +898,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 != return_type::OK) + { + hw_group_state_.at(group_name) = value; + } + return hw_group_state_.at(group_name); + } + // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; @@ -898,6 +933,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_; @@ -1575,6 +1611,9 @@ 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) { @@ -1633,6 +1672,9 @@ 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) { 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) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index ac89dc1553..c7777b3e21 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 @@ -579,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_; @@ -600,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 @@ -814,7 +884,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 +895,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()); @@ -857,6 +927,158 @@ 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 same 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")); + } + + // 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( + 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_ + @@ -865,6 +1087,24 @@ 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_ + diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 6a0c11cf72..2e2cae9807 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"); + ASSERT_THAT(hardware_info.group, IsEmpty()); 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"); + ASSERT_THAT(hardware_info.group, IsEmpty()); 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"); + 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)); @@ -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"); + ASSERT_THAT(hardware_info.group, IsEmpty()); 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"); + 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)); @@ -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"); + 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"); 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