[CM] Use robot_description topic instead of parameter and don't crash on empty URDF 🦿

controller_manager/CMakeLists.txt

@@ -14,6 +14,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   pluginlib
   rclcpp
   realtime_tools
+  std_msgs
 )
 
 find_package(ament_cmake REQUIRED)
@@ -143,6 +144,16 @@ if(BUILD_TESTING)
   ament_target_dependencies(test_controller_manager_srvs
     controller_manager_msgs
   )
+  ament_add_gmock(test_controller_manager_urdf_passing
+  test/test_controller_manager_urdf_passing.cpp
+  )
+  target_link_libraries(test_controller_manager_urdf_passing
+    controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+  ament_target_dependencies(test_controller_manager_urdf_passing
+    controller_manager_msgs
+  )
 
   add_library(test_controller_with_interfaces SHARED
     test/test_controller_with_interfaces/test_controller_with_interfaces.cpp

controller_manager/include/controller_manager/controller_manager.hpp

@@ -51,6 +51,8 @@
 #include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
 #include "rclcpp/parameter.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
 
 namespace controller_manager
 {
@@ -82,6 +84,12 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   virtual ~ControllerManager() = default;
 
+  CONTROLLER_MANAGER_PUBLIC
+  void subscribe_to_robot_description_topic();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void robot_description_callback(const std_msgs::msg::String & msg);
+
   CONTROLLER_MANAGER_PUBLIC
   void init_resource_manager(const std::string & robot_description);
 
@@ -504,6 +512,8 @@ class ControllerManager : public rclcpp::Node
   std::vector<std::string> activate_command_interface_request_,
     deactivate_command_interface_request_;
 
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
+
   struct SwitchParams
   {
     bool do_switch = {false};

controller_manager/package.xml

@@ -26,6 +26,7 @@
   <depend>ros2_control_test_assets</depend>
   <depend>ros2param</depend>
   <depend>ros2run</depend>
+  <depend>std_msgs</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ros2_control_test_assets</test_depend>

controller_manager/src/controller_manager.cpp

@@ -151,13 +151,20 @@ ControllerManager::ControllerManager(
   }
 
   std::string robot_description = "";
+  // TODO(Manuel): robot_description parameter is deprecated and should be removed.
   get_parameter("robot_description", robot_description);
   if (robot_description.empty())
   {
-    throw std::runtime_error("Unable to initialize resource manager, no robot description found.");
+    subscribe_to_robot_description_topic();
+  }
+  else
+  {
+    RCLCPP_WARN(
+      get_logger(),
+      "[Deprecated] Passing the robot description file directly to the control_manager node is "
+      "deprecated. Use robot_state_publisher instead.");
+    init_resource_manager(robot_description);
   }
-
-  init_resource_manager(robot_description);
 
   diagnostics_updater_.setHardwareID("ros2_control");
   diagnostics_updater_.add(
@@ -183,12 +190,55 @@ ControllerManager::ControllerManager(
   {
     RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
   }
+
+  subscribe_to_robot_description_topic();
+
   diagnostics_updater_.setHardwareID("ros2_control");
   diagnostics_updater_.add(
     "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
   init_services();
 }
 
+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)
+  RCLCPP_INFO_STREAM(
+    get_logger(), "Subscribing to '~/robot_description' topic for robot description file.");
+  robot_description_subscription_ = create_subscription<std_msgs::msg::String>(
+    "~/robot_description", rclcpp::QoS(1).transient_local(),
+    std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1));
+}
+
+void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description)
+{
+  RCLCPP_INFO(get_logger(), "Received robot description file.");
+  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
+  {
+    if (resource_manager_->load_urdf_called())
+    {
+      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.data.c_str());
+  }
+  catch (std::runtime_error & e)
+  {
+    RCLCPP_ERROR_STREAM(
+      get_logger(),
+      "The published robot description file (urdf) seems not to be genuine. The following error "
+      "was caught:"
+        << e.what());
+  }
+}
+
 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...

controller_manager/test/controller_manager_test_common.hpp

@@ -22,15 +22,20 @@
 #include <memory>
 #include <string>
 #include <thread>
+#include <utility>
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
 
 #include "controller_manager/controller_manager.hpp"
+#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp"
 #include "controller_manager_msgs/srv/switch_controller.hpp"
 
+#include "rclcpp/rclcpp.hpp"
 #include "rclcpp/utilities.hpp"
 
+#include "std_msgs/msg/string.hpp"
+
 #include "ros2_control_test_assets/descriptions.hpp"
 #include "test_controller_failed_init/test_controller_failed_init.hpp"
 
@@ -60,21 +65,51 @@ template <typename CtrlMgr>
 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)
+  {
+    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);
+    }
+    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);
+      }
+    }
+  }
+
   static void SetUpTestCase() { rclcpp::init(0, nullptr); }
 
   static void TearDownTestCase() { rclcpp::shutdown(); }
 
