Fix thread safety in environment (#964)

* Fix thread safety in environment.cpp * Clang format
tesseract-robotics · Nov 16, 2023 · aa3ac23 · aa3ac23
1 parent cb053ec
commit aa3ac23
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 12 deletions.
diff --git 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
@@ -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
@@ -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,