Add own qualitiy of service and register command forwarders

Author: mamueluth

controller_manager/src/controller_manager.cpp

@@ -53,6 +53,10 @@ static const rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = {
   RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
   false};
 
+rclcpp::QoSInitialization qos_profile_services_keep_all_persist_init(
+  RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1);
+rclcpp::QoS qos_profile_services_keep_all(qos_profile_services_keep_all_persist_init);
+
 inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller)
 {
   return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
@@ -344,7 +348,7 @@ void ControllerManager::init_distributed_main_controller_services()
       std::bind(
         &ControllerManager::register_sub_controller_manager_srv_cb, this, std::placeholders::_1,
         std::placeholders::_2),
-      rmw_qos_profile_services_hist_keep_all, distributed_system_srv_callback_group_);
+      qos_profile_services_keep_all, distributed_system_srv_callback_group_);
 }
 
 void ControllerManager::register_sub_controller_manager_srv_cb(
@@ -362,6 +366,7 @@ void ControllerManager::register_sub_controller_manager_srv_cb(
 
   std::vector<std::shared_ptr<hardware_interface::DistributedReadOnlyHandle>>
     distributed_state_interfaces;
+  distributed_state_interfaces.reserve(sub_ctrl_mng_wrapper->get_state_publisher_count());
   distributed_state_interfaces =
     resource_manager_->import_state_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper);
 
@@ -384,6 +389,7 @@ void ControllerManager::register_sub_controller_manager_srv_cb(
 
   std::vector<std::shared_ptr<hardware_interface::DistributedReadWriteHandle>>
     distributed_command_interfaces;
+  distributed_command_interfaces.reserve(sub_ctrl_mng_wrapper->get_command_forwarder_count());
   distributed_command_interfaces =
     resource_manager_->import_command_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper);
 
@@ -392,14 +398,6 @@ void ControllerManager::register_sub_controller_manager_srv_cb(
     try
     {
       executor_->add_node(command_interface->get_node()->get_node_base_interface());
-      auto msg = controller_manager_msgs::msg::PublisherDescription();
-      msg.ns = get_namespace();
-      msg.name.prefix_name = command_interface->get_prefix_name();
-      msg.name.interface_name = command_interface->get_interface_name();
-      // want topic name relative to namespace
-      msg.publisher_topic = std::string(get_namespace()) + std::string("/") +
-                            command_interface->forward_command_topic_name();
-      response->command_state_publishers.push_back(msg);
     }
     catch (const std::runtime_error & e)
     {
@@ -410,6 +408,13 @@ void ControllerManager::register_sub_controller_manager_srv_cb(
         "Exception:"
           << e.what());
     }
+    auto msg = controller_manager_msgs::msg::PublisherDescription();
+    msg.ns = get_namespace();
+    msg.name.prefix_name = command_interface->get_prefix_name();
+    msg.name.interface_name = command_interface->get_interface_name();
+    // TODO(Manuel) want topic name relative to namespace, but have to treat "root" namespace separate
+    msg.publisher_topic = std::string("/") + command_interface->forward_command_topic_name();
+    response->command_state_publishers.push_back(msg);
   }
 
   response->ok = true;
@@ -462,7 +467,7 @@ void ControllerManager::add_hardware_command_forwarders()
 
 void ControllerManager::register_sub_controller_manager()
 {
-  RCLCPP_INFO(get_logger(), "SubControllerManager:Trying to register StatePublishers.");
+  RCLCPP_INFO(get_logger(), "SubControllerManager: Trying to register StatePublishers.");
   rclcpp::Client<controller_manager_msgs::srv::RegisterSubControllerManager>::SharedPtr client =
     create_client<controller_manager_msgs::srv::RegisterSubControllerManager>(
       "/register_sub_controller_manager");
@@ -510,29 +515,42 @@ void ControllerManager::register_sub_controller_manager()
 
   auto result = client->async_send_request(request);
   if (
-    (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
-     rclcpp::FutureReturnCode::SUCCESS) &&
-    result.get()->ok)
-  {
-    // TODO(Manuel) we should probably make the keys explicit (add key_generation function to handles)
-    // send keys with request
-    for (const auto & command_state_publisher : result.get()->command_state_publishers)
-    {
-      std::string key = command_state_publisher.name.prefix_name + "/" +
-                        command_state_publisher.name.interface_name;
-      auto [found, command_forwarder] = resource_manager_->find_command_forwarder(key);
-      if (found)
-      {
-        command_forwarder->subscribe_to_command_publisher(command_state_publisher.publisher_topic);
-      }
-      else
+    rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    // can call get only once
+    auto res = result.get();
+    if (res->ok)
+    {
+      auto command_state_publishers = res->command_state_publishers;
+      // TODO(Manuel) we should probably make the keys explicit (add key_generation function to handles)
+      // send keys with request
+      for (const auto & command_state_publisher : command_state_publishers)
       {
-        RCLCPP_WARN_STREAM(
-          get_logger(), "SubControllerManager: Could not find a CommandForwarder for key["
-                          << key << "]. No subscription to command state possible.");
+        std::string key = command_state_publisher.name.prefix_name + "/" +
+                          command_state_publisher.name.interface_name;
+        auto [found, command_forwarder] = resource_manager_->find_command_forwarder(key);
+        if (found)
+        {
+          command_forwarder->subscribe_to_command_publisher(
+            command_state_publisher.publisher_topic);
+        }
+        else
+        {
+          RCLCPP_WARN_STREAM(
+            get_logger(), "SubControllerManager: Could not find a CommandForwarder for key["
+                            << key << "]. No subscription to command state possible.");
+        }
       }
+      RCLCPP_INFO(get_logger(), "SubControllerManager: Successfully registered StatePublishers.");
+    }
+    else
+    {
+      RCLCPP_WARN(
+        get_logger(),
+        "SubControllerManager: Registration of StatePublishers failed. Central ControllerManager "
+        "returned error code.");
     }
-    RCLCPP_INFO(get_logger(), "SubControllerManager: Successfully registered StatePublishers.");
   }
   else
   {

hardware_interface/src/resource_manager.cpp

@@ -475,14 +475,14 @@ class ResourceStorage
 
   void add_command_interface(CommandInterface && command_interface)
   {
-    const auto [it, success] = command_interface_map_.emplace(std::make_pair(
-      command_interface.get_name(),
-      std::make_shared<CommandInterface>(std::move(command_interface))));
+    std::string key = command_interface.get_name();
+    const auto [it, success] = command_interface_map_.emplace(
+      std::make_pair(key, std::make_shared<CommandInterface>(std::move(command_interface))));
     if (!success)
     {
       std::string msg(
         "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" +
-        command_interface.get_name() + "]");
+        key + "]");
       throw std::runtime_error(msg);
     }
   }
@@ -890,9 +890,6 @@ ResourceManager::create_hardware_state_publishers(const std::string & ns)
 
   for (const auto & state_interface : available_state_interfaces())
   {
-    RCLCPP_INFO(
-      rclcpp::get_logger("ResourceManager"), "Creating StatePublisher for interface:<%s>.",
-      state_interface.c_str());
     auto state_publisher = std::make_shared<distributed_control::StatePublisher>(
       std::move(std::make_unique<hardware_interface::LoanedStateInterface>(
         claim_state_interface(state_interface))),
@@ -914,9 +911,6 @@ ResourceManager::create_hardware_command_forwarders(const std::string & ns)
 
   for (const auto & command_interface : available_command_interfaces())
   {
-    RCLCPP_INFO(
-      rclcpp::get_logger("ResourceManager"), "Creating CommandForwarder for interface:<%s>.",
-      command_interface.c_str());
     auto command_forwarder = std::make_shared<distributed_control::CommandForwarder>(
       std::move(std::make_unique<hardware_interface::LoanedCommandInterface>(
         claim_command_interface(command_interface))),