From aa3ac23736df4a99fa59708ec8d9d581d55e4817 Mon Sep 17 00:00:00 2001
From: Levi Armstrong <levi.armstrong@gmail.com>
Date: Thu, 16 Nov 2023 10:20:25 -0600
Subject: [PATCH] Fix thread safety in environment (#964)

* Fix thread safety in environment.cpp

* Clang format
---
 .../allowed_collision_matrix.h                |  5 +----
 tesseract_environment/src/environment.cpp     | 20 +++++++++++++++----
 tesseract_scene_graph/src/graph.cpp           |  5 +----
 3 files changed, 18 insertions(+), 12 deletions(-)

diff --git a/tesseract_common/include/tesseract_common/allowed_collision_matrix.h b/tesseract_common/include/tesseract_common/allowed_collision_matrix.h
index 7e5ca4a6547..50f307022fa 100644
--- a/tesseract_common/include/tesseract_common/allowed_collision_matrix.h
+++ b/tesseract_common/include/tesseract_common/allowed_collision_matrix.h
@@ -119,10 +119,7 @@ class AllowedCollisionMatrix
    * @brief Reserve space for the internal data storage
    * @param size The size to reserve
    */
-  void reserveAllowedCollisionMatrix(std::size_t size)
-  {
-    lookup_table_.reserve(size);
-  }
+  void reserveAllowedCollisionMatrix(std::size_t size) { lookup_table_.reserve(size); }
 
   friend std::ostream& operator<<(std::ostream& os, const AllowedCollisionMatrix& acm)
   {
diff --git a/tesseract_environment/src/environment.cpp b/tesseract_environment/src/environment.cpp
index 135d4d5331d..714c017fea4 100644
--- a/tesseract_environment/src/environment.cpp
+++ b/tesseract_environment/src/environment.cpp
@@ -100,7 +100,10 @@ bool Environment::init(const tesseract_scene_graph::SceneGraph& scene_graph,
 
 bool Environment::init(const std::string& urdf_string, const tesseract_common::ResourceLocator::ConstPtr& locator)
 {
-  resource_locator_ = locator;
+  {
+    std::unique_lock<std::shared_mutex> lock(mutex_);
+    resource_locator_ = locator;
+  }
 
   // Parse urdf string into Scene Graph
   tesseract_scene_graph::SceneGraph::Ptr scene_graph;
@@ -123,7 +126,10 @@ bool Environment::init(const std::string& urdf_string,
                        const std::string& srdf_string,
                        const tesseract_common::ResourceLocator::ConstPtr& locator)
 {
-  resource_locator_ = locator;
+  {
+    std::unique_lock<std::shared_mutex> lock(mutex_);
+    resource_locator_ = locator;
+  }
 
   // Parse urdf string into Scene Graph
   tesseract_scene_graph::SceneGraph::Ptr scene_graph;
@@ -158,7 +164,10 @@ bool Environment::init(const std::string& urdf_string,
 bool Environment::init(const tesseract_common::fs::path& urdf_path,
                        const tesseract_common::ResourceLocator::ConstPtr& locator)
 {
-  resource_locator_ = locator;
+  {
+    std::unique_lock<std::shared_mutex> lock(mutex_);
+    resource_locator_ = locator;
+  }
 
   // Parse urdf file into Scene Graph
   tesseract_scene_graph::SceneGraph::Ptr scene_graph;
@@ -181,7 +190,10 @@ bool Environment::init(const tesseract_common::fs::path& urdf_path,
                        const tesseract_common::fs::path& srdf_path,
                        const tesseract_common::ResourceLocator::ConstPtr& locator)
 {
-  resource_locator_ = locator;
+  {
+    std::unique_lock<std::shared_mutex> lock(mutex_);
+    resource_locator_ = locator;
+  }
 
   // Parse urdf file into Scene Graph
   tesseract_scene_graph::SceneGraph::Ptr scene_graph;
diff --git a/tesseract_scene_graph/src/graph.cpp b/tesseract_scene_graph/src/graph.cpp
index 53fe8fd3b91..dc31f0dd072 100644
--- a/tesseract_scene_graph/src/graph.cpp
+++ b/tesseract_scene_graph/src/graph.cpp
@@ -624,10 +624,7 @@ JointLimits::ConstPtr SceneGraph::getJointLimits(const std::string& name)
   return found->second.first->limits;
 }
 
-void SceneGraph::setAllowedCollisionMatrix(tesseract_common::AllowedCollisionMatrix::Ptr acm)
-{
-  acm_ = std::move(acm);
-}
+void SceneGraph::setAllowedCollisionMatrix(tesseract_common::AllowedCollisionMatrix::Ptr acm) { acm_ = std::move(acm); }
 
 void SceneGraph::addAllowedCollision(const std::string& link_name1,
                                      const std::string& link_name2,