[CM] Use robot_description topic instead of parameter and don't cra…

…sh on empty URDF 🦿 (backport #940) (#1052)

controller_manager/CMakeLists.txt

@@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
     pluginlib
     rclcpp
     realtime_tools
+    std_msgs
 )
 
 find_package(ament_cmake REQUIRED)
@@ -133,6 +134,17 @@ if(BUILD_TESTING)
     controller_manager_msgs
     ros2_control_test_assets
   )
+  ament_add_gmock(test_controller_manager_urdf_passing
+    test/test_controller_manager_urdf_passing.cpp
+  )
+  target_include_directories(test_controller_manager_urdf_passing PRIVATE include)
+  target_link_libraries(test_controller_manager_urdf_passing
+    controller_manager
+  )
+  ament_target_dependencies(test_controller_manager_urdf_passing
+    controller_manager_msgs
+    ros2_control_test_assets
+  )
 
   add_library(test_controller_with_interfaces SHARED test/test_controller_with_interfaces/test_controller_with_interfaces.cpp)
   target_include_directories(test_controller_with_interfaces PRIVATE include)

controller_manager/include/controller_manager/controller_manager.hpp

@@ -50,6 +50,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
 {
@@ -81,6 +83,9 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   virtual ~ControllerManager() = default;
 
+  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);
 
@@ -282,6 +287,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();
 
   /**
    * Clear request lists used when switching controllers. The lists are shared between "callback"
@@ -490,6 +496,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

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

controller_manager/src/controller_manager.cpp

@@ -270,10 +270,16 @@ ControllerManager::ControllerManager(
   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 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);
 
   init_services();
 }
@@ -295,9 +301,54 @@ ControllerManager::ControllerManager(
   {
     RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
   }
+
+  if (!resource_manager_->is_urdf_already_loaded())
+  {
+    subscribe_to_robot_description_topic();
+  }
   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(
+    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_->is_urdf_already_loaded())
+    {
+      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()
   {
@@ -121,6 +156,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_->is_urdf_already_loaded());
+}
+
+TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback)
+{
+  ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
+  // 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_->is_urdf_already_loaded());
+}
+
+TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed)
+{
+  ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
+  // 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_->is_urdf_already_loaded());
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  test_best_effort, TestControllerManagerWithTestableCM, testing::Values(best_effort));

hardware_interface/doc/mock_components_userdoc.rst

@@ -27,7 +27,12 @@ Features:
 Parameters
 ,,,,,,,,,,
 
-fake_sensor_commands (optional; boolean; default: false)
+disable_commands (optional; boolean; default: false)
+  Disables mirroring commands to states.
+  This option is helpful to simulate an erroneous connection to the hardware when nothing breaks, but suddenly there is no feedback from a hardware interface.
+  Or it can help you to test your setup when the hardware is running without feedback, i.e., in open loop configuration.
+
+mock_sensor_commands (optional; boolean; default: false)
   Creates fake command interfaces for faking sensor measurements with an external command.
   Those interfaces are usually used by a :ref:`forward controller <forward_command_controller_userdoc>` to provide access from ROS-world.
 

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -77,6 +77,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 is_urdf_already_loaded() const;
+
   /// Claim a state interface given its key.
   /**
    * The resource is claimed as long as being in scope.
@@ -374,6 +384,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
   mutable std::recursive_mutex resource_interfaces_lock_;
   mutable std::recursive_mutex claimed_command_interfaces_lock_;
   std::unique_ptr<ResourceStorage> resource_storage_;
+
+  bool is_urdf_loaded__ = false;
 };
 
 }  // namespace hardware_interface

hardware_interface/src/resource_manager.cpp

@@ -667,6 +667,7 @@ ResourceManager::ResourceManager(
 
 void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces)
 {
+  is_urdf_loaded__ = true;
   const std::string system_type = "system";
   const std::string sensor_type = "sensor";
   const std::string actuator_type = "actuator";
@@ -700,6 +701,9 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac
   }
 }
 
+bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; }
+
+// CM API: Called in "update"-thread
 LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key)
 {
   if (!state_interface_is_available(key))