Pass URDF to controllers on init

ros-controls · Jul 26, 2023 · c80c1e6 · c80c1e6
1 parent 757ccd4
commit c80c1e6
Show file tree

Hide file tree

Showing 8 changed files with 25 additions and 21 deletions.
diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp
@@ -114,8 +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 rclcpp::NodeOptions & node_options = rclcpp::NodeOptions());
+    const std::string & controller_name, const std::string & urdf,
+    const std::string & namespace_ = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions());
 
   /// Custom configure method to read additional parameters for controller-nodes
   /*
@@ -223,6 +223,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_;

diff --git a/controller_interface/src/controller_interface_base.cpp b/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 rclcpp::NodeOptions & node_options)
+  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
   {

diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/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());

diff --git a/controller_interface/test/test_controller_interface.cpp b/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();
 }

diff --git a/controller_interface/test/test_controller_with_options.cpp b/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());

diff --git a/controller_interface/test/test_controller_with_options.hpp b/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())
     {

diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp b/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;
 }

diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp b/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)