From 33cb2e6fc181f227577051b52e3596e7c88a356b Mon Sep 17 00:00:00 2001 From: fengjingchao-cn Date: Thu, 21 Dec 2017 14:31:24 -0800 Subject: [PATCH] fix publishing latched message many times leads to crash bug --- .../roscpp/src/libros/subscription.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/ros/ros_comm/roscpp/src/libros/subscription.cpp b/ros/ros_comm/roscpp/src/libros/subscription.cpp index 0fff1ee..169c8b4 100644 --- a/ros/ros_comm/roscpp/src/libros/subscription.cpp +++ b/ros/ros_comm/roscpp/src/libros/subscription.cpp @@ -680,16 +680,19 @@ uint32_t Subscription::handleMessage(const SerializedMessage& m, bool ser, bool } } - for (V_PublisherLink::iterator it = publisher_links_.begin(); it != publisher_links_.end(); ++it) { - if ((*it)->isLatched()) + boost::mutex::scoped_lock lock(publisher_links_mutex_); + for (V_PublisherLink::iterator it = publisher_links_.begin(); it != publisher_links_.end(); ++it) { - LatchInfo li; - li.connection_header = connection_header; - li.link = (*it); - li.message = m; - li.receipt_time = receipt_time; - latched_messages_[(*it)] = li; + if ((*it)->isLatched()) + { + LatchInfo li; + li.connection_header = connection_header; + li.link = (*it); + li.message = m; + li.receipt_time = receipt_time; + latched_messages_[(*it)] = li; + } } }