Avoid using invalid std::list iterators (ros#293) + Fix accessing fre…

…e'd resources ros#386 (ros#419)

* fix accessing freed resources (ros#386)

* Avoid using invalid std::list iterators (ros#293)

Signed-off-by: Michael Carroll <michael@openrobotics.org>

Co-authored-by: Kazunari Tanaka <tzskp1@gmail.com>
Co-authored-by: Michael Carroll <michael@openrobotics.org>

tf2_ros/include/tf2_ros/message_filter.h

@@ -459,16 +459,17 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
   {
     namespace mt = message_filters::message_traits;
 
-    // find the message this request is associated with
-    typename L_MessageInfo::iterator msg_it = messages_.begin();
-    typename L_MessageInfo::iterator msg_end = messages_.end();
-
     MEvent saved_event;
+    bool event_found = false;
 
     {
       // We will be accessing and mutating messages now, require unique lock
       std::unique_lock<std::mutex> lock(messages_mutex_);
 
+      // find the message this request is associated with
+      typename L_MessageInfo::iterator msg_it = messages_.begin();
+      typename L_MessageInfo::iterator msg_end = messages_.end();
+
       for (; msg_it != msg_end; ++msg_it) {
         MessageInfo & info = *msg_it;
         auto handle_it = std::find(info.handles.begin(), info.handles.end(), handle);
@@ -479,15 +480,14 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
             saved_event = msg_it->event;
             messages_.erase(msg_it);
             --message_count_;
-          } else {
-            msg_it = msg_end;
-          }
+            event_found = true;
+          } 
           break;
         }
       }
     }
 
-    if (msg_it == msg_end) {
+    if (!event_found) {
       return;
     }