diff --git a/rclcpp/executors/multithreaded_executor/multithreaded_executor.cpp b/rclcpp/executors/multithreaded_executor/multithreaded_executor.cpp index 2414dc32..f14de567 100644 --- a/rclcpp/executors/multithreaded_executor/multithreaded_executor.cpp +++ b/rclcpp/executors/multithreaded_executor/multithreaded_executor.cpp @@ -11,11 +11,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" @@ -27,8 +29,14 @@ using namespace std::chrono_literals; **/ std::string string_thread_id() { - auto hashed = std::hash()(std::this_thread::get_id()); - return std::to_string(hashed); + static std::vector known_thread_ids {}; + const auto thread_it = std::find(known_thread_ids.cbegin(), known_thread_ids.cend(), + std::this_thread::get_id()); + if (thread_it != known_thread_ids.cend()) { + return std::to_string(std::distance(known_thread_ids.cbegin(), thread_it)); + } + known_thread_ids.push_back(std::this_thread::get_id()); + return std::to_string(known_thread_ids.size() - 1); } /* For this example, we will be creating a publishing node like the one in minimal_publisher.