Added functionality and tests for getting robot description from the …

…topic.

controller_manager/CMakeLists.txt

@@ -150,6 +150,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/include/controller_manager/controller_manager.hpp

@@ -322,7 +322,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"
@@ -546,6 +546,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/test/test_spawner_unspawner.cpp

@@ -31,6 +31,7 @@ using ::testing::Return;
 using namespace std::chrono_literals;
 class TestLoadController : public ControllerManagerFixture<controller_manager::ControllerManager>
 {
+public:
   void SetUp() override
   {
     ControllerManagerFixture::SetUp();
@@ -274,3 +275,82 @@ TEST_F(TestLoadController, unload_on_kill)
 
   ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul);
 }
+
+class TestLoadControllerWithoutRobotDescription
+: public ControllerManagerFixture<controller_manager::ControllerManager>
+{
+public:
+  TestLoadControllerWithoutRobotDescription()
+  : ControllerManagerFixture<controller_manager::ControllerManager>("")
+  {
+  }
+
+  void SetUp() override
+  {
+    ControllerManagerFixture::SetUp();
+
+    update_timer_ = cm_->create_wall_timer(
+      std::chrono::milliseconds(10),
+      [&]()
+      {
+        cm_->read(time_, PERIOD);
+        cm_->update(time_, PERIOD);
+        cm_->write(time_, PERIOD);
+      });
+
+    update_executor_ =
+      std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::ExecutorOptions(), 2);
+
+    update_executor_->add_node(cm_);
+    update_executor_spin_future_ =
+      std::async(std::launch::async, [this]() -> void { update_executor_->spin(); });
+
+    // This sleep is needed to prevent a too fast test from ending before the
+    // executor has began to spin, which causes it to hang
+    std::this_thread::sleep_for(50ms);
+  }
+
+  void TearDown() override { update_executor_->cancel(); }
+
+  rclcpp::TimerBase::SharedPtr robot_description_sending_timer_;
+
+protected:
+  rclcpp::TimerBase::SharedPtr update_timer_;
+
+  // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks
+  std::shared_ptr<rclcpp::Executor> update_executor_;
+  std::future<void> update_executor_spin_future_;
+};
+
+TEST_F(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spawner_times_out)
+{
+  cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+
+  ControllerManagerRunner cm_runner(this);
+  EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 1)
+    << "could not spawn controller because not robot description and not services for controller "
+       "manager are active";
+}
+
+TEST_F(
+  TestLoadControllerWithoutRobotDescription,
+  controller_starting_with_later_load_of_robot_description)
+{
+  cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+
+  // Delay sending robot description
+  robot_description_sending_timer_ = cm_->create_wall_timer(
+    std::chrono::milliseconds(1500), [&]() { pass_robot_description_to_cm_and_rm(); });
+
+  ControllerManagerRunner cm_runner(this);
+  EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 1)
+    << "could not activate control because not robot description";
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
+  {
+    auto ctrl_1 = cm_->get_loaded_controllers()[0];
+    ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
+    ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+    ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+  }
+}

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -88,6 +88,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * interfaces ought to be validated. Defaults to true.
    * \param[in] load_and_initialize_components boolean argument indicating whether to load and
    * initialize the components present in the parsed URDF. Defaults to true.
+   * \returns false if URDF validation has failed.
    */
   bool load_and_initialize_components(
     const std::string & urdf, bool validate_interfaces = true,
@@ -408,7 +409,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
   bool state_interface_exists(const std::string & key) const;
 
 private:
-  void validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;
+  bool validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;
 
   void release_command_interface(const std::string & key);
 

hardware_interface/src/resource_manager.cpp

@@ -60,7 +60,7 @@ auto trigger_and_print_hardware_state_transition =
   }
   else
   {
-    RCUTILS_LOG_INFO_NAMED(
+    RCUTILS_LOG_WARN_NAMED(
       "resource_manager", "Failed to '%s' hardware '%s'", transition_name.c_str(),
       hardware_name.c_str());
   }
@@ -433,6 +433,7 @@ class ResourceStorage
   template <class HardwareT>
   void import_state_interfaces(HardwareT & hardware)
   {
+    // TODO(destogl): add here exception catch block - this can otherwise crash the whole system
     auto interfaces = hardware.export_state_interfaces();
     std::vector<std::string> interface_names;
     interface_names.reserve(interfaces.size());
@@ -450,6 +451,7 @@ class ResourceStorage
   template <class HardwareT>
   void import_command_interfaces(HardwareT & hardware)
   {
+    // TODO(destogl): add here exception catch block - this can otherwise crash the whole system
     auto interfaces = hardware.export_command_interfaces();
     hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces);
   }
@@ -551,7 +553,7 @@ class ResourceStorage
       }
       else
       {
-        RCUTILS_LOG_WARN_NAMED(
+        RCUTILS_LOG_ERROR_NAMED(
           "resource_manager",
           "Sensor hardware component '%s' from plugin '%s' failed to initialize.",
           hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str());
@@ -793,13 +795,15 @@ bool ResourceManager::load_and_initialize_components(
   // throw on missing state and command interfaces, not specified keys are being ignored
   if (validate_interfaces)
   {
-    validate_storage(hardware_info);
+    result = validate_storage(hardware_info);
   }
 
   std::lock_guard<std::recursive_mutex> guard(resources_lock_);
   read_write_status.failed_hardware_names.reserve(
     resource_storage_->actuators_.size() + resource_storage_->sensors_.size() +
     resource_storage_->systems_.size());
+
+  return result;
 }
 
 bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded_; }
@@ -1433,7 +1437,7 @@ bool ResourceManager::state_interface_exists(const std::string & key) const
 
 // BEGIN: private methods
 
-void ResourceManager::validate_storage(
+bool ResourceManager::validate_storage(
   const std::vector<hardware_interface::HardwareInfo> & hardware_info) const
 {
   std::vector<std::string> missing_state_keys = {};
@@ -1489,20 +1493,28 @@ void ResourceManager::validate_storage(
 
   if (!missing_state_keys.empty() || !missing_command_keys.empty())
   {
-    std::string err_msg = "Wrong state or command interface configuration.\n";
-    err_msg += "missing state interfaces:\n";
+    std::string message = "Wrong state or command interface configuration.\n";
+    message += "missing state interfaces:\n";
     for (const auto & missing_key : missing_state_keys)
     {
-      err_msg += "' " + missing_key + " '" + "\t";
+      message += "' " + missing_key + " '" + "\t";
     }
-    err_msg += "\nmissing command interfaces:\n";
+    message += "\nmissing command interfaces:\n";
     for (const auto & missing_key : missing_command_keys)
     {
-      err_msg += "' " + missing_key + " '" + "\t";
+      message += "' " + missing_key + " '" + "\t";
     }
 
-    throw std::runtime_error(err_msg);
+    RCUTILS_LOG_ERROR_NAMED(
+      "resource_manager",
+      "Discrepancy between robot description file (urdf) and actually exported HW interfaces."
+      "Details: %s",
+      message.c_str());
+
+    return false;
   }
+
+  return true;
 }
 
 // END: private methods

hardware_interface_testing/test/test_resource_manager.cpp

@@ -207,13 +207,11 @@ TEST_F(ResourceManagerTest, no_load_and_initialize_components_function_called)
   ASSERT_FALSE(rm.is_urdf_already_loaded());
 }
 
-TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_valid)
+TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_invalid)
 {
   TestableResourceManager rm;
-  ASSERT_FALSE(
-    rm.load_and_initialize_components(
-      ros2_control_test_assets::minimal_robot_missing_state_keys_urdf),
-    std::exception);
+  ASSERT_FALSE(rm.load_and_initialize_components(
+    ros2_control_test_assets::minimal_robot_missing_state_keys_urdf));
   ASSERT_FALSE(rm.is_urdf_already_loaded());
 }