diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 73d6b4d596..822f5563da 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -56,7 +56,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_chainable() const final; CONTROLLER_INTERFACE_PUBLIC - std::vector export_estimated_interfaces() final; + std::vector export_state_interfaces() final; CONTROLLER_INTERFACE_PUBLIC std::vector export_reference_interfaces() final; @@ -75,12 +75,12 @@ class ChainableControllerInterface : public ControllerInterfaceBase /// chainable interfaces. /** * Each chainable controller implements this methods where all its state(read only) interfaces are - * exported. The method has the same meaning as `export_estimated_interfaces` method from + * exported. The method has the same meaning as `export_state_interfaces` method from * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface. * * \returns list of StateInterfaces that other controller can use as their outputs. */ - virtual std::vector on_export_estimated_interfaces() = 0; + virtual std::vector on_export_state_interfaces() = 0; /// Virtual method that each chainable controller should implement to export its read/write /// chainable interfaces. @@ -131,8 +131,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - /// Storage of values for estimated interfaces - std::vector estimated_interfaces_data_; + /// Storage of values for exported state interfaces + std::vector exported_state_interfaces_data_; /// Storage of values for reference interfaces std::vector reference_interfaces_; diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 8f3e9a08f8..cce5dd3f84 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -48,7 +48,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC - std::vector export_estimated_interfaces() final; + std::vector export_state_interfaces() final; /** * Controller has no reference interfaces. diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index b478c5666d..150474b809 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -203,12 +203,12 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of state interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC - virtual std::vector export_estimated_interfaces() = 0; + virtual std::vector export_state_interfaces() = 0; /** * Set chained mode of a chainable controller. This method triggers internal processes to switch * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode - * usually involves the usage of the controller's reference/estimated interfaces by the other + * usually involves the usage of the controller's reference/state interfaces by the other * controllers * * \returns true if mode is switched successfully and false if not. diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 347921f583..8398f11955 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -45,41 +45,41 @@ return_type ChainableControllerInterface::update( } std::vector -ChainableControllerInterface::export_estimated_interfaces() +ChainableControllerInterface::export_state_interfaces() { - auto estimated_interfaces = on_export_estimated_interfaces(); - // check if the "estimated_interfaces_data_" variable is resized to number of interfaces - if (estimated_interfaces_data_.size() != estimated_interfaces.size()) + auto state_interfaces = on_export_state_interfaces(); + // check if the "state_interfaces_data_" variable is resized to number of interfaces + if (exported_state_interfaces_data_.size() != state_interfaces.size()) { // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the // framework RCLCPP_FATAL( get_node()->get_logger(), - "The internal storage for estimated values 'estimated_interfaces_data_' variable has size " - "'%zu', but it is expected to have the size '%zu' equal to the number of exported reference " - "interfaces. No reference interface will be exported. Please correct and recompile " + "The internal storage for exported state values 'state_interfaces_data_' variable has size " + "'%zu', but it is expected to have the size '%zu' equal to the number of exported state " + "interfaces. No state interface will be exported. Please correct and recompile " "the controller with name '%s' and try again.", - estimated_interfaces_data_.size(), estimated_interfaces.size(), get_node()->get_name()); - estimated_interfaces.clear(); + exported_state_interfaces_data_.size(), state_interfaces.size(), get_node()->get_name()); + state_interfaces.clear(); } // check if the names of the controller state interfaces begin with the controller's name - for (const auto & interface : estimated_interfaces) + for (const auto & interface : state_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { RCLCPP_FATAL( get_node()->get_logger(), "The name of the interface '%s' does not begin with the controller's name. This is " - "mandatory for estimated interfaces. No estimated interface will be exported. Please " + "mandatory for state interfaces. No state interface will be exported. Please " "correct and recompile the controller with name '%s' and try again.", interface.get_name().c_str(), get_node()->get_name()); - estimated_interfaces.clear(); + state_interfaces.clear(); break; } } - return estimated_interfaces; + return state_interfaces; } std::vector diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 40a74195a5..3d82c4d59d 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -28,7 +28,7 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } -std::vector ControllerInterface::export_estimated_interfaces() +std::vector ControllerInterface::export_state_interfaces() { return {}; } diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index ce0e4a1a73..d349621498 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -28,7 +28,7 @@ TEST_F(ChainableControllerInterfaceTest, default_returns) EXPECT_FALSE(controller.is_in_chained_mode()); } -TEST_F(ChainableControllerInterfaceTest, export_estimated_interfaces) +TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) { TestableChainableControllerInterface controller; @@ -36,13 +36,13 @@ TEST_F(ChainableControllerInterfaceTest, export_estimated_interfaces) ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); - auto estimated_interfaces = controller.export_estimated_interfaces(); + auto exported_state_interfaces = controller.export_state_interfaces(); - ASSERT_EQ(estimated_interfaces.size(), 1u); - EXPECT_EQ(estimated_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); - EXPECT_EQ(estimated_interfaces[0].get_interface_name(), "test_state"); + ASSERT_EQ(exported_state_interfaces.size(), 1u); + EXPECT_EQ(exported_state_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(exported_state_interfaces[0].get_interface_name(), "test_state"); - EXPECT_EQ(estimated_interfaces[0].get_value(), ESTIMATED_INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) @@ -75,9 +75,9 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_storage_not_correct_size) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_TRUE(reference_interfaces.empty()); // expect empty return because storage is not resized - controller.estimated_interfaces_data_.clear(); - auto estimated_interfaces = controller.export_estimated_interfaces(); - ASSERT_TRUE(estimated_interfaces.empty()); + controller.exported_state_interfaces_data_.clear(); + auto exported_state_interfaces = controller.export_state_interfaces(); + ASSERT_TRUE(exported_state_interfaces.empty()); } TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) @@ -94,8 +94,8 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_TRUE(reference_interfaces.empty()); // expect empty return because interface prefix is not equal to the node name - auto estimated_interfaces = controller.export_estimated_interfaces(); - ASSERT_TRUE(estimated_interfaces.empty()); + auto exported_state_interfaces = controller.export_state_interfaces(); + ASSERT_TRUE(exported_state_interfaces.empty()); } TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) @@ -108,14 +108,14 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), 1u); - auto estimated_interfaces = controller.export_estimated_interfaces(); - ASSERT_EQ(estimated_interfaces.size(), 1u); + auto exported_state_interfaces = controller.export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 1u); EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); - EXPECT_EQ(estimated_interfaces[0].get_value(), ESTIMATED_INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -128,7 +128,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); - EXPECT_EQ(estimated_interfaces[0].get_value(), ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE); + EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); controller.configure(); EXPECT_TRUE(controller.set_chained_mode(false)); @@ -165,7 +165,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); - ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); // Provoke error in update from subscribers - return ERROR and update_and_write_commands not exec. controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); @@ -173,7 +173,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); - ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); // Provoke error from update - return ERROR, but reference interface is updated and not reduced controller.set_new_reference_interface_value(INTERFACE_VALUE_UPDATE_ERROR); @@ -181,7 +181,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); - ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); controller.reference_interfaces_[0] = 0.0; @@ -196,7 +196,8 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ASSERT_EQ(controller.reference_interfaces_[0], -1.0); - ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE + 1); + ASSERT_EQ( + controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); // Provoke error from update - return ERROR, but reference interface is updated directly controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); @@ -205,5 +206,6 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); - ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE + 1); + ASSERT_EQ( + controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); } diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp index 298769fe44..7b8eaf7db5 100644 --- a/controller_interface/test/test_chainable_controller_interface.hpp +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -29,8 +29,8 @@ constexpr double INTERFACE_VALUE = 1989.0; constexpr double INTERFACE_VALUE_SUBSCRIBER_ERROR = 12345.0; constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0; constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0; -constexpr double ESTIMATED_INTERFACE_VALUE = 21833.0; -constexpr double ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE = 82802.0; +constexpr double EXPORTED_STATE_INTERFACE_VALUE = 21833.0; +constexpr double EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE = 82802.0; class TestableChainableControllerInterface : public controller_interface::ChainableControllerInterface @@ -43,8 +43,8 @@ class TestableChainableControllerInterface { reference_interfaces_.reserve(1); reference_interfaces_.push_back(INTERFACE_VALUE); - estimated_interfaces_data_.reserve(1); - estimated_interfaces_data_.push_back(ESTIMATED_INTERFACE_VALUE); + exported_state_interfaces_data_.reserve(1); + exported_state_interfaces_data_.push_back(EXPORTED_STATE_INTERFACE_VALUE); } controller_interface::CallbackReturn on_init() override @@ -68,14 +68,14 @@ class TestableChainableControllerInterface } // Implementation of ChainableController virtual methods - std::vector on_export_estimated_interfaces() override + std::vector on_export_state_interfaces() override { - std::vector estimated_interfaces; + std::vector state_interfaces; - estimated_interfaces.push_back(hardware_interface::StateInterface( - name_prefix_of_interfaces_, "test_state", &estimated_interfaces_data_[0])); + state_interfaces.push_back(hardware_interface::StateInterface( + name_prefix_of_interfaces_, "test_state", &exported_state_interfaces_data_[0])); - return estimated_interfaces; + return state_interfaces; } // Implementation of ChainableController virtual methods @@ -93,7 +93,7 @@ class TestableChainableControllerInterface { if (reference_interfaces_[0] == 0.0) { - estimated_interfaces_data_[0] = ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE; + exported_state_interfaces_data_[0] = EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE; return true; } else @@ -123,7 +123,7 @@ class TestableChainableControllerInterface } reference_interfaces_[0] -= 1; - estimated_interfaces_data_[0] += 1; + exported_state_interfaces_data_[0] += 1; return controller_interface::return_type::OK; } diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fd05348a49..a8ccc14267 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -71,7 +71,7 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const /// Checks if an interface belongs to a controller based on its prefix. /** * A State/Command interface can be provided by a controller in which case is called - * "estimated/reference" interface. This means that the @interface_name starts with the name of a + * "state/reference" interface. This means that the @interface_name starts with the name of a * controller. * * \param[in] interface_name to be found in the map. @@ -139,7 +139,7 @@ bool is_interface_exported_from_controller( return false; } -bool is_controller_estimated_interfaces_inuse_by_other_controllers( +bool is_controller_exported_state_interfaces_inuse_by_other_controllers( const std::string & controller_name, const std::vector & controllers, std::vector blacklist) @@ -155,8 +155,8 @@ bool is_controller_estimated_interfaces_inuse_by_other_controllers( { continue; } - auto controller_state_interfaces = controller.c->state_interface_configuration().names; - for (const auto & ctrl_itf_name : controller_state_interfaces) + auto controller_exported_state_interfaces = controller.c->state_interface_configuration().names; + for (const auto & ctrl_itf_name : controller_exported_state_interfaces) { if (is_interface_exported_from_controller(ctrl_itf_name, controller_name)) { @@ -677,20 +677,20 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); - auto estimated_interfaces = controller->export_estimated_interfaces(); + auto state_interfaces = controller->export_state_interfaces(); auto ref_interfaces = controller->export_reference_interfaces(); - if (ref_interfaces.empty() && estimated_interfaces.empty()) + if (ref_interfaces.empty() && state_interfaces.empty()) { // TODO(destogl): Add test for this! RCLCPP_ERROR( get_logger(), - "Controller '%s' is chainable, but does not export any estimated or reference interfaces.", + "Controller '%s' is chainable, but does not export any state or reference interfaces.", controller_name.c_str()); return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); - resource_manager_->import_controller_estimated_interfaces( - controller_name, estimated_interfaces); + resource_manager_->import_controller_exported_state_interfaces( + controller_name, state_interfaces); // TODO(destogl): check and resort controllers in the vector } @@ -1335,14 +1335,14 @@ void ControllerManager::switch_chained_mode( // enable references from the controller interfaces controller->toggle_references_from_subscribers(false); // make all the exported interfaces of the controller available - resource_manager_->make_controller_estimated_interfaces_available(request); + resource_manager_->make_controller_exported_state_interfaces_available(request); resource_manager_->make_controller_reference_interfaces_available(request); } else { - // release estimated interfaces of the controller and the release of reference interfaces - // is handled by the to_use_references_from_subscribers_ list - resource_manager_->make_controller_estimated_interfaces_unavailable(request); + // the release of exported state interfaces and the reference interfaces of the + // controller is handled by the to_use_references_from_subscribers_ list + resource_manager_->make_controller_exported_state_interfaces_unavailable(request); } } else @@ -1567,18 +1567,18 @@ void ControllerManager::list_controllers_srv_cb( controller_chain_interface_map[controller_state.name].push_back(interface_type); } } - // check estimated and reference interfaces only if controller is inactive or active + // check state and reference interfaces only if controller is inactive or active auto references = controllers[i].c->export_reference_interfaces(); - auto estimated_interfaces = controllers[i].c->export_estimated_interfaces(); + auto state_interfaces = controllers[i].c->export_state_interfaces(); controller_state.reference_interfaces.reserve(references.size()); - controller_state.estimated_interfaces.reserve(estimated_interfaces.size()); + controller_state.exported_state_interfaces.reserve(state_interfaces.size()); for (const auto & reference : references) { controller_state.reference_interfaces.push_back(reference.get_interface_name()); } - for (const auto & estimate : estimated_interfaces) + for (const auto & state : state_interfaces) { - controller_state.estimated_interfaces.push_back(estimate.get_interface_name()); + controller_state.exported_state_interfaces.push_back(state.get_interface_name()); } } response->controller.push_back(controller_state); @@ -2131,9 +2131,9 @@ void ControllerManager::propagate_deactivation_of_chained_mode( ControllersListIterator following_ctrl_it; if (is_interface_a_chained_interface(ctrl_itf_name, controllers, following_ctrl_it)) { - // If the preceding controller's estimated interfaces are in use by other controllers, + // If the preceding controller's state interfaces are in use by other controllers, // then maintain the chained mode - if (is_controller_estimated_interfaces_inuse_by_other_controllers( + if (is_controller_exported_state_interfaces_inuse_by_other_controllers( following_ctrl_it->info.name, controllers, deactivate_request_)) { auto found_it = std::find( @@ -2211,7 +2211,7 @@ controller_interface::return_type ControllerManager::check_following_controllers { RCLCPP_WARN( get_logger(), - "No estimated/reference interface '%s' exist, since the following controller with name " + "No state/reference interface '%s' exist, since the following controller with name " "'%s' is not chainable.", ctrl_itf_name.c_str(), following_ctrl_it->info.name.c_str()); return controller_interface::return_type::ERROR; @@ -2330,9 +2330,9 @@ controller_interface::return_type ControllerManager::check_preceeding_controller const auto ctrl_ref_itfs = resource_manager_->get_controller_reference_interface_names(controller_it->info.name); - const auto ctrl_estim_itfs = - resource_manager_->get_controller_estimated_interface_names(controller_it->info.name); - for (const auto & controller_interfaces : {ctrl_ref_itfs, ctrl_estim_itfs}) + const auto ctrl_exp_state_itfs = + resource_manager_->get_controller_exported_state_interface_names(controller_it->info.name); + for (const auto & controller_interfaces : {ctrl_ref_itfs, ctrl_exp_state_itfs}) { for (const auto & ref_itf_name : controller_interfaces) { diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index 69a37970b0..feb2a4e615 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -103,18 +103,19 @@ controller_interface::return_type TestChainableController::update_and_write_comm { command_interfaces_[i].set_value(reference_interfaces_[i] - state_interfaces_[i].get_value()); } - // If there is a command interface then integrate and set it to the estimated interface data - for (size_t i = 0; i < estimated_interface_names_.size() && i < command_interfaces_.size(); ++i) + // If there is a command interface then integrate and set it to the exported state interface data + for (size_t i = 0; i < exported_state_interface_names_.size() && i < command_interfaces_.size(); + ++i) { - estimated_interfaces_data_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT; + exported_state_interfaces_data_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT; } // If there is no command interface and if there is a state interface then just forward the same // value as in the state interface - for (size_t i = 0; i < estimated_interface_names_.size() && i < state_interfaces_.size() && + for (size_t i = 0; i < exported_state_interface_names_.size() && i < state_interfaces_.size() && command_interfaces_.empty(); ++i) { - estimated_interfaces_data_[i] = state_interfaces_[i].get_value(); + exported_state_interfaces_data_[i] = state_interfaces_[i].get_value(); } return controller_interface::return_type::OK; @@ -171,17 +172,18 @@ CallbackReturn TestChainableController::on_cleanup( } std::vector -TestChainableController::on_export_estimated_interfaces() +TestChainableController::on_export_state_interfaces() { - std::vector estimated_interfaces; + std::vector state_interfaces; - for (size_t i = 0; i < estimated_interface_names_.size(); ++i) + for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) { - estimated_interfaces.push_back(hardware_interface::StateInterface( - get_node()->get_name(), estimated_interface_names_[i], &estimated_interfaces_data_[i])); + state_interfaces.push_back(hardware_interface::StateInterface( + get_node()->get_name(), exported_state_interface_names_[i], + &exported_state_interfaces_data_[i])); } - return estimated_interfaces; + return state_interfaces; } std::vector @@ -218,12 +220,12 @@ void TestChainableController::set_reference_interface_names( reference_interfaces_.resize(reference_interface_names.size(), 0.0); } -void TestChainableController::set_estimated_interface_names( - const std::vector & estimated_interface_names) +void TestChainableController::set_exported_state_interface_names( + const std::vector & state_interface_names) { - estimated_interface_names_ = estimated_interface_names; + exported_state_interface_names_ = state_interface_names; - estimated_interfaces_data_.resize(estimated_interface_names_.size(), 0.0); + exported_state_interfaces_data_.resize(exported_state_interface_names_.size(), 0.0); } void TestChainableController::set_imu_sensor_name(const std::string & name) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index 96c961a135..2862b8e2e7 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -63,7 +63,7 @@ class TestChainableController : public controller_interface::ChainableController CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; CONTROLLER_MANAGER_PUBLIC - std::vector on_export_estimated_interfaces() override; + std::vector on_export_state_interfaces() override; CONTROLLER_MANAGER_PUBLIC std::vector on_export_reference_interfaces() override; @@ -86,7 +86,7 @@ class TestChainableController : public controller_interface::ChainableController void set_reference_interface_names(const std::vector & reference_interface_names); CONTROLLER_MANAGER_PUBLIC - void set_estimated_interface_names(const std::vector & estimated_interface_names); + void set_exported_state_interface_names(const std::vector & state_interface_names); CONTROLLER_MANAGER_PUBLIC void set_imu_sensor_name(const std::string & name); @@ -95,7 +95,7 @@ class TestChainableController : public controller_interface::ChainableController controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; std::vector reference_interface_names_; - std::vector estimated_interface_names_; + std::vector exported_state_interface_names_; std::unique_ptr imu_sensor_; realtime_tools::RealtimeBuffer> rt_command_ptr_; diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 8d16e40cd3..92d8171538 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -153,13 +153,13 @@ class TestControllerChainingWithControllerManager diff_drive_controller->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); diff_drive_controller->set_state_interface_configuration(diff_drive_state_ifs_cfg); diff_drive_controller->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); - diff_drive_controller->set_estimated_interface_names({"odom_x", "odom_y"}); + diff_drive_controller->set_exported_state_interface_names({"odom_x", "odom_y"}); // configure Diff Drive Two controller (Has same command interfaces as Diff Drive controller) diff_drive_controller_two->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); diff_drive_controller_two->set_state_interface_configuration(diff_drive_state_ifs_cfg); diff_drive_controller_two->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); - diff_drive_controller_two->set_estimated_interface_names({"odom_x", "odom_y"}); + diff_drive_controller_two->set_exported_state_interface_names({"odom_x", "odom_y"}); // configure Position Tracking controller controller_interface::InterfaceConfiguration position_tracking_cmd_ifs_cfg = { @@ -180,7 +180,7 @@ class TestControllerChainingWithControllerManager // configure sensor fusion controller sensor_fusion_controller->set_imu_sensor_name("base_imu"); sensor_fusion_controller->set_state_interface_configuration(odom_and_fusion_ifs_cfg); - sensor_fusion_controller->set_estimated_interface_names({"odom_x", "odom_y", "yaw"}); + sensor_fusion_controller->set_exported_state_interface_names({"odom_x", "odom_y", "yaw"}); // configure Robot Localization controller controller_interface::InterfaceConfiguration odom_ifs_cfg = { @@ -341,10 +341,10 @@ class TestControllerChainingWithControllerManager T>::type * = nullptr> void SetToChainedModeAndMakeInterfacesAvailable( std::shared_ptr & controller, const std::string & controller_name, - const std::vector & estimated_interfaces, + const std::vector & exported_state_interfaces, const std::vector & reference_interfaces) { - if (!estimated_interfaces.empty() || !reference_interfaces.empty()) + if (!exported_state_interfaces.empty() || !reference_interfaces.empty()) { controller->set_chained_mode(true); EXPECT_TRUE(controller->is_in_chained_mode()); @@ -359,11 +359,12 @@ class TestControllerChainingWithControllerManager EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } } - if (!estimated_interfaces.empty()) + if (!exported_state_interfaces.empty()) { - // make estimated interface state_interfaces available - cm_->resource_manager_->make_controller_estimated_interfaces_available(controller_name); - for (const auto & interface : estimated_interfaces) + // make state interface state_interfaces available + cm_->resource_manager_->make_controller_exported_state_interfaces_available( + controller_name); + for (const auto & interface : exported_state_interfaces) { EXPECT_TRUE(cm_->resource_manager_->state_interface_exists(interface)); EXPECT_TRUE(cm_->resource_manager_->state_interface_is_available(interface)); @@ -496,15 +497,15 @@ class TestControllerChainingWithControllerManager ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); - EXP_ESTIMATED_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); - EXP_ESTIMATED_ODOM_Y = chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); - ASSERT_EQ(sensor_fusion_controller->estimated_interfaces_data_.size(), 3u); + EXP_STATE_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); + EXP_STATE_ODOM_Y = chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ(sensor_fusion_controller->exported_state_interfaces_data_.size(), 3u); ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 3u); - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); ASSERT_EQ(odom_publisher_controller->get_state_interface_data().size(), 2u); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); @@ -513,8 +514,8 @@ class TestControllerChainingWithControllerManager // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); @@ -524,8 +525,8 @@ class TestControllerChainingWithControllerManager // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); } // check data propagation through controllers and if individual controllers are working @@ -552,7 +553,7 @@ class TestControllerChainingWithControllerManager "pid_right_wheel_controller/velocity"}; const std::vector DIFF_DRIVE_REFERENCE_INTERFACES = { "diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}; - const std::vector DIFF_DRIVE_ESTIMATED_INTERFACES = { + const std::vector DIFF_DRIVE_STATE_INTERFACES = { "diff_drive_controller/odom_x", "diff_drive_controller/odom_y"}; const std::vector SENSOR_FUSION_ESIMTATED_INTERFACES = { "sensor_fusion_controller/odom_x", "sensor_fusion_controller/odom_y", @@ -584,8 +585,8 @@ class TestControllerChainingWithControllerManager double EXP_RIGHT_WHEEL_HW_STATE = 0.0; double EXP_LEFT_WHEEL_REF = 0.0; double EXP_RIGHT_WHEEL_REF = 0.0; - double EXP_ESTIMATED_ODOM_X = 0.0; - double EXP_ESTIMATED_ODOM_Y = 0.0; + double EXP_STATE_ODOM_X = 0.0; + double EXP_STATE_ODOM_Y = 0.0; // Expected behaviors struct used in chaining activation/deactivation tests struct ExpectedBehaviorStruct @@ -639,7 +640,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) SetToChainedModeAndMakeInterfacesAvailable( pid_right_wheel_controller, PID_RIGHT_WHEEL, {}, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); SetToChainedModeAndMakeInterfacesAvailable( - diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_ESTIMATED_INTERFACES, + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_STATE_INTERFACES, DIFF_DRIVE_REFERENCE_INTERFACES); SetToChainedModeAndMakeInterfacesAvailable( sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, SENSOR_FUSION_ESIMTATED_INTERFACES, {}); @@ -688,7 +689,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); - // Sensor Controller uses estimated interfaces of Diff-Drive Controller and IMU + // Sensor Controller uses exported state interfaces of Diff-Drive Controller and IMU ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); ASSERT_EQ(sensor_fusion_controller->internal_counter, 1u); ASSERT_EQ(position_tracking_controller->internal_counter, 3u); @@ -696,7 +697,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 7u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 9u); - // Robot localization Controller uses estimated interfaces of Diff-Drive Controller + // Robot localization Controller uses exported state interfaces of Diff-Drive Controller ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); ASSERT_EQ(robot_localization_controller->internal_counter, 1u); ASSERT_EQ(sensor_fusion_controller->internal_counter, 3u); @@ -705,7 +706,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 9u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 11u); - // Odometry Publisher Controller uses estimated interfaces of Diff-Drive Controller + // Odometry Publisher Controller uses exported state interfaces of Diff-Drive Controller ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); // 'rot_z' reference interface is not claimed for (const auto & interface : {"diff_drive_controller/rot_z"}) @@ -754,16 +755,16 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(robot_localization_controller->internal_counter, 4u); odom_publisher_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(odom_publisher_controller->internal_counter, 2u); - EXP_ESTIMATED_ODOM_X = + EXP_STATE_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 / dt - EXP_ESTIMATED_ODOM_Y = + EXP_STATE_ODOM_Y = chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 / dt ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 3u); - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); ASSERT_EQ(odom_publisher_controller->get_state_interface_data().size(), 2u); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); // update PID controllers that are writing to hardware pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -782,8 +783,8 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_ESTIMATED_ODOM_X); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); // 128 - 0 EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); @@ -797,8 +798,8 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation - ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); - ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_ESTIMATED_ODOM_Y); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); // update all controllers at once and see that all have expected values --> also checks the order // of controller execution @@ -893,17 +894,17 @@ TEST_P( EXPECT_TRUE(cm_->resource_manager_->state_interface_is_available(interface)); } - // Sensor fusion Controller uses estimated interfaces of Diff-Drive Controller and IMU + // Sensor fusion Controller uses exported state interfaces of Diff-Drive Controller and IMU ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); - // Robot localization Controller uses estimated interfaces of sensor fusion Controller + // Robot localization Controller uses exported state interfaces of sensor fusion Controller ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); - // Odometry Publisher Controller uses estimated interfaces of Diff-Drive Controller + // Odometry Publisher Controller uses exported state interfaces of Diff-Drive Controller ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); @@ -919,7 +920,7 @@ TEST_P( // PositionController is deactivated --> DiffDrive controller still continues in chained mode // As the DiffDriveController is in chained mode, right now we tend to also deactivate - // the other controllers that rely on the DiffDriveController expected interfaces + // the other controllers that rely on the DiffDriveController exported interfaces DeactivateAndCheckController( position_tracking_controller, POSITION_TRACKING_CONTROLLER, POSITION_CONTROLLER_CLAIMED_INTERFACES, 10u); @@ -928,7 +929,7 @@ TEST_P( ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); - // SensorFusionController continues to stay in the chained mode as it is still using the estimated + // SensorFusionController continues to stay in the chained mode as it is still using the state // interfaces ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( @@ -1500,9 +1501,9 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}}; // Test switch 'from chained mode' when controllers are deactivated and possible combination of - // disabling controllers that use reference/estimated interfaces of the other controller. This is + // disabling controllers that use reference/state interfaces of the other controller. This is // important to check that deactivation is not trigger irrespective of the type - // (reference/estimated) interface that is shared among the other controllers + // (reference/state) interface that is shared among the other controllers // PositionController is deactivated --> DiffDrive controller still continues in chained mode // As the DiffDriveController is in chained mode, right now we tend to also deactivate @@ -1516,7 +1517,7 @@ TEST_P( ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); - // SensorFusionController continues to stay in the chained mode as it is still using the estimated + // SensorFusionController continues to stay in the chained mode as it is still using the state // interfaces EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); diff --git a/controller_manager_msgs/msg/ControllerState.msg b/controller_manager_msgs/msg/ControllerState.msg index ed0c64dd4c..65d773bddc 100644 --- a/controller_manager_msgs/msg/ControllerState.msg +++ b/controller_manager_msgs/msg/ControllerState.msg @@ -6,6 +6,6 @@ string[] required_command_interfaces # command interfaces required by con string[] required_state_interfaces # state interfaces required by controller bool is_chainable # specifies whether or not controller can export references for a controller chain bool is_chained # specifies whether or not controller's exported references are claimed by another controller -string[] estimated_interfaces # estimated interfaces to be exported (only applicable if is_chainable is true) +string[] exported_state_interfaces # state interfaces to be exported (only applicable if is_chainable is true) string[] reference_interfaces # references to be exported (only applicable if is_chainable is true) ChainConnection[] chain_connections # specifies list of controllers and their exported references that the controller is chained to diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 0d358eb229..f43090b1c1 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -130,9 +130,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; - /// Add controllers' estimated interfaces to resource manager. + /// Add controllers' exported state interfaces to resource manager. /** - * Interface for transferring management of estimated interfaces to resource manager. + * Interface for transferring management of exported state interfaces to resource manager. * When chaining controllers, state interfaces are used by the preceding * controllers. * Therefore, they should be managed in the same way as state interface of hardware. @@ -140,30 +140,31 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] controller_name name of the controller which state interfaces are imported. * \param[in] interfaces list of controller's state interfaces as StateInterfaces. */ - void import_controller_estimated_interfaces( + void import_controller_exported_state_interfaces( const std::string & controller_name, std::vector & interfaces); - /// Get list of estimated interface of a controller. + /// Get list of exported tate interface of a controller. /** - * Returns lists of stored state interfaces names for a controller. + * Returns lists of stored exported state interfaces names for a controller. * * \param[in] controller_name for which list of state interface names is returned. * \returns list of reference interface names. */ - std::vector get_controller_estimated_interface_names( + std::vector get_controller_exported_state_interface_names( const std::string & controller_name); - /// Add controller's estimated interfaces to available list. + /// Add controller's exported state interfaces to available list. /** * Adds state interfacess of a controller with given name to the available list. This method * should be called when a controller gets activated with chained mode turned on. That means, the - * controller's estimated interfaces can be used by another controllers in chained architectures. + * controller's exported state interfaces can be used by another controllers in chained + * architectures. * * \param[in] controller_name name of the controller which interfaces should become available. */ - void make_controller_estimated_interfaces_available(const std::string & controller_name); + void make_controller_exported_state_interfaces_available(const std::string & controller_name); - /// Remove controller's estimated interface to available list. + /// Remove controller's exported state interface to available list. /** * Removes interfaces of a controller with given name from the available list. This method should * be called when a controller gets deactivated and its reference interfaces cannot be used by @@ -171,16 +172,16 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * * \param[in] controller_name name of the controller which interfaces should become unavailable. */ - void make_controller_estimated_interfaces_unavailable(const std::string & controller_name); + void make_controller_exported_state_interfaces_unavailable(const std::string & controller_name); - /// Remove controllers estimated interfaces from resource manager. + /// Remove controllers exported state interfaces from resource manager. /** - * Remove state interfaces from resource manager, i.e., resource storage. + * Remove exported state interfaces from resource manager, i.e., resource storage. * The interfaces will be deleted from all internal maps and lists. * * \param[in] controller_name list of interface names that will be deleted from resource manager. */ - void remove_controller_estimated_interfaces(const std::string & controller_name); + void remove_controller_exported_state_interfaces(const std::string & controller_name); /// Add controllers' reference interfaces to resource manager. /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 29448364e8..f619531de7 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -746,7 +746,8 @@ class ResourceStorage std::unordered_map> hardware_used_by_controllers_; /// Mapping between controllers and list of interfaces they are using - std::unordered_map> controllers_estimated_interfaces_map_; + std::unordered_map> + controllers_exported_state_interfaces_map_; std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces @@ -882,27 +883,27 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con } // CM API: Called in "callback/slow"-thread -void ResourceManager::import_controller_estimated_interfaces( +void ResourceManager::import_controller_exported_state_interfaces( const std::string & controller_name, std::vector & interfaces) { std::lock_guard guard(resource_interfaces_lock_); auto interface_names = resource_storage_->add_state_interfaces(interfaces); - resource_storage_->controllers_estimated_interfaces_map_[controller_name] = interface_names; + resource_storage_->controllers_exported_state_interfaces_map_[controller_name] = interface_names; } // CM API: Called in "callback/slow"-thread -std::vector ResourceManager::get_controller_estimated_interface_names( +std::vector ResourceManager::get_controller_exported_state_interface_names( const std::string & controller_name) { - return resource_storage_->controllers_estimated_interfaces_map_.at(controller_name); + return resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); } // CM API: Called in "update"-thread -void ResourceManager::make_controller_estimated_interfaces_available( +void ResourceManager::make_controller_exported_state_interfaces_available( const std::string & controller_name) { auto interface_names = - resource_storage_->controllers_estimated_interfaces_map_.at(controller_name); + resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); std::lock_guard guard(resource_interfaces_lock_); resource_storage_->available_state_interfaces_.insert( resource_storage_->available_state_interfaces_.end(), interface_names.begin(), @@ -910,11 +911,11 @@ void ResourceManager::make_controller_estimated_interfaces_available( } // CM API: Called in "update"-thread -void ResourceManager::make_controller_estimated_interfaces_unavailable( +void ResourceManager::make_controller_exported_state_interfaces_unavailable( const std::string & controller_name) { auto interface_names = - resource_storage_->controllers_estimated_interfaces_map_.at(controller_name); + resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); std::lock_guard guard(resource_interfaces_lock_); for (const auto & interface : interface_names) @@ -932,11 +933,12 @@ void ResourceManager::make_controller_estimated_interfaces_unavailable( } // CM API: Called in "callback/slow"-thread -void ResourceManager::remove_controller_estimated_interfaces(const std::string & controller_name) +void ResourceManager::remove_controller_exported_state_interfaces( + const std::string & controller_name) { auto interface_names = - resource_storage_->controllers_estimated_interfaces_map_.at(controller_name); - resource_storage_->controllers_estimated_interfaces_map_.erase(controller_name); + resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); + resource_storage_->controllers_exported_state_interfaces_map_.erase(controller_name); std::lock_guard guard(resource_interfaces_lock_); resource_storage_->remove_state_interfaces(interface_names);