Add controller manager services

controller_manager/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(ament_cmake)
 find_package(ament_cmake_core REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(controller_interface REQUIRED)
+find_package(controller_manager_msgs REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -23,6 +24,7 @@ target_include_directories(controller_manager PRIVATE include)
 ament_target_dependencies(controller_manager
   ament_index_cpp
   controller_interface
+  controller_manager_msgs
   hardware_interface
   pluginlib
   rclcpp
@@ -62,7 +64,7 @@ if(BUILD_TESTING)
   target_link_libraries(test_controller controller_manager)
   target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
 
-  ament_add_gtest(
+  ament_add_gmock(
     test_controller_manager
     test/test_controller_manager.cpp
   )
@@ -85,6 +87,18 @@ if(BUILD_TESTING)
     test_robot_hardware
   )
 
+  ament_add_gmock(
+    test_controller_manager_srvs
+    test/test_controller_manager_srvs.cpp
+    APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$<CONFIG>
+  )
+  target_include_directories(test_controller_manager_srvs PRIVATE include)
+  target_link_libraries(test_controller_manager_srvs controller_manager test_controller)
+  ament_target_dependencies(
+    test_controller_manager_srvs
+    test_robot_hardware
+  )
+
   pluginlib_export_plugin_description_file(controller_interface test/test_controller.xml)
 
   install(TARGETS test_controller
@@ -100,6 +114,7 @@ ament_export_include_directories(
 )
 ament_export_dependencies(
   controller_interface
+  controller_manager_msgs
   pluginlib
 )
 ament_package()

controller_manager/include/controller_manager/controller_loader_interface.hpp

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "controller_interface/controller_interface.hpp"
 
@@ -29,7 +30,8 @@ class ControllerLoaderInterface
 {
 public:
   CONTROLLER_MANAGER_PUBLIC
-  ControllerLoaderInterface() = default;
+  explicit ControllerLoaderInterface(const std::string & name)
+  : name_(name) {}
 
   CONTROLLER_MANAGER_PUBLIC
   virtual ~ControllerLoaderInterface() = default;
@@ -40,6 +42,18 @@ class ControllerLoaderInterface
 
   CONTROLLER_MANAGER_PUBLIC
   virtual bool is_available(const std::string & controller_type) const = 0;
+
+  CONTROLLER_MANAGER_PUBLIC
+  const std::string & getName() const {return name_;}
+
+  CONTROLLER_MANAGER_PUBLIC
+  virtual std::vector<std::string> getDeclaredClasses() = 0;
+
+  CONTROLLER_MANAGER_PUBLIC
+  virtual void reload() = 0;
+
+private:
+  const std::string name_;
 };
 
 using ControllerLoaderInterfaceSharedPtr = std::shared_ptr<ControllerLoaderInterface>;

controller_manager/include/controller_manager/controller_loader_pluginlib.hpp

@@ -19,6 +19,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "pluginlib/class_loader.hpp"
 
@@ -40,6 +41,12 @@ class ControllerLoaderPluginlib : public ControllerLoaderInterface
   CONTROLLER_MANAGER_PUBLIC
   bool is_available(const std::string & controller_type) const;
 
+  CONTROLLER_MANAGER_PUBLIC
+  std::vector<std::string> getDeclaredClasses() override;
+
+  CONTROLLER_MANAGER_PUBLIC
+  void reload() override;
+
 private:
   std::shared_ptr<pluginlib::ClassLoader<controller_interface::ControllerInterface>> loader_;
 };

controller_manager/include/controller_manager/controller_manager.hpp

@@ -23,7 +23,14 @@
 #include "controller_interface/controller_interface.hpp"
 
 #include "controller_manager/controller_loader_interface.hpp"
+#include "controller_manager/controller_spec.hpp"
 #include "controller_manager/visibility_control.h"
+#include "controller_manager_msgs/srv/list_controllers.hpp"
+#include "controller_manager_msgs/srv/list_controller_types.hpp"
+#include "controller_manager_msgs/srv/load_controller.hpp"
+#include "controller_manager_msgs/srv/reload_controller_libraries.hpp"
+#include "controller_manager_msgs/srv/switch_controller.hpp"
+#include "controller_manager_msgs/srv/unload_controller.hpp"
 
 #include "hardware_interface/robot_hardware.hpp"
 
@@ -36,6 +43,9 @@ namespace controller_manager
 class ControllerManager : public rclcpp::Node
 {
 public:
+  static constexpr bool WAIT_FOR_ALL_RESOURCES = false;
+  static constexpr double INFINITE_TIMEOUT = 0.0;
+
   CONTROLLER_MANAGER_PUBLIC
   ControllerManager(
     std::shared_ptr<hardware_interface::RobotHardware> hw,
@@ -47,20 +57,25 @@ class ControllerManager : public rclcpp::Node
   ~ControllerManager() = default;
 
   CONTROLLER_MANAGER_PUBLIC
-  std::shared_ptr<controller_interface::ControllerInterface>
+  controller_interface::ControllerInterfaceSharedPtr
   load_controller(
     const std::string & controller_name,
     const std::string & controller_type);
 
+  /**
+   * @brief load_controller loads a controller by name, the type must be defined in the parameter server
+   */
   CONTROLLER_MANAGER_PUBLIC
-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>>
-  get_loaded_controllers() const;
+  controller_interface::ControllerInterfaceSharedPtr
+  load_controller(
+    const std::string & controller_name);
 
-  [[deprecated(
-    "get_loaded_controller is deprecated, it has been renamed to get_loaded_controllers")]]
   CONTROLLER_MANAGER_PUBLIC
-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>>
-  get_loaded_controller() const;
+  controller_interface::return_type unload_controller(
+    const std::string & controller_name);
+
+  CONTROLLER_MANAGER_PUBLIC
+  std::vector<ControllerSpec> get_loaded_controllers() const;
 
   CONTROLLER_MANAGER_PUBLIC
   void register_controller_loader(ControllerLoaderInterfaceSharedPtr loader);
@@ -69,44 +84,208 @@ class ControllerManager : public rclcpp::Node
     typename T,
     typename std::enable_if<std::is_convertible<
       T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr>
-  std::shared_ptr<controller_interface::ControllerInterface>
-  add_controller(std::shared_ptr<T> controller, std::string controller_name)
+  controller_interface::ControllerInterfaceSharedPtr
+  add_controller(
+    std::shared_ptr<T> controller, std::string controller_name,
+    std::string controller_type)
   {
-    return add_controller_impl(controller, controller_name);
+    ControllerSpec controller_spec;
+    controller_spec.c = controller;
+    controller_spec.info.name = controller_name;
+    controller_spec.info.type = controller_type;
+    return add_controller_impl(controller_spec);
   }
 
+  /**
+   * @brief switch_controller Stops some controllers and others.
+   * @see Documentation in controller_manager_msgs/SwitchController.srv
+   */
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::return_type
+  switch_controller(
+    const std::vector<std::string> & start_controllers,
+    const std::vector<std::string> & stop_controllers,
+    int strictness, bool start_asap = WAIT_FOR_ALL_RESOURCES,
+    const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT));
+
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::return_type
   update();
 
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::return_type
-  configure() const;
+  configure();
 
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::return_type
-  activate() const;
+  activate();
 
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::return_type
-  deactivate() const;
+  deactivate();
 
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::return_type
-  cleanup() const;
+  cleanup();
 
 protected:
   CONTROLLER_MANAGER_PUBLIC
-  std::shared_ptr<controller_interface::ControllerInterface>
-  add_controller_impl(
-    std::shared_ptr<controller_interface::ControllerInterface> controller,
-    const std::string & controller_name);
+  controller_interface::ControllerInterfaceSharedPtr
+  add_controller_impl(const ControllerSpec & controller);
+
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::ControllerInterfaceSharedPtr get_controller_by_name(
+    const std::string & name);
+
+  CONTROLLER_MANAGER_PUBLIC
+  void manage_switch();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void stop_controllers();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void start_controllers();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void start_controllers_asap();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void list_controllers_srv_cb(
+    const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,
+    std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response);
+
+  CONTROLLER_MANAGER_PUBLIC
+  void list_controller_types_srv_cb(
+    const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,
+    std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);
+
+  CONTROLLER_MANAGER_PUBLIC
+  void load_controller_service_cb(
+    const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,
+    std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response);
+
+  CONTROLLER_MANAGER_PUBLIC
+  void reload_controller_libraries_service_cb(
+    const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,
+    std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response);
+
+  CONTROLLER_MANAGER_PUBLIC
+  void switch_controller_service_cb(
+    const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,
+    std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response);
+
+  CONTROLLER_MANAGER_PUBLIC
+  void unload_controller_service_cb(
+    const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,
+    std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);
 
 private:
+  void get_controller_names(std::vector<std::string> & names);
+
+
   std::shared_ptr<hardware_interface::RobotHardware> hw_;
   std::shared_ptr<rclcpp::Executor> executor_;
   std::vector<ControllerLoaderInterfaceSharedPtr> loaders_;
-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> loaded_controllers_;
+
+  /**
+   * @brief The RTControllerListWrapper class wraps a  double-buffered list of controllers
+   * to avoid needing to lock the  real-time thread when switching controllers in
+   * the non-real-time thread.
+   *
+   * There's always an "updated" list and an "outdated" one
+   * There's always an "used by rt" list and an "unused by rt" list
+   *
+   * The updated state changes on the switch_updated_list()
+   * The rt usage state changes on the get_used_by_rt_list()
+   */
+  class RTControllerListWrapper
+  {
+public:
+    /**
+     * @brief get_used_by_rt_list Makes the "updated" list the "used by rt" list
+     * @warning Should only be called by the RT thread, noone should modify the
+     * updated list while it's being used
+     * @return reference to the updated list
+     */
+    std::vector<ControllerSpec> & get_used_by_rt_list();
+
+    /**
+     * @brief get_unused_list Waits until the "outdated" and "unused by rt"
+     *  lists match and returns a reference to it
+     * This referenced list can be modified safely until switch_updated_controller_list()
+     * is called, at this point the RT thread may start using it at any time
+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list
+     */
+    std::vector<ControllerSpec> & get_unused_list(
+      const std::lock_guard<std::recursive_mutex> & guard);
+
+    /**
+     * @brief get_updated_list Returns a const reference to the most updated list,
+     * @warning May or may not being used by the realtime thread, read-only reference for safety
+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list
+     */
+    const std::vector<ControllerSpec> & get_updated_list(
+      const std::lock_guard<std::recursive_mutex> & guard) const;
+
+    /**
+     * @brief switch_updated_list Switches the "updated" and "outdated" lists, and waits
+     *  until the RT thread is using the new "updated" list.
+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list
+     */
+    void switch_updated_list(const std::lock_guard<std::recursive_mutex> & guard);
+
+
+    // Mutex protecting the controllers list
+    // must be acquired before using any list other than the "used by rt"
+    mutable std::recursive_mutex controllers_lock_;
+
+private:
+    int get_other_list(int index) const;
+
+    void wait_until_rt_not_using(int index) const;
+
+    std::vector<ControllerSpec> controllers_lists_[2];
+    /// The index of the controller list with the most updated information
+    int updated_controllers_index_ = {0};
+    /// The index of the controllers list being used in the real-time thread.
+    int used_by_realtime_controllers_index_ = {-1};
+  };
+
+  RTControllerListWrapper rt_controllers_wrapper_;
+  /// mutex copied from ROS1 Control, protects service callbacks
+  /// not needed if we're guaranteed that the callbacks don't come from multiple threads
+  std::mutex services_lock_;
+  rclcpp::Service<controller_manager_msgs::srv::ListControllers>::SharedPtr
+    list_controllers_service_;
+  rclcpp::Service<controller_manager_msgs::srv::ListControllerTypes>::SharedPtr
+    list_controller_types_service_;
+  rclcpp::Service<controller_manager_msgs::srv::LoadController>::SharedPtr
+    load_controller_service_;
+  rclcpp::Service<controller_manager_msgs::srv::ReloadControllerLibraries>::SharedPtr
+    reload_controller_libraries_service_;
+  rclcpp::Service<controller_manager_msgs::srv::SwitchController>::SharedPtr
+    switch_controller_service_;
+  rclcpp::Service<controller_manager_msgs::srv::UnloadController>::SharedPtr
+    unload_controller_service_;
+
+  std::vector<controller_interface::ControllerInterfaceSharedPtr> start_request_, stop_request_;
+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING
+//  std::list<hardware_interface::ControllerInfo> switch_start_list_, switch_stop_list_;
+#endif
+
+  struct SwitchParams
+  {
+    bool do_switch = {false};
+    bool started = {false};
+    rclcpp::Time init_time = {rclcpp::Time::max()};
+
+    // Switch options
+    int strictness = {0};
+    bool start_asap = {false};
+    rclcpp::Duration timeout = rclcpp::Duration{0, 0};
+  };
+
+  SwitchParams switch_params_;
 };
 
 }  // namespace controller_manager