diff --git a/CHANGELOG.md b/CHANGELOG.md index 877482c5a..72f4c3ca0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,7 +17,7 @@ Release Versions: - feat: improve logging in parameter translators (#65) - fix(component-interface): make python subscriptions type safe (#71) - build: change base workspace image version -- feat: add modulo controllers (#84, #93, #94, #95, #96, #97, #98) +- feat: add modulo controllers (#84, #93, #94, #95, #96, #97, #98, #99) ## 4.1.0 diff --git a/source/modulo_controllers/include/modulo_controllers/ControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/ControllerInterface.hpp index ce56c72c8..941edc06e 100644 --- a/source/modulo_controllers/include/modulo_controllers/ControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/ControllerInterface.hpp @@ -615,6 +615,7 @@ class ControllerInterface : public controller_interface::ControllerInterface { std::timed_mutex command_mutex_; // TODO make missed_locks an internal parameter unsigned int missed_locks_; + bool on_init_called_; }; template diff --git a/source/modulo_controllers/src/ControllerInterface.cpp b/source/modulo_controllers/src/ControllerInterface.cpp index 2aa36d940..2a3d9e83c 100644 --- a/source/modulo_controllers/src/ControllerInterface.cpp +++ b/source/modulo_controllers/src/ControllerInterface.cpp @@ -21,9 +21,11 @@ namespace modulo_controllers { ControllerInterface::ControllerInterface(bool claim_all_state_interfaces) : controller_interface::ControllerInterface(), input_validity_period_(1.0), - claim_all_state_interfaces_(claim_all_state_interfaces) {} + claim_all_state_interfaces_(claim_all_state_interfaces), + on_init_called_(false) {} rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ControllerInterface::on_init() { + on_init_called_ = true; parameter_cb_handle_ = get_node()->add_on_set_parameters_callback( [this](const std::vector& parameters) -> rcl_interfaces::msg::SetParametersResult { return on_set_parameters_callback(parameters); @@ -52,6 +54,13 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Contro rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ControllerInterface::on_configure(const rclcpp_lifecycle::State&) { + if (!on_init_called_) { + RCLCPP_ERROR( + get_node()->get_logger(), + "The controller has not been properly initialized! Derived controller classes must call " + "'ControllerInterface::on_init()' during their own initialization before being configured."); + return CallbackReturn::ERROR; + } auto hardware_name = get_parameter("hardware_name"); if (hardware_name->is_empty()) { RCLCPP_ERROR(get_node()->get_logger(), "Parameter 'hardware_name' cannot be empty");