Sys status: add mutex to M_VehicleInfo vehicles unordered map

Author: jonas

mavros/src/plugins/sys_status.cpp

@@ -20,6 +20,8 @@
 #include <vector>
 #include <unordered_map>
 #include <memory>
+#include <mutex>
+#include <shared_mutex>
 #include <utility>
 
 #include "rcpputils/asserts.hpp"
@@ -678,6 +680,7 @@ class SystemStatusPlugin : public plugin::Plugin
 
   using M_VehicleInfo = std::unordered_map<uint16_t, mavros_msgs::msg::VehicleInfo>;
   M_VehicleInfo vehicles;
+  mutable std::shared_mutex vehicles_mutex;
 
   /* -*- mid-level helpers -*- */
 
@@ -687,8 +690,8 @@ class SystemStatusPlugin : public plugin::Plugin
     return sysid << 8 | compid;
   }
 
-  // Find or create vehicle info
-  inline M_VehicleInfo::iterator find_or_create_vehicle_info(uint8_t sysid, uint8_t compid)
+  // Caller must hold vehicles_mutex exclusively.
+  inline M_VehicleInfo::iterator find_or_create_vehicle_info_unlocked(uint8_t sysid, uint8_t compid)
   {
     auto key = get_vehicle_key(sysid, compid);
     M_VehicleInfo::iterator ret = vehicles.find(key);
@@ -899,26 +902,28 @@ class SystemStatusPlugin : public plugin::Plugin
 
     // XXX(vooon): i assume that UAS not interested in HBs from non-target system.
 
-    // Store generic info of all heartbeats seen
-    auto it = find_or_create_vehicle_info(msg->sysid, msg->compid);
-
     auto vehicle_mode = uas->str_mode_v10(hb.base_mode, hb.custom_mode);
     auto stamp = node->now();
 
-    // Update vehicle data
-    it->second.header.stamp = stamp;
-    it->second.available_info |= mavros_msgs::msg::VehicleInfo::HAVE_INFO_HEARTBEAT;
-    it->second.autopilot = hb.autopilot;
-    it->second.type = hb.type;
-    it->second.system_status = hb.system_status;
-    it->second.base_mode = hb.base_mode;
-    it->second.custom_mode = hb.custom_mode;
-    it->second.mode = vehicle_mode;
-
-    if (!(hb.base_mode & enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED))) {
-      it->second.mode_id = hb.base_mode;
-    } else {
-      it->second.mode_id = hb.custom_mode;
+    {
+      std::unique_lock<std::shared_mutex> lock(vehicles_mutex);
+      auto it = find_or_create_vehicle_info_unlocked(msg->sysid, msg->compid);
+
+      // Update vehicle data
+      it->second.header.stamp = stamp;
+      it->second.available_info |= mavros_msgs::msg::VehicleInfo::HAVE_INFO_HEARTBEAT;
+      it->second.autopilot = hb.autopilot;
+      it->second.type = hb.type;
+      it->second.system_status = hb.system_status;
+      it->second.base_mode = hb.base_mode;
+      it->second.custom_mode = hb.custom_mode;
+      it->second.mode = vehicle_mode;
+
+      if (!(hb.base_mode & enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED))) {
+        it->second.mode_id = hb.base_mode;
+      } else {
+        it->second.mode_id = hb.custom_mode;
+      }
     }
 
     // Continue from here only if vehicle is my target
@@ -1094,21 +1099,26 @@ class SystemStatusPlugin : public plugin::Plugin
       process_autopilot_version_normal(apv, msg->sysid, msg->compid);
     }
 
-    // Store generic info of all autopilot seen
-    auto it = find_or_create_vehicle_info(msg->sysid, msg->compid);
-
-    // Update vehicle data
-    it->second.header.stamp = node->now();
-    it->second.available_info |= mavros_msgs::msg::VehicleInfo::HAVE_INFO_AUTOPILOT_VERSION;
-    it->second.capabilities = apv.capabilities;
-    it->second.flight_sw_version = apv.flight_sw_version;
-    it->second.middleware_sw_version = apv.middleware_sw_version;
-    it->second.os_sw_version = apv.os_sw_version;
-    it->second.board_version = apv.board_version;
-    it->second.flight_custom_version = custom_version_to_hex_string(apv.flight_custom_version);
-    it->second.vendor_id = apv.vendor_id;
-    it->second.product_id = apv.product_id;
-    it->second.uid = apv.uid;
+    auto stamp = node->now();
+    auto flight_custom_version = custom_version_to_hex_string(apv.flight_custom_version);
+
+    {
+      std::unique_lock<std::shared_mutex> lock(vehicles_mutex);
+      auto it = find_or_create_vehicle_info_unlocked(msg->sysid, msg->compid);
+
+      // Update vehicle data
+      it->second.header.stamp = stamp;
+      it->second.available_info |= mavros_msgs::msg::VehicleInfo::HAVE_INFO_AUTOPILOT_VERSION;
+      it->second.capabilities = apv.capabilities;
+      it->second.flight_sw_version = apv.flight_sw_version;
+      it->second.middleware_sw_version = apv.middleware_sw_version;
+      it->second.os_sw_version = apv.os_sw_version;
+      it->second.board_version = apv.board_version;
+      it->second.flight_custom_version = flight_custom_version;
+      it->second.vendor_id = apv.vendor_id;
+      it->second.product_id = apv.product_id;
+      it->second.uid = apv.uid;
+    }
   }
 
   void handle_battery_status(
@@ -1378,6 +1388,7 @@ class SystemStatusPlugin : public plugin::Plugin
       publish_disconnection();
 
       // Clear known vehicles
+      std::unique_lock<std::shared_mutex> lock(vehicles_mutex);
       vehicles.clear();
     }
   }
@@ -1506,7 +1517,10 @@ class SystemStatusPlugin : public plugin::Plugin
     mavros_msgs::srv::VehicleInfoGet::Response::SharedPtr res)
   {
     if (req->get_all) {
+      std::shared_lock<std::shared_mutex> lock(vehicles_mutex);
+
       // Send all vehicles
+      res->vehicles.reserve(vehicles.size());
       for (const auto & got : vehicles) {
         res->vehicles.emplace_back(got.second);
       }
@@ -1525,6 +1539,7 @@ class SystemStatusPlugin : public plugin::Plugin
     }
 
     uint16_t key = get_vehicle_key(req_sysid, req_compid);
+    std::shared_lock<std::shared_mutex> lock(vehicles_mutex);
     auto it = vehicles.find(key);
     if (it == vehicles.end()) {
       // Vehicle not found