GzOdeCollisionDetector: Use static mutex in create (#675)

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 <[email protected]>
Co-authored-by: Ian Chen <[email protected]>

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());
 }