ros-controls · destogl · Jul 10, 2021 · Aug 27, 2021 · Jul 17, 2021 · Jul 17, 2021
diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt
@@ -157,6 +157,14 @@ if(BUILD_TESTING)
   target_include_directories(test_spawner_unspawner PRIVATE include)
   target_link_libraries(test_spawner_unspawner controller_manager test_controller)
   ament_target_dependencies(test_spawner_unspawner ros2_control_test_assets)
+
+  ament_add_gmock(
+    test_hardware_management_srvs
+    test/test_hardware_management_srvs.cpp
+  )
+  target_include_directories(test_hardware_management_srvs PRIVATE include)
+  target_link_libraries(test_hardware_management_srvs controller_manager test_controller)
+  ament_target_dependencies(test_hardware_management_srvs ros2_control_test_assets)
 endif()
 
 # Install Python modules

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -21,8 +21,11 @@
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
+#include "controller_manager_msgs/msg/hardware_components_state.hpp"
+#include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/state.hpp"
 
 namespace controller_manager
 {
@@ -74,10 +77,7 @@ ControllerManager::ControllerManager(
     throw std::runtime_error("Unable to initialize resource manager, no robot description found.");
   }
 
-  resource_manager_->load_urdf(robot_description);
-
-  // TODO(all): Here we should start only "auto-start" resources
-  resource_manager_->start_components();
+  init_resource_manager(robot_description);
 
   init_services();
 }
@@ -95,6 +95,32 @@ ControllerManager::ControllerManager(
   init_services();
 }
 
+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...
+  resource_manager_->load_urdf(robot_description);
+
+  using lifecycle_msgs::msg::State;
+
+  std::vector<std::string> autoconfigure_components = {""};
+  get_parameter("autoconfigure_components", autoconfigure_components);
+  rclcpp_lifecycle::State inactive_state(
-  rclcpp_lifecycle::State inactive_state(
+  const rclcpp_lifecycle::State inactive_state(
-  rclcpp_lifecycle::State inactive_state(
+  const rclcpp_lifecycle::State inactive_state(
+    State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE);
+  for (const auto & component : autoconfigure_components)
+  {
+    resource_manager_->set_component_state(component, inactive_state);
+  }
+
+  std::vector<std::string> autostart_components = {""};
+  get_parameter("autostart_components", autostart_components);
+  rclcpp_lifecycle::State active_state(
+    State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
+  for (const auto & component : autostart_components)
+  {
+    resource_manager_->set_component_state(component, active_state);
+  }
+}
+
 void ControllerManager::init_services()
 {
   // TODO(anyone): Due to issues with the MutliThreadedExecutor, this control loop does not rely on
@@ -444,7 +470,7 @@ controller_interface::return_type ControllerManager::switch_controller(
     std::vector<std::string> command_interface_names = {};
     if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
     {
-      command_interface_names = resource_manager_->command_interface_keys();
+      command_interface_names = resource_manager_->available_command_interfaces();
     }
     if (
       command_interface_config.type ==
@@ -590,7 +616,7 @@ controller_interface::return_type ControllerManager::switch_controller(
       auto command_interface_config = controller.c->command_interface_configuration();
       if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
       {
-        controller.info.claimed_interfaces = resource_manager_->command_interface_keys();
+        controller.info.claimed_interfaces = resource_manager_->available_command_interfaces();
       }
       if (
         command_interface_config.type ==
@@ -758,7 +784,7 @@ void ControllerManager::start_controllers()
     std::vector<std::string> command_interface_names = {};
     if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
     {
-      command_interface_names = resource_manager_->command_interface_keys();
+      command_interface_names = resource_manager_->available_command_interfaces();
     }
     if (
       command_interface_config.type ==
@@ -802,7 +828,7 @@ void ControllerManager::start_controllers()
     std::vector<std::string> state_interface_names = {};
     if (state_interface_config.type == controller_interface::interface_configuration_type::ALL)
     {
-      state_interface_names = resource_manager_->state_interface_keys();
+      state_interface_names = resource_manager_->available_state_interfaces();
     }
     if (
       state_interface_config.type == controller_interface::interface_configuration_type::INDIVIDUAL)
@@ -1155,6 +1181,42 @@ void ControllerManager::list_hardware_components_srv_cb(
   std::lock_guard<std::mutex> guard(services_lock_);
   RCLCPP_DEBUG(get_logger(), "list hardware components service locked");
 
+  auto hw_components_info = resource_manager_->get_components_status();
+
+  response->component.reserve(hw_components_info.size());
+
+  for (auto it = hw_components_info.begin(); it != hw_components_info.end(); ++it)
+  {
+    auto component = controller_manager_msgs::msg::HardwareComponentState();
+    component.name = it->second.name;
+    component.type = it->second.type;
+    component.class_type = it->second.class_type;
+    component.state.id = it->second.state.id();
+    component.state.label = it->second.state.label();
+
+    component.command_interfaces.reserve(it->second.command_interfaces.size());
+    for (const auto & interface : it->second.command_interfaces)
+    {
+      controller_manager_msgs::msg::HardwareInterface hwi;
+      hwi.name = interface;
+      hwi.is_available = resource_manager_->command_interface_is_available(interface);
+      hwi.is_claimed = resource_manager_->command_interface_is_claimed(interface);
+      component.command_interfaces.push_back(hwi);
+    }
+
+    component.state_interfaces.reserve(it->second.state_interfaces.size());
+    for (const auto & interface : it->second.state_interfaces)
+    {
+      controller_manager_msgs::msg::HardwareInterface hwi;
+      hwi.name = interface;
+      hwi.is_available = resource_manager_->state_interface_is_available(interface);
+      hwi.is_claimed = false;
+      component.state_interfaces.push_back(hwi);
+    }
+
+    response->component.push_back(component);
+  }
+
   RCLCPP_DEBUG(get_logger(), "list hardware components service finished");
 }
 
@@ -1171,6 +1233,7 @@ void ControllerManager::list_hardware_interfaces_srv_cb(
   {
     controller_manager_msgs::msg::HardwareInterface hwi;
     hwi.name = state_interface_name;
+    hwi.is_available = resource_manager_->state_interface_is_available(state_interface_name);
     hwi.is_claimed = false;
     response->state_interfaces.push_back(hwi);
   }
@@ -1179,6 +1242,7 @@ void ControllerManager::list_hardware_interfaces_srv_cb(
   {
     controller_manager_msgs::msg::HardwareInterface hwi;
     hwi.name = command_interface_name;
+    hwi.is_available = resource_manager_->command_interface_is_available(command_interface_name);
     hwi.is_claimed = resource_manager_->command_interface_is_claimed(command_interface_name);
     response->command_interfaces.push_back(hwi);
   }
@@ -1196,6 +1260,27 @@ void ControllerManager::set_hardware_component_state_srv_cb(
 
   RCLCPP_DEBUG(get_logger(), "set hardware component state '%s'", request->name.c_str());
 
+  auto hw_components_info = resource_manager_->get_components_status();
+  if (hw_components_info.find(request->name) != hw_components_info.end())
+  {
+    rclcpp_lifecycle::State target_state(
+      request->target_state.id,
+      // the ternary operator is needed because label in State constructor cannot be an empty string
+      request->target_state.label.empty() ? "-" : request->target_state.label);
+    response->ok =
+      (resource_manager_->set_component_state(request->name, target_state) ==
+       hardware_interface::return_type::OK);
+    hw_components_info = resource_manager_->get_components_status();
+    response->state.id = hw_components_info[request->name].state.id();
+    response->state.label = hw_components_info[request->name].state.label();
+  }
+  else
+  {
+    RCLCPP_ERROR(
+      get_logger(), "hardware component with name '%s' does not exist", request->name.c_str());
+    response->ok = false;
+  }
+
   RCLCPP_DEBUG(get_logger(), "set hardware component state service finished");
 }
 

diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp
@@ -38,6 +38,8 @@
 constexpr auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT;
 constexpr auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
 
+const auto TEST_CM_NAME = "test_controller_manager";
+
 class ControllerManagerFixture : public ::testing::Test
 {
 public:
@@ -50,8 +52,8 @@ class ControllerManagerFixture : public ::testing::Test
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
     cm_ = std::make_shared<controller_manager::ControllerManager>(
       std::make_unique<hardware_interface::ResourceManager>(
-        ros2_control_test_assets::minimal_robot_urdf),
-      executor_, "test_controller_manager");
+        ros2_control_test_assets::minimal_robot_urdf, true, true),
+      executor_, TEST_CM_NAME);
     run_updater_ = false;
   }
 
@@ -85,6 +87,65 @@ class ControllerManagerFixture : public ::testing::Test
   bool run_updater_;
 };
 
+class TestControllerManagerSrvs : public ControllerManagerFixture
+{
+public:
+  TestControllerManagerSrvs() {}
+
+  void SetUp() override
+  {
+    ControllerManagerFixture::SetUp();
+    SetUpSrvsCMExecutor();
+  }
+
+  void SetUpSrvsCMExecutor()
+  {
+    update_timer_ = cm_->create_wall_timer(std::chrono::milliseconds(10), [&]() {
+      cm_->read();
+      cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
+      cm_->write();
+    });
+
+    executor_->add_node(cm_);
+
+    executor_spin_future_ = std::async(std::launch::async, [this]() -> void { 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(std::chrono::milliseconds(50));
+  }
+
+  // FIXME: This can be deleted!
+  void TearDown() override { executor_->cancel(); }
+
+  template <typename T>
+  std::shared_ptr<typename T::Response> call_service_and_wait(
+    rclcpp::Client<T> & client, std::shared_ptr<typename T::Request> request,
+    rclcpp::Executor & service_executor, bool update_controller_while_spinning = false)
+  {
+    EXPECT_TRUE(client.wait_for_service(std::chrono::milliseconds(500)));
+    auto result = client.async_send_request(request);
+    // Wait for the result.
+    if (update_controller_while_spinning)
+    {
+      while (service_executor.spin_until_future_complete(result, std::chrono::milliseconds(50)) !=
+             rclcpp::FutureReturnCode::SUCCESS)
+      {
+        cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
+      }
+    }
+    else
+    {
+      EXPECT_EQ(
+        service_executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS);
+    }
+    return result.get();
+  }
+
+protected:
+  rclcpp::TimerBase::SharedPtr update_timer_;
+  std::future<void> executor_spin_future_;
+};
+
 class ControllerManagerRunner
 {
 public: