Pass URDF to controllers on init (#1088)

* Pass URDF to controllers on init

* store the robot_description in the controller_manager class variable

* added minimal test to verify the update of robot description in controllers

* fix code formatting

* added test with loading and unloading the controller

* added changes as per bence's review comments

* added get_robot_description method to the base class

---------

Co-authored-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -114,7 +114,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
 
   CONTROLLER_INTERFACE_PUBLIC
   virtual return_type init(
-    const std::string & controller_name, const std::string & namespace_ = "",
+    const std::string & controller_name, const std::string & urdf,
+    const std::string & namespace_ = "",
     const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true));
 
   /// Custom configure method to read additional parameters for controller-nodes
@@ -155,6 +156,9 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   bool is_async() const;
 
+  CONTROLLER_INTERFACE_PUBLIC
+  const std::string & get_robot_description() const;
+
   /// Declare and initialize a parameter with a type.
   /**
    *
@@ -223,6 +227,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   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_;

controller_interface/src/controller_interface_base.cpp

@@ -25,11 +25,12 @@
 namespace controller_interface
 {
 return_type ControllerInterfaceBase::init(
-  const std::string & controller_name, const std::string & namespace_,
+  const std::string & controller_name, const std::string & urdf, const std::string & namespace_,
   const rclcpp::NodeOptions & node_options)
 {
   node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
     controller_name, namespace_, node_options, false);  // disable LifecycleNode service interfaces
+  urdf_ = urdf;
 
   try
   {
@@ -132,4 +133,6 @@ unsigned int ControllerInterfaceBase::get_update_rate() const { return update_ra
 
 bool ControllerInterfaceBase::is_async() const { return is_async_; }
 
+const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; }
+
 }  // namespace controller_interface

controller_interface/test/test_chainable_controller_interface.cpp

@@ -21,7 +21,7 @@ TEST_F(ChainableControllerInterfaceTest, default_returns)
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   EXPECT_TRUE(controller.is_chainable());
@@ -33,7 +33,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   auto reference_interfaces = controller.export_reference_interfaces();
@@ -50,7 +50,7 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correc
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   // expect empty return because storage is not resized
@@ -64,7 +64,7 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   controller.set_name_prefix_of_reference_interfaces("some_not_correct_interface_prefix");
@@ -79,7 +79,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   auto reference_interfaces = controller.export_reference_interfaces();
@@ -126,7 +126,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic)
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   EXPECT_FALSE(controller.is_in_chained_mode());

controller_interface/test/test_controller_interface.cpp

@@ -38,7 +38,7 @@ TEST(TestableControllerInterface, init)
   ASSERT_THROW(controller.get_node(), std::runtime_error);
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   // update_rate is set to default 0
@@ -60,7 +60,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
 
   TestableControllerInterface controller;
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
 
   // initialize executor to be able to get parameter update
   auto executor =
@@ -122,7 +122,7 @@ TEST(TestableControllerInterfaceInitError, init_with_error)
   TestableControllerInterfaceInitError controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::ERROR);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::ERROR);
 
   rclcpp::shutdown();
 }
@@ -136,7 +136,7 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure)
   TestableControllerInterfaceInitFailure controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::ERROR);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::ERROR);
 
   rclcpp::shutdown();
 }

controller_interface/test/test_controller_with_options.cpp

@@ -42,7 +42,7 @@ TEST(ControllerWithOption, init_with_overrides)
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::OK);
+  EXPECT_EQ(controller.init("controller_name", ""), controller_interface::return_type::OK);
   // checks that the node options have been updated
   const auto & node_options = controller.get_node()->get_node_options();
   EXPECT_TRUE(node_options.allow_undeclared_parameters());
@@ -63,7 +63,7 @@ TEST(ControllerWithOption, init_without_overrides)
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::ERROR);
+  EXPECT_EQ(controller.init("controller_name", ""), controller_interface::return_type::ERROR);
   // checks that the node options have been updated
   const auto & node_options = controller.get_node()->get_node_options();
   EXPECT_TRUE(node_options.allow_undeclared_parameters());

controller_interface/test/test_controller_with_options.hpp

@@ -40,13 +40,14 @@ class ControllerWithOptions : public controller_interface::ControllerInterface
   }
 
   controller_interface::return_type init(
-    const std::string & controller_name, const std::string & namespace_ = "",
+    const std::string & controller_name, const std::string & urdf,
+    const std::string & namespace_ = "",
     const rclcpp::NodeOptions & node_options =
       rclcpp::NodeOptions()
         .allow_undeclared_parameters(true)
         .automatically_declare_parameters_from_overrides(true)) override
   {
-    ControllerInterface::init(controller_name, namespace_, node_options);
+    ControllerInterface::init(controller_name, urdf, namespace_, node_options);
 
     switch (on_init())
     {

controller_manager/include/controller_manager/controller_manager.hpp

@@ -534,6 +534,7 @@ class ControllerManager : public rclcpp::Node
   std::vector<std::string> activate_command_interface_request_,
     deactivate_command_interface_request_;
 
+  std::string robot_description_;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
 
   struct SwitchParams

controller_manager/src/controller_manager.cpp

@@ -262,10 +262,10 @@ ControllerManager::ControllerManager(
     RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
   }
 
-  std::string robot_description = "";
+  robot_description_ = "";
   // TODO(destogl): remove support at the end of 2023
-  get_parameter("robot_description", robot_description);
-  if (robot_description.empty())
+  get_parameter("robot_description", robot_description_);
+  if (robot_description_.empty())
   {
     subscribe_to_robot_description_topic();
   }
@@ -275,7 +275,7 @@ ControllerManager::ControllerManager(
       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_resource_manager(robot_description_);
   }
 
   diagnostics_updater_.setHardwareID("ros2_control");
@@ -332,6 +332,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
   // 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(
@@ -340,7 +341,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
         "description file.");
       return;
     }
-    init_resource_manager(robot_description.data.c_str());
+    init_resource_manager(robot_description_);
   }
   catch (std::runtime_error & e)
   {
@@ -1290,7 +1291,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
   }
 
   if (
-    controller.c->init(controller.info.name, get_namespace()) ==
+    controller.c->init(controller.info.name, robot_description_, get_namespace()) ==
     controller_interface::return_type::ERROR)
   {
     to.clear();

controller_manager/test/test_controller/test_controller.hpp

@@ -30,6 +30,7 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
 // indicating the node name under which the controller node
 // is being loaded.
 constexpr char TEST_CONTROLLER_NAME[] = "test_controller_name";
+constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name";
 // corresponds to the name listed within the pluginlib xml
 constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_controller";
 class TestController : public controller_interface::ControllerInterface
@@ -65,6 +66,8 @@ class TestController : public controller_interface::ControllerInterface
   CONTROLLER_MANAGER_PUBLIC
   void set_state_interface_configuration(const controller_interface::InterfaceConfiguration & cfg);
 
+  const std::string & getRobotDescription() const;
+
   unsigned int internal_counter = 0;
   bool simulate_cleanup_failure = false;
   // Variable where we store when cleanup was called, pointer because the controller

controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp

@@ -32,8 +32,8 @@ TestControllerFailedInit::on_init()
 }
 
 controller_interface::return_type TestControllerFailedInit::init(
-  const std::string & /* controller_name */, const std::string & /*namespace_*/,
-  const rclcpp::NodeOptions & /*node_options*/)
+  const std::string & /* controller_name */, const std::string & /* urdf */,
+  const std::string & /*namespace_*/, const rclcpp::NodeOptions & /*node_options*/)
 {
   return controller_interface::return_type::ERROR;
 }

controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp

@@ -40,7 +40,8 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac
 
   CONTROLLER_INTERFACE_PUBLIC
   controller_interface::return_type init(
-    const std::string & controller_name, const std::string & namespace_ = "",
+    const std::string & controller_name, const std::string & urdf,
+    const std::string & namespace_ = "",
     const rclcpp::NodeOptions & node_options =
       rclcpp::NodeOptions()
         .allow_undeclared_parameters(true)

controller_manager/test/test_controller_manager.cpp

@@ -33,6 +33,34 @@ class TestControllerManagerWithStrictness
 {
 };
 
+class TestControllerManagerRobotDescription
+: public ControllerManagerFixture<controller_manager::ControllerManager>
+{
+};
+
+TEST_F(TestControllerManagerRobotDescription, controller_robot_description_update)
+{
+  auto test_controller = std::make_shared<test_controller::TestController>();
+  auto test_controller2 = std::make_shared<test_controller::TestController>();
+  cm_->add_controller(
+    test_controller, test_controller::TEST_CONTROLLER_NAME,
+    test_controller::TEST_CONTROLLER_CLASS_NAME);
+  ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description());
+
+  // Now change the robot description and then load a new controller and see if the new controller
+  // gets the new description and the old controller still maintains the configuration
+  auto msg = std_msgs::msg::String();
+  msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf;
+  cm_->robot_description_callback(msg);
+  cm_->add_controller(
+    test_controller2, test_controller::TEST_CONTROLLER2_NAME,
+    test_controller::TEST_CONTROLLER_CLASS_NAME);
+  ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description());
+  ASSERT_EQ(
+    ros2_control_test_assets::minimal_robot_missing_state_keys_urdf,
+    test_controller2->get_robot_description());
+}
+
 TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
 {
   const auto test_param = GetParam();

controller_manager/test/test_controller_manager_srvs.cpp

@@ -457,6 +457,47 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv)
   EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
 }
 
+TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controller)
+{
+  rclcpp::executors::SingleThreadedExecutor srv_executor;
+  rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
+  srv_executor.add_node(srv_node);
+  rclcpp::Client<controller_manager_msgs::srv::UnloadController>::SharedPtr unload_client =
+    srv_node->create_client<controller_manager_msgs::srv::UnloadController>(
+      "test_controller_manager/unload_controller");
+
+  auto test_controller = std::make_shared<TestController>();
+  auto abstract_test_controller = cm_->add_controller(
+    test_controller, test_controller::TEST_CONTROLLER_NAME,
+    test_controller::TEST_CONTROLLER_CLASS_NAME);
+  EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
+
+  // check the robot description
+  ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description());
+
+  // Now change the robot description and then see that the controller maintains the old URDF until
+  // it is unloaded and loaded again
+  auto msg = std_msgs::msg::String();
+  msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf;
+  cm_->robot_description_callback(msg);
+  ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description());
+
+  // now unload and load the controller and see if the controller gets the new robot description
+  auto unload_request = std::make_shared<controller_manager_msgs::srv::UnloadController::Request>();
+  unload_request->name = test_controller::TEST_CONTROLLER_NAME;
+  auto result = call_service_and_wait(*unload_client, unload_request, srv_executor, true);
+  EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
+
+  // now load it and check if it got the new robot description
+  cm_->add_controller(
+    test_controller, test_controller::TEST_CONTROLLER_NAME,
+    test_controller::TEST_CONTROLLER_CLASS_NAME);
+  EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
+  ASSERT_EQ(
+    ros2_control_test_assets::minimal_robot_missing_state_keys_urdf,
+    test_controller->get_robot_description());
+}
+
 TEST_F(TestControllerManagerSrvs, configure_controller_srv)
 {
   rclcpp::executors::SingleThreadedExecutor srv_executor;