protect subscriber_statistics_collectors_ with a mutex (#1084)

* subscriber_statistics_collectors_ should be protected with mutex.

Co-Authored-By: William Woodall <william+github@osrfoundation.org>
Signed-off-by: Tomoya.Fujita <Tomoya.Fujita@sony.com>

rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp

@@ -94,6 +94,8 @@ public:
 
   /// Handle a message received by the subscription to collect statistics.
   /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   *
    * \param received_message the message received by the subscription
    * \param now_nanoseconds current time in nanoseconds
    */
@@ -101,6 +103,7 @@ public:
     const CallbackMessageT & received_message,
     const rclcpp::Time now_nanoseconds) const
   {
+    std::lock_guard<std::mutex> lock(mutex_);
     for (const auto & collector : subscriber_statistics_collectors_) {
       collector->OnMessageReceived(received_message, now_nanoseconds.nanoseconds());
     }
@@ -116,10 +119,16 @@ public:
   }
 
   /// Publish a populated MetricsStatisticsMessage.
+  /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   */
   virtual void publish_message()
   {
+    std::vector<MetricsMessage> msgs;
     rclcpp::Time window_end{get_current_nanoseconds_since_epoch()};
 
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
       for (auto & collector : subscriber_statistics_collectors_) {
         const auto collected_stats = collector->GetStatisticsResults();
 
@@ -130,7 +139,12 @@ public:
           window_start_,
           window_end,
           collected_stats);
-      publisher_->publish(message);
+        msgs.push_back(message);
+      }
+    }
+
+    for (auto & msg : msgs) {
+      publisher_->publish(msg);
     }
     window_start_ = window_end;
   }
@@ -138,11 +152,14 @@ public:
 protected:
   /// Return a vector of all the currently collected data.
   /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   *
    * \return a vector of all the collected data
    */
   std::vector<StatisticData> get_current_collector_data() const
   {
     std::vector<StatisticData> data;
+    std::lock_guard<std::mutex> lock(mutex_);
     for (const auto & collector : subscriber_statistics_collectors_) {
       data.push_back(collector->GetStatisticsResults());
     }
@@ -151,23 +168,35 @@ protected:
 
 private:
   /// Construct and start all collectors and set window_start_.
+  /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   */
   void bring_up()
   {
     auto received_message_period = std::make_unique<ReceivedMessagePeriod>();
     received_message_period->Start();
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
       subscriber_statistics_collectors_.emplace_back(std::move(received_message_period));
+    }
 
     window_start_ = rclcpp::Time(get_current_nanoseconds_since_epoch());
   }
 
   /// Stop all collectors, clear measurements, stop publishing timer, and reset publisher.
+  /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   */
   void tear_down()
   {
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
       for (auto & collector : subscriber_statistics_collectors_) {
         collector->Stop();
       }
 
       subscriber_statistics_collectors_.clear();
+    }
 
     if (publisher_timer_) {
       publisher_timer_->cancel();
@@ -187,6 +216,8 @@ private:
     return std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count();
   }
 
+  /// Mutex to protect the subsequence vectors
+  mutable std::mutex mutex_;
   /// Collection of statistics collectors
   std::vector<std::unique_ptr<TopicStatsCollector>> subscriber_statistics_collectors_{};
   /// Node name used to generate topic statistics messages to be published