[CM] Remove support for the description parameter and use only robot_description topic

controller_manager/CMakeLists.txt

@@ -152,6 +152,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/doc/userdoc.rst

@@ -48,7 +48,7 @@ jitter, using a lowlatency kernel can improve things a lot with being really eas
 Subscribers
 -----------
 
-~/robot_description [std_msgs::msg::String]
+robot_description [std_msgs::msg::String]
  String with the URDF xml, e.g., from ``robot_state_publisher``.
  Reloading of the URDF is not supported yet.
  All joints defined in the ``<ros2_control>``-tag have to be present in the URDF.

controller_manager/include/controller_manager/controller_manager.hpp

@@ -356,7 +356,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"
@@ -585,6 +585,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

@@ -200,38 +200,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 of %d Hz.", update_rate_);
- }
-
- 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 the 'robot_description' topic from 'robot_state_publisher' instead.");
- init_resource_manager(robot_description_);
- if (is_resource_manager_initialized())
- {
- RCLCPP_WARN(
- get_logger(),
- "You have to restart the framework when using robot description from parameter!");
- init_services();
- }
- }
-
- diagnostics_updater_.setHardwareID("ros2_control");
- diagnostics_updater_.add(
- "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
+ init_controller_manager();
 }
 
 ControllerManager::ControllerManager(
@@ -261,7 +230,6 @@ ControllerManager::ControllerManager(
  {
  init_services();
  }
- subscribe_to_robot_description_topic();
 
  diagnostics_updater_.setHardwareID("ros2_control");
  diagnostics_updater_.add(
@@ -282,25 +250,30 @@ 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 of %d Hz.", update_rate_);
  }
 
+ 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");
+ });
  if (is_resource_manager_initialized())
  {
  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>(
@@ -309,6 +282,11 @@ 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);
 }
 
 void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description)
@@ -321,8 +299,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
  {
  RCLCPP_WARN(
  get_logger(),
- "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot "
- "description file.");
+ "ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description.");
  return;
  }
  init_resource_manager(robot_description_);
@@ -397,6 +374,10 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
  State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
  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()
@@ -898,6 +879,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 (!is_resource_manager_initialized())
+ {
+ 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())

controller_manager/test/controller_manager_test_common.hpp

@@ -65,46 +65,18 @@ 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>(
+ rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
+ 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>(
- rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
- 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_, rm_node_->get_node_clock_interface(),
- rm_node_->get_node_logging_interface(), true, 100),
- executor_, TEST_CM_NAME);
- }
- else
- {
- // TODO(mamueluth) : 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>(
- rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
- 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());
  }
@@ -140,6 +112,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,
@@ -162,7 +145,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_;
 
 protected: