Enable controller manager services to control hardware lifecycle #abi…

…-breaking (ros-controls#637)

* Implement CM services for hardware lifecycle management.
* Added default behavior to activate all controller and added description of CM parameters.

controller_manager/CMakeLists.txt

@@ -150,6 +150,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

controller_manager/doc/userdoc.rst

@@ -6,6 +6,39 @@ Controller Manager is the main component in the ros2_control framework.
 It manages lifecycle of controllers, access to the hardware interfaces and offers services to the ROS-world.
 
 
+Parameters
+-----------
+
+activate_components_on_start (optional; list<string>; default: empty)
+  Define which hardware components should be activated when controller manager is started.
+  The names of the components are defined as attribute of ``<ros2_control>``-tag in ``robot_description``.
+  All other components will stay ``UNCONFIGURED``.
+  If this and ``configure_components_on_start`` are empty, all available components will be activated.
+  If this or ``configure_components_on_start`` are not empty, any component not in either list will be in unconfigured state.
+
+
+configure_components_on_start (optional; list<string>; default: empty)
+  Define which hardware components should be configured when controller manager is started.
+  The names of the components are defined as attribute of ``<ros2_control>``-tag in ``robot_description``.
+  All other components will stay ``UNCONFIGURED``.
+  If this and ``activate_components_on_start`` are empty, all available components will be activated.
+  If this or ``activate_components_on_start`` are not empty, any component not in either list will be in unconfigured state.
+
+
+robot_description (mandatory; string)
+  String with the URDF string as robot description.
+  This is usually result of the parsed description files by ``xacro`` command.
+
+update_rate (mandatory; double)
+  The frequency of controller manager's real-time update loop.
+  This loop reads states from hardware, updates controller and writes commands to hardware.
+
+
+<controller_name>.type
+  Name of a plugin exported using ``pluginlib`` for a controller.
+  This is a class from which controller's instance with name "``controller_name``" is created.
+
+
 Helper scripts
 --------------
 There are two scripts to interact with controller manager from launch files:

controller_manager/src/controller_manager.cpp

@@ -21,8 +21,11 @@
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
+#include "controller_manager_msgs/msg/hardware_component_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
 {  // utility
@@ -103,10 +106,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();
 }
@@ -121,11 +121,42 @@ ControllerManager::ControllerManager(
   loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
     kControllerInterfaceName, kControllerInterface))
 {
-  resource_manager_->start_components();
-
   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> configure_components_on_start = std::vector<std::string>({});
+  get_parameter("configure_components_on_start", configure_components_on_start);
+  rclcpp_lifecycle::State inactive_state(
+    State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE);
+  for (const auto & component : configure_components_on_start)
+  {
+    resource_manager_->set_component_state(component, inactive_state);
+  }
+
+  std::vector<std::string> activate_components_on_start = std::vector<std::string>({});
+  get_parameter("activate_components_on_start", activate_components_on_start);
+  rclcpp_lifecycle::State active_state(
+    State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
+  for (const auto & component : activate_components_on_start)
+  {
+    resource_manager_->set_component_state(component, active_state);
+  }
+
+  // if both parameter are empty or non-existing preserve behavior where all components are
+  // activated per default
+  if (configure_components_on_start.empty() && activate_components_on_start.empty())
+  {
+    resource_manager_->activate_all_components();
+  }
+}
+
 void ControllerManager::init_services()
 {
   // TODO(anyone): Due to issues with the MutliThreadedExecutor, this control loop does not rely on
@@ -1184,12 +1215,48 @@ void ControllerManager::unload_controller_service_cb(
 
 void ControllerManager::list_hardware_components_srv_cb(
   const std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Request>,
-  std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Response> /*response*/)
+  std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Response> response)
 {
   RCLCPP_DEBUG(get_logger(), "list hardware components service called");
   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 (const auto & [component_name, component_info] : hw_components_info)
+  {
+    auto component = controller_manager_msgs::msg::HardwareComponentState();
+    component.name = component_name;
+    component.type = component_info.type;
+    component.class_type = component_info.class_type;
+    component.state.id = component_info.state.id();
+    component.state.label = component_info.state.label();
+
+    component.command_interfaces.reserve(component_info.command_interfaces.size());
+    for (const auto & interface : component_info.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(component_info.state_interfaces.size());
+    for (const auto & interface : component_info.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");
 }
 
@@ -1225,14 +1292,35 @@ void ControllerManager::list_hardware_interfaces_srv_cb(
 
 void ControllerManager::set_hardware_component_state_srv_cb(
   const std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Request> request,
-  std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Response> /*response*/)
+  std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Response> response)
 {
   RCLCPP_DEBUG(get_logger(), "set hardware component state service called");
   std::lock_guard<std::mutex> guard(services_lock_);
   RCLCPP_DEBUG(get_logger(), "set hardware component state service locked");
 
   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");
 }
 

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:

controller_manager/test/test_controller_manager_srvs.cpp

@@ -18,73 +18,18 @@
 #include <string>
 #include <vector>
 
+#include "controller_manager_test_common.hpp"
+
 #include "controller_interface/controller_interface.hpp"
 #include "controller_manager/controller_manager.hpp"
 #include "controller_manager_msgs/srv/list_controller_types.hpp"
 #include "controller_manager_msgs/srv/list_controllers.hpp"
 #include "controller_manager_msgs/srv/switch_controller.hpp"
-#include "controller_manager_test_common.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 
 using ::testing::_;
 using ::testing::Return;
 
-using namespace std::chrono_literals;
-
-class TestControllerManagerSrvs : public ControllerManagerFixture
-{
-public:
-  TestControllerManagerSrvs() {}
-
-  void SetUp() override
-  {
-    ControllerManagerFixture::SetUp();
-
-    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(50ms);
-  }
-
-  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, 50ms) !=
-             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_;
-};
-
 TEST_F(TestControllerManagerSrvs, list_controller_types)
 {
   rclcpp::executors::SingleThreadedExecutor srv_executor;
@@ -198,8 +143,8 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv)
   ASSERT_THAT(
     result->controller[0].claimed_interfaces,
     testing::ElementsAre(
-      "joint1/position", "joint1/max_velocity", "joint2/velocity", "joint3/velocity",
-      "joint2/max_acceleration", "configuration/max_tcp_jerk"));
+      "joint2/velocity", "joint3/velocity", "joint2/max_acceleration", "configuration/max_tcp_jerk",
+      "joint1/position", "joint1/max_velocity"));
   ASSERT_THAT(
     result->controller[0].required_command_interfaces,
     testing::ElementsAre(