feat(autoware_control_center): separate filter_deregister_services f…

…unction Signed-off-by: Alexey Panferov <[email protected]>
autowarefoundation · Mar 25, 2024 · 7895ade · 7895ade
1 parent 529c29e
commit 7895ade
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 9 deletions.
diff --git a/common/autoware_control_center/include/autoware_control_center/autoware_control_center.hpp b/common/autoware_control_center/include/autoware_control_center/autoware_control_center.hpp
@@ -90,6 +90,7 @@ class AutowareControlCenter : public rclcpp_lifecycle::LifecycleNode
   void liveliness_callback(rclcpp::QOSLivelinessChangedInfo & event, const std::string & node_name);
   void heartbeat_callback(const typename autoware_control_center_msgs::msg::Heartbeat::SharedPtr msg,
                                                     const std::string & node_name);
+  void filter_deregister_services(std::map<std::string, std::vector<std::string>>& srv_list);
 };
 
 }  // namespace autoware_control_center

diff --git a/common/autoware_control_center/src/autoware_control_center.cpp b/common/autoware_control_center/src/autoware_control_center.cpp
@@ -129,15 +129,16 @@ void AutowareControlCenter::startup_callback()
     this->startup_timer_->cancel();
     RCLCPP_INFO(get_logger(), "Startup timer stop.");
     std::map<std::string, std::vector<std::string>> srv_list = this->get_service_names_and_types();
-    auto it = srv_list.begin();
-    // filter out srv with type autoware_control_center_msgs/srv/AutowareControlCenterDeregister
-    while (it != srv_list.end()) {
-      if (it->second[0] != "autoware_control_center_msgs/srv/AutowareControlCenterDeregister") {
-        srv_list.erase(it++);
-      } else {
-        ++it;
-      }
-    }
+    filter_deregister_services(srv_list);
+    // auto it = srv_list.begin();
+    // // filter out srv with type autoware_control_center_msgs/srv/AutowareControlCenterDeregister
+    // while (it != srv_list.end()) {
+    //   if (it->second[0] != "autoware_control_center_msgs/srv/AutowareControlCenterDeregister") {
+    //     srv_list.erase(it++);
+    //   } else {
+    //     ++it;
+    //   }
+    // }
     RCLCPP_INFO(get_logger(), "Filtered service list");
     for (auto const & pair : srv_list) {
       RCLCPP_INFO(get_logger(), "Service Name: %s", pair.first.c_str());
@@ -266,4 +267,17 @@ void AutowareControlCenter::heartbeat_callback(const typename autoware_control_c
       node_status_map_[node_name].last_heartbeat = msg->stamp;
     }
 
+void AutowareControlCenter::filter_deregister_services(std::map<std::string, std::vector<std::string>>& srv_list)
+{
+  auto it = srv_list.begin();
+  // filter out srv with type autoware_control_center_msgs/srv/AutowareControlCenterDeregister
+  while (it != srv_list.end()) {
+    if (it->second[0] != "autoware_control_center_msgs/srv/AutowareControlCenterDeregister") {
+      srv_list.erase(it++);
+    } else {
+      ++it;
+    }
+  }
+}
+
 }  // namespace autoware_control_center