-  void SetUp()
-  {
-    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-    cm_ = std::make_shared<CtrlMgr>(
-      std::make_unique<hardware_interface::ResourceManager>(
-        ros2_control_test_assets::minimal_robot_urdf, true, true),
-      executor_, TEST_CM_NAME);
-    run_updater_ = false;
-  }
+  void SetUp() override { run_updater_ = false; }
 
-  void TearDown() { stopCmUpdater(); }
+  void TearDown() override { stopCmUpdater(); }
 
   void startCmUpdater()
   {
@@ -120,6 +155,8 @@ class ControllerManagerFixture : public ::testing::Test
 
   std::thread updater_;
   bool run_updater_;
+  const std::string robot_description_;
+  const bool pass_urdf_as_parameter_;
 };
 
 class TestControllerManagerSrvs

controller_manager/test/test_controller_manager_urdf_passing.cpp

@@ -0,0 +1,88 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "controller_manager/controller_manager.hpp"
+#include "controller_manager_test_common.hpp"
+
+#include "ros2_control_test_assets/descriptions.hpp"
+
+class TestControllerManagerWithTestableCM;
+
+class TestableControllerManager : public controller_manager::ControllerManager
+{
+  friend TestControllerManagerWithTestableCM;
+
+  FRIEND_TEST(TestControllerManagerWithTestableCM, initial_no_load_urdf_called);
+  FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
+  FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed);
+  FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
+  FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
+
+public:
+  TestableControllerManager(
+    std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
+    std::shared_ptr<rclcpp::Executor> executor,
+    const std::string & manager_node_name = "controller_manager",
+    const std::string & namespace_ = "")
+  : controller_manager::ControllerManager(
+      std::move(resource_manager), executor, manager_node_name, namespace_)
+  {
+  }
+};
+
+class TestControllerManagerWithTestableCM
+: public ControllerManagerFixture<TestableControllerManager>,
+  public testing::WithParamInterface<Strictness>
+{
+public:
+  // create cm with no urdf
+  TestControllerManagerWithTestableCM()
+  : ControllerManagerFixture<TestableControllerManager>("", false)
+  {
+  }
+};
+
+TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called)
+{
+  ASSERT_FALSE(cm_->resource_manager_->load_urdf_called());
+}
+
+TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback)
+{
+  ASSERT_FALSE(cm_->resource_manager_->load_urdf_called());
+  // mimic callback
+  auto msg = std_msgs::msg::String();
+  msg.data = ros2_control_test_assets::minimal_robot_urdf;
+  cm_->robot_description_callback(msg);
+  ASSERT_TRUE(cm_->resource_manager_->load_urdf_called());
+}
+
+TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed)
+{
+  ASSERT_FALSE(cm_->resource_manager_->load_urdf_called());
+  // mimic callback
+  auto msg = std_msgs::msg::String();
+  msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf;
+  cm_->robot_description_callback(msg);
+  ASSERT_TRUE(cm_->resource_manager_->load_urdf_called());
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  test_best_effort, TestControllerManagerWithTestableCM, testing::Values(best_effort));

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -90,6 +90,16 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    */
   void load_urdf(const std::string & urdf, bool validate_interfaces = true);
 
+  /**
+   * @brief if the resource manager load_urdf(...) function has been called this returns true.
+   * We want to permit to load the urdf later on but we currently don't want to permit multiple
+   * calls to load_urdf (reloading/loading different urdf).
+   *
+   * @return true if resource manager's load_urdf() has been already called.
+   * @return false if resource manager's load_urdf() has not been yet called.
+   */
+  bool load_urdf_called() const;
+
   /// Claim a state interface given its key.
   /**
    * The resource is claimed as long as being in scope.
@@ -413,6 +423,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
 
   // Structure to store read and write status so it is not initialized in the real-time loop
   HardwareReadWriteStatus read_write_status;
+
+  bool load_urdf_called_ = false;
 };
 
 }  // namespace hardware_interface

hardware_interface/src/resource_manager.cpp

@@ -703,6 +703,7 @@ ResourceManager::ResourceManager(
 // CM API: Called in "callback/slow"-thread
 void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces)
 {
+  load_urdf_called_ = true;
   const std::string system_type = "system";
   const std::string sensor_type = "sensor";
   const std::string actuator_type = "actuator";
@@ -741,6 +742,8 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac
     resource_storage_->systems_.size());
 }
 
+bool ResourceManager::load_urdf_called() const { return load_urdf_called_; }
+
 // CM API: Called in "update"-thread
 LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key)
 {