From fed0d09f01173a57c4b55f5f663db1656288ae47 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 21 Aug 2024 13:38:23 +0200
Subject: [PATCH] GzOdeCollisionDetector: Use static mutex in create

This is done to avoid that non-thread-safe ODE functions are called at the same time, causing errors such as
> ODE INTERNAL ERROR 1: assertion "!colliders_initialized" failed in dInitColliders() [collision_kernel.cpp:168]

Signed-off-by: Silvio Traversaro <silvio.traversaro@iit.it>
---
 dartsim/src/GzOdeCollisionDetector.cc | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc
index 6a81b4e06..13e96e980 100644
--- a/dartsim/src/GzOdeCollisionDetector.cc
+++ b/dartsim/src/GzOdeCollisionDetector.cc
@@ -16,6 +16,7 @@
 */
 
 #include <memory>
+#include <mutex>
 #include <unordered_map>
 #include <utility>
 
@@ -43,6 +44,13 @@ GzOdeCollisionDetector::Registrar<GzOdeCollisionDetector>
 /////////////////////////////////////////////////
 std::shared_ptr<GzOdeCollisionDetector> GzOdeCollisionDetector::create()
 {
+  // GzOdeCollisionDetector constructor calls the OdeCollisionDetector
+  // constructor, that calls the non-thread safe dInitODE2(0).
+  // To mitigate this problem, we use a static mutex to ensure that
+  // each GzOdeCollisionDetector constructor is called not at the same time.
+  // See https://github.com/gazebosim/gz-sim/issues/18 for more info.
+  static std::mutex odeInitMutex;
+  std::unique_lock<std::mutex> lock(odeInitMutex);
   return std::shared_ptr<GzOdeCollisionDetector>(new GzOdeCollisionDetector());
 }