Use ControllerSpec for storing controller type and name

controller_manager/CMakeLists.txt

@@ -102,6 +102,7 @@ ament_export_include_directories(
 )
 ament_export_dependencies(
   controller_interface
+  controller_manager_msgs
   pluginlib
 )
 ament_package()

controller_manager/include/controller_manager/controller_manager.hpp

@@ -23,7 +23,9 @@
 #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 "hardware_interface/robot_hardware.hpp"
 
@@ -47,7 +49,7 @@ 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);
@@ -57,14 +59,7 @@ class ControllerManager : public rclcpp::Node
     const std::string & controller_name);
 
   CONTROLLER_MANAGER_PUBLIC
-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>>
-  get_loaded_controllers() const;
-
-  [[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;
+  std::vector<ControllerSpec> get_loaded_controllers() const;
 
   CONTROLLER_MANAGER_PUBLIC
   void register_controller_loader(ControllerLoaderInterfaceSharedPtr loader);
@@ -73,10 +68,16 @@ 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);
   }
 
   CONTROLLER_MANAGER_PUBLIC
@@ -108,10 +109,8 @@ class ControllerManager : public rclcpp::Node
 
 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::ControllerInterface * get_controller_by_name(const std::string & name);
@@ -128,6 +127,11 @@ class ControllerManager : public rclcpp::Node
   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);
+
 private:
   std::shared_ptr<hardware_interface::RobotHardware> hw_;
   std::shared_ptr<rclcpp::Executor> executor_;
@@ -139,7 +143,7 @@ class ControllerManager : public rclcpp::Node
    *\{*/
   /// Mutex protecting the current controllers list
   std::recursive_mutex controllers_lock_;
-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> controllers_lists_[2];
+  std::vector<ControllerSpec> controllers_lists_[2];
   /// The index of the current controllers list
   int current_controllers_list_ = {0};
   /// The index of the controllers list being used in the real-time thread.

controller_manager/include/controller_manager/controller_spec.hpp

@@ -0,0 +1,47 @@
+// Copyright 2017 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.
+
+
+/*
+ * Author: Wim Meeussen
+ */
+
+#ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
+#define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
+#pragma GCC diagnostic ignored "-Wextra"
+
+
+#include <map>
+#include <string>
+#include <vector>
+#include "controller_interface/controller_interface.hpp"
+#include "hardware_interface/controller_info.hpp"
+
+namespace controller_manager
+{
+
+/** \brief Controller Specification
+ *
+ * This struct contains both a pointer to a given controller, \ref c, as well
+ * as information about the controller, \ref info.
+ *
+ */
+struct ControllerSpec
+{
+  hardware_interface::ControllerInfo info;
+  controller_interface::ControllerInterfaceSharedPtr c;
+};
+
+}  // namespace controller_manager
+#endif  // CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_