add skeleton for of service for registration of reference interfaces

Author: mamueluth

controller_manager/include/controller_manager/controller_manager.hpp

@@ -36,6 +36,7 @@
 #include "controller_manager_msgs/srv/list_hardware_interfaces.hpp"
 #include "controller_manager_msgs/srv/load_controller.hpp"
 #include "controller_manager_msgs/srv/register_sub_controller_manager.hpp"
+#include "controller_manager_msgs/srv/register_sub_controller_manager_references.hpp"
 #include "controller_manager_msgs/srv/reload_controller_libraries.hpp"
 #include "controller_manager_msgs/srv/set_hardware_component_state.hpp"
 #include "controller_manager_msgs/srv/switch_controller.hpp"
@@ -586,6 +587,8 @@ class ControllerManager : public rclcpp::Node
   std::mutex central_controller_manager_srv_lock_;
   rclcpp::Service<controller_manager_msgs::srv::RegisterSubControllerManager>::SharedPtr
     register_sub_controller_manager_srv_;
+  rclcpp::Service<controller_manager_msgs::srv::RegisterSubControllerManagerReferences>::SharedPtr
+    register_sub_controller_manager_references_srv_;
 
   std::vector<std::string> activate_request_, deactivate_request_;
   std::vector<std::string> to_chained_mode_request_, from_chained_mode_request_;

controller_manager/src/controller_manager.cpp

@@ -556,6 +556,14 @@ void ControllerManager::init_distributed_central_controller_manager_services()
         &ControllerManager::register_sub_controller_manager_srv_cb, this, std::placeholders::_1,
         std::placeholders::_2),
       qos_profile_services_keep_all, distributed_system_srv_callback_group_);
+
+  register_sub_controller_manager_references_srv_ =
+    create_service<controller_manager_msgs::srv::RegisterSubControllerManagerReferences>(
+      "register_sub_controller_manager_references",
+      std::bind(
+        &ControllerManager::register_sub_controller_manager_references_srv_cb, this,
+        std::placeholders::_1, std::placeholders::_2),
+      qos_profile_services_keep_all, distributed_system_srv_callback_group_);
 }
 
 void ControllerManager::register_sub_controller_manager_srv_cb(

controller_manager_msgs/CMakeLists.txt

@@ -22,6 +22,7 @@ set(srv_files
   srv/ListHardwareInterfaces.srv
   srv/LoadController.srv
   srv/RegisterSubControllerManager.srv
+  srv/RegisterSubControllerManagerReferences.srv
   srv/ReloadControllerLibraries.srv
   srv/SetHardwareComponentState.srv
   srv/SwitchController.srv

controller_manager_msgs/srv/RegisterSubControllerManagerReferences.srv

@@ -0,0 +1,19 @@
+# The RegisterSubControllerManagerReferences service allows a distributed controller
+# manager to register it's reference interfaces at the central controller manager
+
+# Req:
+# To register a controller the distributed sub controller manager has to 
+# define:
+#   * the full namespace it operates in 
+#   * all statePublishers the sub controller provides
+
+# Res:
+# The response includes:
+#   * a bool if the registration succeeded 
+
+string sub_controller_manager_namespace
+string sub_controller_manager_name
+controller_manager_msgs/PublisherDescription[] command_state_publishers
+---
+controller_manager_msgs/PublisherDescription[] command_state_publishers
+bool ok