diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 4322b22655..3feff42e4b 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -168,6 +168,7 @@ hardware_interface * With (`#1567 `_) all the Hardware components now have a fully functional asynchronous functionality, by simply adding ``is_async`` tag to the ros2_control tag in the URDF. This will allow the hardware components to run in a separate thread, and the controller manager will be able to run the controllers in parallel with the hardware components. * The hardware components can be easily introspect the internal member variables using the macro ``REGISTER_ROS2_CONTROL_INTROSPECTION`` (`#1918 `_) * Added new ``get_optional`` method that returns ``std::optional`` of the templated type, and this can be used to check if the value is available or not (`#1976 `_ and `#2061 `_) +* The ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` methods will now only receive the start/stop interfaces that belong to the hardware component instead of everything (`#2120 `_) * Added hardware components execution time and periodicity statistics diagnostics (`#2086 `_) joint_limits diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index e48e1dff17..ddbf14445d 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -350,12 +350,10 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \note This is a non-realtime evaluation of whether a set of command interface claims are * possible, and call to start preparing data structures for the upcoming switch that will occur. - * \note All starting and stopping interface keys are passed to all components, so the function - * should return return_type::OK by default when given interface keys not relevant for this - * component. \param[in] start_interfaces vector of string identifiers for the command interfaces - * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces - * stopping. \return return_type::OK if the new command interface combination can be prepared, or - * if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. + * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. + * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. + * \return return_type::OK if the new command interface combination can be prepared (or) if the + * interface key is not relevant to this actuator. Returns return_type::ERROR otherwise. */ virtual return_type prepare_command_mode_switch( const std::vector & /*start_interfaces*/, @@ -369,12 +367,10 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * Perform the mode-switching for the new command interface combination. * * \note This is part of the realtime update loop, and should be fast. - * \note All starting and stopping interface keys are passed to all components, so the function - * should return return_type::OK by default when given interface keys not relevant for this - * component. \param[in] start_interfaces vector of string identifiers for the command interfaces - * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces - * stopping. \return return_type::OK if the new command interface combination can be switched to, - * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. + * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. + * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. + * \return return_type::OK if the new command interface combination can be switched to (or) if the + * interface key is not relevant to this actuator. Returns return_type::ERROR otherwise. */ virtual return_type perform_command_mode_switch( const std::vector & /*start_interfaces*/, diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 8e5d7959fb..da32bee930 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -380,12 +380,10 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \note This is a non-realtime evaluation of whether a set of command interface claims are * possible, and call to start preparing data structures for the upcoming switch that will occur. - * \note All starting and stopping interface keys are passed to all components, so the function - * should return return_type::OK by default when given interface keys not relevant for this - * component. \param[in] start_interfaces vector of string identifiers for the command interfaces - * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces - * stopping. \return return_type::OK if the new command interface combination can be prepared, or - * if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. + * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. + * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. + * \return return_type::OK if the new command interface combination can be prepared (or) if the + * interface key is not relevant to this system. Returns return_type::ERROR otherwise. */ virtual return_type prepare_command_mode_switch( const std::vector & /*start_interfaces*/, @@ -399,12 +397,10 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * Perform the mode-switching for the new command interface combination. * * \note This is part of the realtime update loop, and should be fast. - * \note All starting and stopping interface keys are passed to all components, so the function - * should return return_type::OK by default when given interface keys not relevant for this - * component. \param[in] start_interfaces vector of string identifiers for the command interfaces - * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces - * stopping. \return return_type::OK if the new command interface combination can be switched to, - * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. + * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. + * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. + * \return return_type::OK if the new command interface combination can be switched to (or) if the + * interface key is not relevant to this system. Returns return_type::ERROR otherwise. */ virtual return_type perform_command_mode_switch( const std::vector & /*start_interfaces*/, diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index eb67f9fec1..2601e6e6a2 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -89,6 +89,22 @@ std::string interfaces_to_string( return ss.str(); }; +void get_hardware_related_interfaces( + const std::vector & hw_command_itfs, + const std::vector & start_stop_interfaces_list, + std::vector & hw_interfaces) +{ + hw_interfaces.clear(); + for (const auto & interface : start_stop_interfaces_list) + { + if ( + std::find(hw_command_itfs.begin(), hw_command_itfs.end(), interface) != hw_command_itfs.end()) + { + hw_interfaces.push_back(interface); + } + } +} + class ResourceStorage { static constexpr const char * pkg_name = "hardware_interface"; @@ -666,6 +682,8 @@ class ResourceStorage auto interfaces = hardware.export_command_interfaces(); hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); + start_interfaces_buffer_.reserve(start_interfaces_buffer_.capacity() + interfaces.size()); + stop_interfaces_buffer_.reserve(stop_interfaces_buffer_.capacity() + interfaces.size()); // TODO(Manuel) END: for backward compatibility } catch (const std::exception & ex) @@ -1297,6 +1315,10 @@ class ResourceStorage /// The callback to be called when a component state is switched std::function on_component_state_switch_callback_ = nullptr; + // To be used with the prepare and perform command switch for the hardware components + std::vector start_interfaces_buffer_; + std::vector stop_interfaces_buffer_; + // Update rate of the controller manager, and the clock interface of its node // Used by async components. unsigned int cm_update_rate_ = 100; @@ -1861,12 +1883,24 @@ bool ResourceManager::prepare_command_mode_switch( return false; } + const auto & hardware_info_map = resource_storage_->hardware_info_map_; auto call_prepare_mode_switch = - [&start_interfaces, &stop_interfaces, logger = get_logger()](auto & components) + [&start_interfaces, &stop_interfaces, &hardware_info_map, logger = get_logger()]( + auto & components, auto & start_interfaces_buffer, auto & stop_interfaces_buffer) { bool ret = true; for (auto & component : components) { + const auto & hw_command_itfs = hardware_info_map.at(component.get_name()).command_interfaces; + get_hardware_related_interfaces(hw_command_itfs, start_interfaces, start_interfaces_buffer); + get_hardware_related_interfaces(hw_command_itfs, stop_interfaces, stop_interfaces_buffer); + if (start_interfaces_buffer.empty() && stop_interfaces_buffer.empty()) + { + RCLCPP_DEBUG( + logger, "Component '%s' after filtering has no command interfaces to switch", + component.get_name().c_str()); + continue; + } if ( component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || @@ -1876,12 +1910,12 @@ bool ResourceManager::prepare_command_mode_switch( { if ( return_type::OK != - component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) + component.prepare_command_mode_switch(start_interfaces_buffer, stop_interfaces_buffer)) { RCLCPP_ERROR( logger, "Component '%s' did not accept command interfaces combination: \n%s", component.get_name().c_str(), - interfaces_to_string(start_interfaces, stop_interfaces).c_str()); + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str()); ret = false; } } @@ -1892,7 +1926,8 @@ bool ResourceManager::prepare_command_mode_switch( "Exception of type : %s occurred while preparing command mode switch for component " "'%s' for the interfaces: \n %s : %s", typeid(e).name(), component.get_name().c_str(), - interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what()); + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str(), + e.what()); ret = false; } catch (...) @@ -1902,16 +1937,27 @@ bool ResourceManager::prepare_command_mode_switch( "Unknown exception occurred while preparing command mode switch for component '%s' for " "the interfaces: \n %s", component.get_name().c_str(), - interfaces_to_string(start_interfaces, stop_interfaces).c_str()); + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str()); ret = false; } } + else + { + RCLCPP_WARN( + logger, "Component '%s' is not in INACTIVE or ACTIVE state, skipping the prepare switch", + component.get_name().c_str()); + ret = false; + } } return ret; }; - const bool actuators_result = call_prepare_mode_switch(resource_storage_->actuators_); - const bool systems_result = call_prepare_mode_switch(resource_storage_->systems_); + const bool actuators_result = call_prepare_mode_switch( + resource_storage_->actuators_, resource_storage_->start_interfaces_buffer_, + resource_storage_->stop_interfaces_buffer_); + const bool systems_result = call_prepare_mode_switch( + resource_storage_->systems_, resource_storage_->start_interfaces_buffer_, + resource_storage_->stop_interfaces_buffer_); return actuators_result && systems_result; } @@ -1927,12 +1973,24 @@ bool ResourceManager::perform_command_mode_switch( return true; } + const auto & hardware_info_map = resource_storage_->hardware_info_map_; auto call_perform_mode_switch = - [&start_interfaces, &stop_interfaces, logger = get_logger()](auto & components) + [&start_interfaces, &stop_interfaces, &hardware_info_map, logger = get_logger()]( + auto & components, auto & start_interfaces_buffer, auto & stop_interfaces_buffer) { bool ret = true; for (auto & component : components) { + const auto & hw_command_itfs = hardware_info_map.at(component.get_name()).command_interfaces; + get_hardware_related_interfaces(hw_command_itfs, start_interfaces, start_interfaces_buffer); + get_hardware_related_interfaces(hw_command_itfs, stop_interfaces, stop_interfaces_buffer); + if (start_interfaces_buffer.empty() && stop_interfaces_buffer.empty()) + { + RCLCPP_DEBUG( + logger, "Component '%s' after filtering has no command interfaces to perform switch", + component.get_name().c_str()); + continue; + } if ( component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || @@ -1942,10 +2000,12 @@ bool ResourceManager::perform_command_mode_switch( { if ( return_type::OK != - component.perform_command_mode_switch(start_interfaces, stop_interfaces)) + component.perform_command_mode_switch(start_interfaces_buffer, stop_interfaces_buffer)) { RCLCPP_ERROR( - logger, "Component '%s' could not perform switch", component.get_name().c_str()); + logger, "Component '%s' could not perform switch for the command interfaces: \n%s", + component.get_name().c_str(), + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str()); ret = false; } } @@ -1956,7 +2016,8 @@ bool ResourceManager::perform_command_mode_switch( "Exception of type : %s occurred while performing command mode switch for component " "'%s' for the interfaces: \n %s : %s", typeid(e).name(), component.get_name().c_str(), - interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what()); + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str(), + e.what()); ret = false; } catch (...) @@ -1967,16 +2028,27 @@ bool ResourceManager::perform_command_mode_switch( "for " "the interfaces: \n %s", component.get_name().c_str(), - interfaces_to_string(start_interfaces, stop_interfaces).c_str()); + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str()); ret = false; } } + else + { + RCLCPP_WARN( + logger, "Component '%s' is not in INACTIVE or ACTIVE state, skipping the perform switch", + component.get_name().c_str()); + ret = false; + } } return ret; }; - const bool actuators_result = call_perform_mode_switch(resource_storage_->actuators_); - const bool systems_result = call_perform_mode_switch(resource_storage_->systems_); + const bool actuators_result = call_perform_mode_switch( + resource_storage_->actuators_, resource_storage_->start_interfaces_buffer_, + resource_storage_->stop_interfaces_buffer_); + const bool systems_result = call_perform_mode_switch( + resource_storage_->systems_, resource_storage_->start_interfaces_buffer_, + resource_storage_->stop_interfaces_buffer_); if (actuators_result && systems_result) { diff --git a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp index 48a8768744..51f7b91ab7 100644 --- a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp +++ b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp @@ -148,26 +148,33 @@ TEST_F( EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - // When TestActuatorHardware is UNCONFIGURED expect OK + // When TestActuatorHardware is UNCONFIGURED expect ERROR EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 403.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)) + << "The actuator HW component is unconfigured, so the perform should fail!"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0) + << "Shouldn't change as the perform command mode switch is done only for actuator interfaces"; EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 403.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0) + << "Shouldn't change as the perform command mode switch is done only for actuator interfaces"; EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 503.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)) + << "The actuator HW component is unconfigured, so the perform should fail!"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0) + << "Shouldn't change as the perform command mode switch is done only for actuator interfaces"; EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 503.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0) + << "Shouldn't change as the perform command mode switch is done only for actuator interfaces"; EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 603.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0) + << "Shouldn't change as the perform command mode switch is done only for actuator interfaces"; EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); }; @@ -183,46 +190,47 @@ TEST_F( // When TestSystemCommandModes is ACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 102.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 202.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 203.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); // When TestActuatorHardware is INACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 304.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.304, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 404.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 405.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 505.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 506.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.506, 1e-7); - EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 606.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.606, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)) + << "Inactive is OK"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7); }; // System : INACTIVE @@ -237,46 +245,52 @@ TEST_F( // When TestSystemCommandModes is INACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 102.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 202.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 203.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; // When TestActuatorHardware is ACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 304.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.304, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 404.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 405.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 505.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 506.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.506, 1e-7); - EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 606.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.606, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7); }; // System : UNCONFIGURED @@ -292,46 +306,49 @@ TEST_F( // When TestSystemCommandModes is UNCONFIGURED expect error EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)) + << "The system HW component is unconfigured, so the perform should fail!"; EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.100, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.100, 1e-7); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)) + << "The system HW component is unconfigured, so the perform should fail!"; EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.200, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.200, 1e-7); - EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)) + << "The system HW component is unconfigured, so the perform should fail!"; EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.300, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - // When TestActuatorHardware is INACTIVE expect OK + // When TestActuatorHardware is ACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.301, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.401, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.402, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.502, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.503, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.603, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7); }; // System : UNCONFIGURED @@ -348,43 +365,43 @@ TEST_F( EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - // When TestActuatorHardware is INACTIVE expect OK + // When TestActuatorHardware is FINALIZED expect ERROR EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); };