Merge branch 'master' into add/sw_joint_limiter

Author: saikishor

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -258,12 +258,12 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
 protected:
   std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
   std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
-  unsigned int update_rate_ = 0;
-  bool is_async_ = false;
-  std::string urdf_ = "";
 
 private:
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
+  unsigned int update_rate_ = 0;
+  bool is_async_ = false;
+  std::string urdf_ = "";
 };
 
 using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;

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(
@@ -251,21 +220,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_);
-  }
-
-  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);
+  init_controller_manager();
 }
 
 ControllerManager::ControllerManager(
@@ -282,6 +237,12 @@ 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(
@@ -292,15 +253,17 @@ ControllerManager::ControllerManager(
   {
     init_services();
   }
-  subscribe_to_robot_description_topic();
-
-  diagnostics_updater_.setHardwareID("ros2_control");
-  diagnostics_updater_.add(
-    "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
-}
+  else
+  {
+    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::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 +272,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,13 +289,16 @@ 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_);
   if (is_resource_manager_initialized())
   {
+    RCLCPP_INFO(
+      get_logger(),
+      "Resource Manager has been successfully initialized. Starting Controller Manager "
+      "services...");
     init_services();
   }
 }
@@ -397,6 +368,7 @@ 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);
   }
+  robot_description_notification_timer_->cancel();
 }
 
 void ControllerManager::init_services()
@@ -898,6 +870,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: