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 index 0582d7ea5a..7413176c25 100644 --- 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>& 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 index dc0789acd5..2957e97965 100644 --- 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> 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>& 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