Add check in controller interface to update 'update_rate' parameter only when doing 'configure' transition.

Author: destogl

controller_interface/src/controller_interface.cpp

@@ -19,7 +19,7 @@
 #include <utility>
 #include <vector>
 
-#include "hardware_interface/types/lifecycle_state_names.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
 
 namespace controller_interface
 {
@@ -73,9 +73,20 @@ return_type ControllerInterface::init(const std::string & controller_name)
 
 const rclcpp_lifecycle::State & ControllerInterface::configure()
 {
-  update_rate_ = node_->get_parameter("update_rate").as_int();
+  // TODO(destogl): this should actually happen in "on_configure" but I am not sure how to get
+  // overrides correctly in combination with std::bind. The goal is to have the following calls:
+  // 1. CM: controller.get_node()->configure()
+  // 2. LifecycleNode: ControllerInterface::on_configure()
+  // 3. ControllerInterface: <controller>::on_configure()
+  // Then we don't need to do state-machine related checks.
+  //
+  // Other solution is to add check into the LifecycleNode if a transition is valid to trigger
+  if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  {
+    update_rate_ = get_node()->get_parameter("update_rate").as_int();
+  }
 
-  return node_->configure();
+  return get_node()->configure();
 }
 
 void ControllerInterface::assign_interfaces(

controller_interface/test/test_controller_interface.cpp

@@ -83,10 +83,26 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
 
   // Test updating of update_rate parameter
   EXPECT_EQ(std::system("ros2 param set /testable_controller_interface update_rate 623"), 0);
+  // Keep the same update rate until transition from 'UNCONFIGURED' TO 'INACTIVE' does not happen
+  controller.configure();  // No transition so the update rate should stay intact
+  ASSERT_NE(controller.get_update_rate(), 623u);
   ASSERT_EQ(controller.get_update_rate(), 2812u);
+
   controller.get_node()->activate();
+  controller.configure();  // No transition so the update rate should stay intact
+  ASSERT_NE(controller.get_update_rate(), 623u);
+  ASSERT_EQ(controller.get_update_rate(), 2812u);
+
   controller.update(controller.get_node()->now(), rclcpp::Duration::from_seconds(0.1));
+  controller.configure();  // No transition so the update rate should stay intact
+  ASSERT_NE(controller.get_update_rate(), 623u);
+  ASSERT_EQ(controller.get_update_rate(), 2812u);
+
   controller.get_node()->deactivate();
+  controller.configure();  // No transition so the update rate should stay intact
+  ASSERT_NE(controller.get_update_rate(), 623u);
+  ASSERT_EQ(controller.get_update_rate(), 2812u);
+
   controller.get_node()->cleanup();
   ASSERT_EQ(controller.get_update_rate(), 2812u);
   // It is first changed after controller is configured again.