feat: add mutex to member variables of TF manager

awviz_common/include/awviz_common/transformation/transformation_manager.hpp

@@ -24,6 +24,7 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <memory>
+#include <mutex>
 #include <optional>
 #include <string>
 #include <unordered_map>
@@ -50,8 +51,9 @@ class TransformationManager
    *
    * @return Shared pointer of the entity paths map.
    */
-  const std::shared_ptr<std::unordered_map<std::string, std::string>> entities() const
+  const std::shared_ptr<std::unordered_map<std::string, std::string>> entities()
   {
+    std::lock_guard lock(entities_mtx_);
     return entities_;
   }
 
@@ -91,6 +93,9 @@ class TransformationManager
   std::shared_ptr<std::unordered_map<std::string, std::string>>
     entities_;  //!< Map stores a entity path of a corresponding frame ID.
   std::unordered_map<std::string, double> last_log_stamps_;  //!< Map stores last log timestamps.
+
+  std::mutex tf_tree_mtx_;   //!< Mutex of tf_tree_;
+  std::mutex entities_mtx_;  //!< Mutex of entities_;
 };
 }  // namespace awviz_common
 #endif  // AWVIZ_COMMON__TRANSFORMATION_MANAGER_HPP_

awviz_common/src/transformation/transformation_manager.cpp

@@ -18,7 +18,11 @@
 
 #include <yaml-cpp/yaml.h>
 
+#include <algorithm>
 #include <chrono>
+#include <cmath>
+#include <mutex>
+#include <string>
 
 namespace
 {
@@ -76,6 +80,8 @@ void TransformationManager::update_tree()
   try {
     YAML::Node frames = YAML::Load(yaml);
     for (YAML::const_iterator itr = frames.begin(); itr != frames.end(); ++itr) {
+      std::lock_guard lock(entities_mtx_);
+
       const auto id = itr->first.as<std::string>();
       const auto parent = itr->second["parent"].as<std::string>();
 
@@ -90,6 +96,7 @@ void TransformationManager::update_tree()
 
 void TransformationManager::update_entity(const TfFrame & frame)
 {
+  std::scoped_lock lock(tf_tree_mtx_, entities_mtx_);
   if (!tf_tree_->can_link_to(frame, TF_ROOT)) {
     return;
   }
@@ -99,6 +106,7 @@ void TransformationManager::update_entity(const TfFrame & frame)
 
 void TransformationManager::log_transform(const TfFrame & frame)
 {
+  std::lock_guard lock(entities_mtx_);
   // Root frame is skipped to log transformation
   if (frame.is_root() || entities_->count(frame.id()) == 0) {
     return;