diff --git a/cie_thread_configurator/src/thread_configurator_node.cpp b/cie_thread_configurator/src/thread_configurator_node.cpp index cd1dd22..28b0f92 100644 --- a/cie_thread_configurator/src/thread_configurator_node.cpp +++ b/cie_thread_configurator/src/thread_configurator_node.cpp @@ -21,11 +21,28 @@ ThreadConfiguratorNode::ThreadConfiguratorNode(const YAML::Node &yaml) unapplied_num_ = callback_groups.size(); callback_group_configs_.resize(callback_groups.size()); + // For backward compatibility: remove trailing "Waitable@"s + auto remove_trailing_waitable = [](std::string s) { + static constexpr std::string_view suffix = "@Waitable"; + const std::size_t suffix_size = suffix.size(); + std::size_t s_size = s.size(); + + while (s_size >= suffix_size && + std::char_traits::compare(s.data() + (s_size - suffix_size), + suffix.data(), suffix_size) == 0) { + s_size -= suffix_size; + } + s.resize(s_size); + + return s; + }; + for (size_t i = 0; i < callback_groups.size(); i++) { const auto &callback_group = callback_groups[i]; auto &config = callback_group_configs_[i]; - config.callback_group_id = callback_group["id"].as(); + config.callback_group_id = + remove_trailing_waitable(callback_group["id"].as()); for (auto &cpu : callback_group["affinity"]) config.affinity.push_back(cpu.as()); config.policy = callback_group["policy"].as(); diff --git a/cie_thread_configurator/src/util.cpp b/cie_thread_configurator/src/util.cpp index fbe2320..646424e 100644 --- a/cie_thread_configurator/src/util.cpp +++ b/cie_thread_configurator/src/util.cpp @@ -43,11 +43,7 @@ std::string create_callback_group_id( ss << "Timer(" << period << ")@"; }; - auto waitable_func = [&ss](const rclcpp::Waitable::SharedPtr &waitable) { - (void)waitable; - ss << "Waitable" - << "@"; - }; + auto waitable_func = [](auto &&) {}; group->collect_all_ptrs(sub_func, service_func, client_func, timer_func, waitable_func);