Fix missed initialization

Author: VitezGabriela

controller_manager/include/controller_manager/controller_manager.hpp

@@ -96,6 +96,8 @@ class ControllerManager : public rclcpp::Node
 
   void init_resource_manager(const std::string & robot_description);
 
+  void set_initial_hardware_components_state();
+
   controller_interface::ControllerInterfaceBaseSharedPtr load_controller(
     const std::string & controller_name, const std::string & controller_type);
 

controller_manager/src/controller_manager.cpp

@@ -482,8 +482,7 @@ ControllerManager::ControllerManager(
   {
     initialize_parameters();
     init_controller_manager();
-    resource_manager_->set_on_component_state_switch_callback(
-      std::bind(&ControllerManager::publish_activity, this));
+    set_initial_hardware_components_state();
   }
   else
   {
@@ -665,6 +664,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
   }
 
   init_resource_manager(robot_description_);
+  set_initial_hardware_components_state();
   if (!is_resource_manager_initialized())
   {
     resource_manager_ =
@@ -723,6 +723,15 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
       get_logger(), "Exception caught while loading and initializing components: %s", e.what());
     return;
   }
+
+  if (robot_description_notification_timer_)
+  {
+    robot_description_notification_timer_->cancel();
+  }
+}
+
+void ControllerManager::set_initial_hardware_components_state()
+{
   resource_manager_->set_on_component_state_switch_callback(
     std::bind(&ControllerManager::publish_activity, this));
 
@@ -815,7 +824,6 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
       }
     }
   }
-  robot_description_notification_timer_->cancel();
   auto hw_components_info = resource_manager_->get_components_status();
 
   for (const auto & [component_name, component_info] : hw_components_info)

controller_manager/test/test_controller_manager_with_resource_manager.cpp

@@ -99,7 +99,7 @@ TEST_F(ControllerManagerTest, robot_description_callback_handles_nonexistent_plu
 
 TEST_F(ControllerManagerTest, robot_description_callback_handles_no_geometry)
 {
-  const std::string invalid_urdf = ros2_control_test_assets::minimal_robot_urdf_no_limits;
+  const std::string invalid_urdf = ros2_control_test_assets::invalid_urdf_no_geometry;
 
   TestControllerManager cm(
     executor_, invalid_urdf, false, "test_controller_manager", "", rclcpp::NodeOptions{});