From d11629bf677f93fbc72345dcc178516b3bd485dc Mon Sep 17 00:00:00 2001
From: sdcnlab <haridevanamaldev@gmail.com>
Date: Wed, 11 Sep 2024 18:56:51 -0400
Subject: [PATCH] Fixed GIL release issue with Python System and Python
 TestFixture. Signed-off-by: Amal Dev Haridevan haridevanamaldev@gmail.com

---
 python/src/gz/sim/TestFixture.cc              | 11 +++++
 python/test/gravity.sdf                       |  5 +++
 python/test/python_system_TEST.py             | 44 +++++++++++++++++++
 .../PythonSystemLoader.cc                     |  8 ++++
 4 files changed, 68 insertions(+)
 create mode 100644 python/test/python_system_TEST.py

diff --git a/python/src/gz/sim/TestFixture.cc b/python/src/gz/sim/TestFixture.cc
index 826558fbf4..978299f6f1 100644
--- a/python/src/gz/sim/TestFixture.cc
+++ b/python/src/gz/sim/TestFixture.cc
@@ -56,7 +56,14 @@ defineSimTestFixture(pybind11::object module)
       [](TestFixture* self, std::function<void(
           const UpdateInfo &, EntityComponentManager &)> _cb)
       {
+        // Add explicit scoped acquire and release of GIL, so that Python Systems
+        // can be executed
+        // This acquire and release is only required from the PythonSystem code
+        // However, adding this here may prevent undefined or unintended behaviors
+        // in future
+        pybind11::gil_scoped_acquire gil;
         self->OnPreUpdate(_cb);
+        pybind11::gil_scoped_release gilr;
       }
     ),
     pybind11::return_value_policy::reference,
@@ -67,7 +74,9 @@ defineSimTestFixture(pybind11::object module)
       [](TestFixture* self, std::function<void(
           const UpdateInfo &, EntityComponentManager &)> _cb)
       {
+        pybind11::gil_scoped_acquire gil;
         self->OnUpdate(_cb);
+        pybind11::gil_scoped_release gilr;
       }
     ),
     pybind11::return_value_policy::reference,
@@ -78,7 +87,9 @@ defineSimTestFixture(pybind11::object module)
       [](TestFixture* self, std::function<void(
           const UpdateInfo &, const EntityComponentManager &)> _cb)
       {
+        pybind11::gil_scoped_acquire gil;
         self->OnPostUpdate(_cb);
+        pybind11::gil_scoped_release gilr;
       }
     ),
     pybind11::return_value_policy::reference,
diff --git a/python/test/gravity.sdf b/python/test/gravity.sdf
index c2ae2a17cd..28c50e1c55 100644
--- a/python/test/gravity.sdf
+++ b/python/test/gravity.sdf
@@ -17,6 +17,11 @@
           <mass>1.0</mass>
         </inertial>
       </link>
+      <plugin filename="gz-sim-python-system-loader-system"
+        name="gz::sim::systems::PythonSystemLoader">
+        <module_name>python_system_TEST</module_name>
+      </plugin>
     </model>
+
   </world>
 </sdf>
diff --git a/python/test/python_system_TEST.py b/python/test/python_system_TEST.py
new file mode 100644
index 0000000000..557adc4f6b
--- /dev/null
+++ b/python/test/python_system_TEST.py
@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+# Copyright (C) 2021 Open Source Robotics Foundation
+
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+
+#       http://www.apache.org/licenses/LICENSE-2.0
+
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from gz_test_deps.math import Vector3d
+from gz_test_deps.sim import Model, Link
+import random
+ 
+ 
+class TestSystem(object):
+    def __init__(self):
+        self.id = random.randint(1, 100)
+ 
+    def configure(self, entity, sdf, ecm, event_mgr):
+        self.model = Model(entity)
+        self.link = Link(self.model.canonical_link(ecm))
+        print("Configured on", entity)
+        print("sdf name:", sdf.get_name())
+        self.force = sdf.get_double("force")
+        print(f"Applying {self.force} N on link {self.link.name(ecm)}")
+ 
+    def pre_update(self, info, ecm):
+        if info.paused:
+            return
+ 
+        if info.iterations % 3000 == 0:
+            self.link.add_world_force(
+                ecm, Vector3d(0, 0, self.force),
+                Vector3d(random.random(), random.random(), 0))
+ 
+ 
+def get_system():
+    return TestSystem()
\ No newline at end of file
diff --git a/src/systems/python_system_loader/PythonSystemLoader.cc b/src/systems/python_system_loader/PythonSystemLoader.cc
index d9dcdb0547..86e977e9f2 100644
--- a/src/systems/python_system_loader/PythonSystemLoader.cc
+++ b/src/systems/python_system_loader/PythonSystemLoader.cc
@@ -194,14 +194,21 @@ void PythonSystemLoader::CallPythonMethod(py::object _method, Args &&..._args)
 void PythonSystemLoader::PreUpdate(const UpdateInfo &_info,
                                    EntityComponentManager &_ecm)
 {
+  // Add explicit scoped acquire and release of GIL, so that Python
+  // Systems can be executed.This acquire and release is only required
+  // from the PythonSystem code
+  py::gil_scoped_acquire gil;
   CallPythonMethod(this->preUpdateMethod, _info, &_ecm);
+  py::gil_scoped_release gilr;
 }
 
 //////////////////////////////////////////////////
 void PythonSystemLoader::Update(const UpdateInfo &_info,
                                 EntityComponentManager &_ecm)
 {
+  py::gil_scoped_acquire gil;
   CallPythonMethod(this->updateMethod, _info, &_ecm);
+  py::gil_scoped_release gilr;
 }
 
 //////////////////////////////////////////////////
@@ -210,6 +217,7 @@ void PythonSystemLoader::PostUpdate(const UpdateInfo &_info,
 {
   py::gil_scoped_acquire gil;
   CallPythonMethod(this->postUpdateMethod, _info, &_ecm);
+  py::gil_scoped_release gilr;
 }
 //////////////////////////////////////////////////
 void PythonSystemLoader::Reset(const UpdateInfo &_info,