feat: check that init of base class has been called (#99)

aica-technology · May 27, 2024 · 0eb6d27 · 0eb6d27
1 parent 383bed4
commit 0eb6d27
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 2 deletions.
diff --git 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
@@ -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<typename T>

diff --git 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<rclcpp::Parameter>& 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");