[CM] Remove accepting robot description using parameter and harden the behavior when getting it from topic.

controller_manager/CMakeLists.txt

@@ -150,6 +150,7 @@ if(BUILD_TESTING)
   )
   target_link_libraries(test_controller_manager_urdf_passing
     controller_manager
+    test_controller
     ros2_control_test_assets::ros2_control_test_assets
   )
   ament_target_dependencies(test_controller_manager_urdf_passing

controller_manager/include/controller_manager/controller_manager.hpp

@@ -322,7 +322,7 @@ class ControllerManager : public rclcpp::Node
   std::vector<std::string> get_controller_names();
   std::pair<std::string, std::string> split_command_interface(
     const std::string & command_interface);
-  void subscribe_to_robot_description_topic();
+  void init_controller_manager();
 
   /**
    * Clear request lists used when switching controllers. The lists are shared between "callback"
@@ -546,6 +546,7 @@ class ControllerManager : public rclcpp::Node
 
   std::string robot_description_;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
+  rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;
 
   struct SwitchParams
   {

controller_manager/src/controller_manager.cpp

@@ -267,31 +267,7 @@ ControllerManager::ControllerManager(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
       kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
 {
-  if (!get_parameter("update_rate", update_rate_))
-  {
-    RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
-  }
-
-  robot_description_ = "";
-  // TODO(destogl): remove support at the end of 2023
-  get_parameter("robot_description", robot_description_);
-  if (robot_description_.empty())
-  {
-    subscribe_to_robot_description_topic();
-  }
-  else
-  {
-    RCLCPP_WARN(
-      get_logger(),
-      "[Deprecated] Passing the robot description parameter directly to the control_manager node "
-      "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.");
-    init_resource_manager(robot_description_);
-    init_services();
-  }
-
-  diagnostics_updater_.setHardwareID("ros2_control");
-  diagnostics_updater_.add(
-    "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
+  init_controller_manager();
 }
 
 ControllerManager::ControllerManager(
@@ -308,24 +284,17 @@ ControllerManager::ControllerManager(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
       kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
 {
+  init_controller_manager();
+}
+
+void ControllerManager::init_controller_manager()
+{
+  // Get parameters needed for RT "update" loop to work
   if (!get_parameter("update_rate", update_rate_))
   {
     RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
   }
 
-  if (resource_manager_->is_urdf_already_loaded())
-  {
-    init_services();
-  }
-  subscribe_to_robot_description_topic();
-
-  diagnostics_updater_.setHardwareID("ros2_control");
-  diagnostics_updater_.add(
-    "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
-}
-
-void ControllerManager::subscribe_to_robot_description_topic()
-{
   // set QoS to transient local to get messages that have already been published
   // (if robot state publisher starts before controller manager)
   robot_description_subscription_ = create_subscription<std_msgs::msg::String>(
@@ -334,43 +303,48 @@ void ControllerManager::subscribe_to_robot_description_topic()
   RCLCPP_INFO(
     get_logger(), "Subscribing to '%s' topic for robot description.",
     robot_description_subscription_->get_topic_name());
+
+  // Setup diagnostics
+  diagnostics_updater_.setHardwareID("ros2_control");
+  diagnostics_updater_.add(
+    "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
+
+  robot_description_notification_timer_ = create_wall_timer(
+    std::chrono::seconds(1),
+    [&]()
+    {
+      RCLCPP_WARN(
+        get_logger(), "Waiting for data on '~/robot_description' topic to finish initialization");
+    });
 }
 
 void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description)
 {
   RCLCPP_INFO(get_logger(), "Received robot description from topic.");
   RCLCPP_DEBUG(
     get_logger(), "'Content of robot description file: %s", robot_description.data.c_str());
-  // TODO(Manuel): errors should probably be caught since we don't want controller_manager node
-  // to die if a non valid urdf is passed. However, should maybe be fine tuned.
-  try
-  {
-    robot_description_ = robot_description.data;
-    if (resource_manager_->is_urdf_already_loaded())
-    {
-      RCLCPP_WARN(
-        get_logger(),
-        "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot "
-        "description file.");
-      return;
-    }
-    init_resource_manager(robot_description_);
-    init_services();
-  }
-  catch (std::runtime_error & e)
+  robot_description_ = robot_description.data;
+  if (resource_manager_->is_urdf_already_loaded())
   {
-    RCLCPP_ERROR_STREAM(
+    RCLCPP_WARN(
       get_logger(),
-      "The published robot description file (urdf) seems not to be genuine. The following error "
-      "was caught:"
-        << e.what());
+      "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot "
+      "description file.");
+    return;
   }
+  init_resource_manager(robot_description_);
 }
 
 void ControllerManager::init_resource_manager(const std::string & robot_description)
 {
-  // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong...
-  resource_manager_->load_urdf(robot_description);
+  if (!resource_manager_->load_urdf(robot_description))
+  {
+    RCLCPP_WARN(
+      get_logger(),
+      "URDF validation went wrong check the previous output. This might only mean that interfaces "
+      "defined in URDF and exported by the hardware do not match. Therefore continue initializing "
+      "controller manager...");
+  }
 
   // Get all components and if they are not defined in parameters activate them automatically
   auto components_to_activate = resource_manager_->get_components_status();
@@ -414,57 +388,22 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
       State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED));
 
   // inactive (configured)
-  // BEGIN: Keep old functionality on for backwards compatibility (Remove at the end of 2023)
-  std::vector<std::string> configure_components_on_start = std::vector<std::string>({});
-  get_parameter("configure_components_on_start", configure_components_on_start);
-  if (!configure_components_on_start.empty())
-  {
-    RCLCPP_WARN(
-      get_logger(),
-      "Parameter 'configure_components_on_start' is deprecated. "
-      "Use 'hardware_components_initial_state.inactive' instead, to set component's initial "
-      "state to 'inactive'. Don't use this parameters in combination with the new "
-      "'hardware_components_initial_state' parameter structure.");
-    set_components_to_state(
-      "configure_components_on_start",
-      rclcpp_lifecycle::State(
-        State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE));
-  }
-  // END: Keep old functionality on humble backwards compatibility (Remove at the end of 2023)
-  else
-  {
-    set_components_to_state(
-      "hardware_components_initial_state.inactive",
-      rclcpp_lifecycle::State(
-        State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE));
-  }
+  set_components_to_state(
+    "hardware_components_initial_state.inactive",
+    rclcpp_lifecycle::State(
+      State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE));
 
-  // BEGIN: Keep old functionality on for backwards compatibility (Remove at the end of 2023)
-  std::vector<std::string> activate_components_on_start = std::vector<std::string>({});
-  get_parameter("activate_components_on_start", activate_components_on_start);
+  // activate all other components
   rclcpp_lifecycle::State active_state(
     State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
-  if (!activate_components_on_start.empty())
+  for (const auto & [component, state] : components_to_activate)
   {
-    RCLCPP_WARN(
-      get_logger(),
-      "Parameter 'activate_components_on_start' is deprecated. "
-      "Components are activated per default. Don't use this parameters in combination with the new "
-      "'hardware_components_initial_state' parameter structure.");
-    for (const auto & component : activate_components_on_start)
-    {
-      resource_manager_->set_component_state(component, active_state);
-    }
-  }
-  // END: Keep old functionality on humble for backwards compatibility (Remove at the end of 2023)
-  else
-  {
-    // activate all other components
-    for (const auto & [component, state] : components_to_activate)
-    {
-      resource_manager_->set_component_state(component, active_state);
-    }
+    resource_manager_->set_component_state(component, active_state);
   }
+
+  // Init CM services first after the URDF is loaded an components are set
+  init_services();
+  robot_description_notification_timer_->cancel();
 }
 
 void ControllerManager::init_services()
@@ -838,6 +777,15 @@ controller_interface::return_type ControllerManager::switch_controller(
   const std::vector<std::string> & deactivate_controllers, int strictness, bool activate_asap,
   const rclcpp::Duration & timeout)
 {
+  if (!resource_manager_->is_urdf_already_loaded())
+  {
+    RCLCPP_ERROR(
+      get_logger(),
+      "Resource Manager is not initialized yet! Please provide robot description on "
+      "'~/robot_description' topic before trying to switch controllers.");
+    return controller_interface::return_type::ERROR;
+  }
+
   switch_params_ = SwitchParams();
 
   if (!deactivate_request_.empty() || !activate_request_.empty())
@@ -877,14 +825,15 @@ controller_interface::return_type ControllerManager::switch_controller(
     strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
   }
 
-  RCLCPP_DEBUG(get_logger(), "Switching controllers:");
+  RCLCPP_DEBUG(get_logger(), "Activating controllers:");
   for (const auto & controller : activate_controllers)
   {
-    RCLCPP_DEBUG(get_logger(), "- Activating controller '%s'", controller.c_str());
+    RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str());
   }
+  RCLCPP_DEBUG(get_logger(), "Deactivating controllers:");
   for (const auto & controller : deactivate_controllers)
   {
-    RCLCPP_DEBUG(get_logger(), "- Deactivating controller '%s'", controller.c_str());
+    RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str());
   }
 
   const auto list_controllers = [this, strictness](
@@ -1222,18 +1171,24 @@ controller_interface::return_type ControllerManager::switch_controller(
     return controller_interface::return_type::OK;
   }
 
-  if (
-    !activate_command_interface_request_.empty() || !deactivate_command_interface_request_.empty())
+  RCLCPP_DEBUG(get_logger(), "Request for command interfaces from activating controllers:");
+  for (const auto & interface : activate_command_interface_request_)
   {
-    if (!resource_manager_->prepare_command_mode_switch(
-          activate_command_interface_request_, deactivate_command_interface_request_))
-    {
-      RCLCPP_ERROR(
-        get_logger(),
-        "Could not switch controllers since prepare command mode switch was rejected.");
-      clear_requests();
-      return controller_interface::return_type::ERROR;
-    }
+    RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str());
+  }
+  RCLCPP_DEBUG(get_logger(), "Request for command interfaces from deactivating controllers:");
+  for (const auto & interface : deactivate_command_interface_request_)
+  {
+    RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str());
+  }
+
+  if (!resource_manager_->prepare_command_mode_switch(
+        activate_command_interface_request_, deactivate_command_interface_request_))
+  {
+    RCLCPP_ERROR(
+      get_logger(), "Could not switch controllers since prepare command mode switch was rejected.");
+    clear_requests();
+    return controller_interface::return_type::ERROR;
   }
 
   // start the atomic controller switching

controller_manager/test/controller_manager_test_common.hpp

@@ -65,40 +65,16 @@ class ControllerManagerFixture : public ::testing::Test
 {
 public:
   explicit ControllerManagerFixture(
-    const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf,
-    const bool & pass_urdf_as_parameter = false)
-  : robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter)
+    const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf)
+  : robot_description_(robot_description)
   {
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-    // We want to be able to create a ResourceManager where no urdf file has been passed to
-    if (robot_description_.empty())
+    cm_ = std::make_shared<CtrlMgr>(
+      std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
+    // We want to be able to not pass robot description immediately
+    if (!robot_description_.empty())
     {
-      cm_ = std::make_shared<CtrlMgr>(
-        std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
-    }
-    else
-    {
-      // can be removed later, needed if we want to have the deprecated way of passing the robot
-      // description file to the controller manager covered by tests
-      if (pass_urdf_as_parameter_)
-      {
-        cm_ = std::make_shared<CtrlMgr>(
-          std::make_unique<hardware_interface::ResourceManager>(robot_description_, true, true),
-          executor_, TEST_CM_NAME);
-      }
-      else
-      {
-        // TODO(Manuel) : passing via topic not working in test setup, tested cm does
-        // not receive msg. Have to check this...
-
-        // this is just a workaround to skip passing
-        cm_ = std::make_shared<CtrlMgr>(
-          std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
-        // mimic topic call
-        auto msg = std_msgs::msg::String();
-        msg.data = robot_description_;
-        cm_->robot_description_callback(msg);
-      }
+      pass_robot_description_to_cm_and_rm(robot_description_);
     }
     time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type());
   }
@@ -134,6 +110,17 @@ class ControllerManagerFixture : public ::testing::Test
     }
   }
 
+  void pass_robot_description_to_cm_and_rm(
+    const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf)
+  {
+    // TODO(Manuel) : passing via topic not working in test setup, tested cm does
+    // not receive msg. Have to check this...
+    // this is just a workaround to skip passing - mimic topic call
+    auto msg = std_msgs::msg::String();
+    msg.data = robot_description;
+    cm_->robot_description_callback(msg);
+  }
+
   void switch_test_controllers(
     const std::vector<std::string> & start_controllers,
     const std::vector<std::string> & stop_controllers, const int strictness,
@@ -156,7 +143,6 @@ class ControllerManagerFixture : public ::testing::Test
   std::thread updater_;
   bool run_updater_;
   const std::string robot_description_;
-  const bool pass_urdf_as_parameter_;
   rclcpp::Time time_;
 };