From e95ae88a0949371a7ad17be081bde12f3971fe2a Mon Sep 17 00:00:00 2001 From: illyakaynov-vbti <163301017+illyakaynov-vbti@users.noreply.github.com> Date: Tue, 17 Nov 2026 21:27:10 +0100 Subject: [PATCH] moveit_py: release the GIL in plan(), the collision checks and set_from_ik (#3935) * moveit_py: PlanningComponent.plan() releases the GIL * moveit_py: collision checks and set_from_ik release the GIL * moveit_py: check_self_collision releases the GIL The other collision queries of PlanningScene already do; the three check_self_collision overloads were missed. * moveit_py: note in the docstrings which GIL-releasing calls write state Apply the review suggestions: the calls that write to the object or to the passed state while the GIL is released now say so, and that the object must not be used from another Python thread meanwhile. Co-authored-by: Nathan Brooks Co-Authored-By: Claude Opus 5.5 * moveit_py: plan() note as a Note section, warn that read_only() is a shared lock Co-Authored-By: Claude Opus 5.5 * moveit_py: test that the collision checks release the GIL A second Python thread counts while the main thread runs a collision check in a loop. The switch interval is raised to 30 s, so the counter only advances while the binding releases the GIL: about 3000 ticks in 0.5 s with the guard, 1 without it. Co-Authored-By: Claude Opus 5.5 --------- Co-authored-by: Matthijs van der Burgh Co-authored-by: Nathan Brooks Co-authored-by: Claude Opus 5.5 (cherry picked from commit b9bf61ea752156fb3c137456e9bb415a94a66742) --- moveit_py/CMakeLists.txt | 3 +- .../planning_scene/planning_scene.cpp | 64 +++++++++-- .../moveit_core/robot_state/robot_state.cpp | 7 +- .../moveit_cpp/planning_component.cpp | 8 ++ moveit_py/test/unit/test_planning_scene.py | 107 ++++++++++++++++++ 5 files changed, 177 insertions(+), 12 deletions(-) create mode 100644 moveit_py/test/unit/test_planning_scene.py diff --git a/moveit_py/CMakeLists.txt b/moveit_py/CMakeLists.txt index 99aab735d7..c750fbf247 100644 --- a/moveit_py/CMakeLists.txt +++ b/moveit_py/CMakeLists.txt @@ -84,7 +84,8 @@ configure_build_install_location(planning) if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) - set(_pytest_tests test/unit/test_robot_model.py test/unit/test_robot_state.py) + set(_pytest_tests test/unit/test_planning_scene.py + test/unit/test_robot_model.py test/unit/test_robot_state.py) foreach(test_path ${_pytest_tests}) get_filename_component(_test_name ${test_path} NAME_WE) ament_add_pytest_test( diff --git a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp index 0331f3ba92..6d23a90b6c 100644 --- a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp @@ -271,7 +271,8 @@ void initPlanningScene(py::module& m) .def("is_state_valid", py::overload_cast( &planning_scene::PlanningScene::isStateValid, py::const_), - py::arg("robot_state"), py::arg("joint_model_group_name"), py::arg("verbose") = false) + py::arg("robot_state"), py::arg("joint_model_group_name"), py::arg("verbose") = false, + py::call_guard()) .def("is_state_colliding", py::overload_cast(&planning_scene::PlanningScene::isStateColliding), py::arg("joint_model_group_name"), py::arg("verbose") = false, @@ -283,7 +284,14 @@ void initPlanningScene(py::module& m) verbose (bool): If true, print the link names of the links in collision. Returns: bool: True if the robot state is in collision, false otherwise. - )") + + Note: + Releases the GIL. Do not use this planning scene from another Python thread during the call: + it may update the collision transforms of the scene's current state. + Holding planning_scene_monitor.read_only() is not enough: that lock is shared, so two threads + inside it can still run this call on the same scene at once. + )", + py::call_guard()) .def("is_state_colliding", py::overload_cast( @@ -298,7 +306,8 @@ void initPlanningScene(py::module& m) verbose (bool): If true, print the link names of the links in collision. Returns: bool: True if the robot state is in collision, false otherwise. - )") + )", + py::call_guard()) .def("is_state_constrained", py::overload_cast( @@ -331,7 +340,8 @@ void initPlanningScene(py::module& m) Returns: bool: true if the path is valid otherwise false. - )") + )", + py::call_guard()) // TODO (peterdavidfagan): remove collision result from input parameters and write separate binding code. // TODO (peterdavidfagan): consider merging check_collision and check_collision_unpadded into one function with unpadded_param @@ -348,7 +358,14 @@ void initPlanningScene(py::module& m) Returns: bool: true if state is in collision otherwise false. - )") + + Note: + Releases the GIL. Do not use this planning scene from another Python thread during the call: + it may update the collision transforms of the scene's current state. + Holding planning_scene_monitor.read_only() is not enough: that lock is shared, so two threads + inside it can still run this call on the same scene at once. + )", + py::call_guard()) .def("check_collision", py::overload_cast()) .def("check_collision", py::overload_cast()) // DEPRECATED! Use check_collision instead .def("check_collision_unpadded", py::overload_cast( @@ -446,7 +473,14 @@ void initPlanningScene(py::module& m) Returns: bool: true if state is in collision otherwise false. - )") + + Note: + Releases the GIL. Do not use this planning scene from another Python thread during the call: + it may update the collision transforms of the scene's current state. + Holding planning_scene_monitor.read_only() is not enough: that lock is shared, so two threads + inside it can still run this call on the same scene at once. + )", + py::call_guard()) .def("check_self_collision", py::overload_cast()) .def("check_self_collision", py::overload_cast()) .def("save_geometry_to_file", &saveGeometryToFile, py::arg("file_path_and_name"), R"( diff --git a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp index 625ec2eeae..673a086ddc 100644 --- a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp @@ -450,7 +450,12 @@ void initRobotState(py::module& m) geometry_pose (geometry_msgs.msg.Pose): The pose of the end-effector to solve the inverse kinematics for. tip_name (str): The name of the link that is the tip of the end-effector. timeout (float): The amount of time to wait for the IK solution to be found. - )") + + Note: + Releases the GIL. Do not use this robot state from another Python thread during the call: + it writes the joint values. + )", + py::call_guard()) // Setting entire state values .def("set_to_default_values", py::overload_cast<>(&moveit::core::RobotState::setToDefaultValues), diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp index 0053e1129d..403b9c4921 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -333,11 +333,19 @@ void initPlanningComponent(py::module& m) py::arg("multi_plan_parameters") = nullptr, py::arg("planning_scene") = nullptr, py::arg("solution_selection_function") = nullptr, py::arg("stopping_criterion_callback") = nullptr, py::return_value_policy::move, + // Planning takes seconds and needs no Python: let other Python threads run meanwhile, as + // MoveItPy.execute does. A Python callback passed in takes the GIL back itself (pybind11's + // std::function caster acquires it around the call). + py::call_guard(), R"( Plan a motion plan using the current start and goal states. Args: plan_parameters (moveit_py.core.PlanParameters): The parameters to use for planning. + + Note: + Releases the GIL. Do not use this planning component, its start state, or the passed planning + scene from another Python thread during the call. )") .def("set_path_constraints", &moveit_cpp::PlanningComponent::setPathConstraints, py::arg("path_constraints"), diff --git a/moveit_py/test/unit/test_planning_scene.py b/moveit_py/test/unit/test_planning_scene.py new file mode 100644 index 0000000000..9c05fde2da --- /dev/null +++ b/moveit_py/test/unit/test_planning_scene.py @@ -0,0 +1,107 @@ +import os +import sys +import threading +import time +import unittest + +from test_moveit.core.collision_detection import CollisionRequest, CollisionResult +from test_moveit.core.planning_scene import PlanningScene +from test_moveit.core.robot_model import RobotModel +from test_moveit.core.robot_state import RobotState + +dir_path = os.path.dirname(os.path.realpath(__file__)) +URDF_FILE = "{}/fixtures/panda.urdf".format(dir_path) +SRDF_FILE = "{}/fixtures/panda.srdf".format(dir_path) + +GROUP = "panda_arm" + + +def get_robot_model(): + """Helper function that returns a RobotModel instance.""" + return RobotModel(urdf_xml_path=URDF_FILE, srdf_xml_path=SRDF_FILE) + + +def count_ticks_while(call, duration=0.5): + """ + Run call() in a loop for duration seconds and count how often another Python thread gets to run meanwhile. + + The switch interval is raised far above duration, so the interpreter does not hand the GIL to the other + thread on its own: the other thread only runs while call() releases the GIL. + """ + stop = threading.Event() + ticks = 0 + + def tick(): + nonlocal ticks + while not stop.is_set(): + ticks += 1 + time.sleep(0.0001) # releases the GIL, so call() can take it back + + old_interval = sys.getswitchinterval() + sys.setswitchinterval(30.0) + ticker = threading.Thread(target=tick, daemon=True) + try: + ticker.start() + deadline = time.monotonic() + duration + while time.monotonic() < deadline: + call() + result = ticks + finally: + stop.set() + sys.setswitchinterval(old_interval) + ticker.join() + return result + + +class TestPlanningSceneGilRelease(unittest.TestCase): + """The collision checks release the GIL, so other Python threads keep running during bulk checks.""" + + @classmethod + def setUpClass(cls): + cls.robot_model = get_robot_model() + + def setUp(self): + self.scene = PlanningScene(self.robot_model) + self.state = RobotState(self.robot_model) + self.state.set_to_default_values() + self.state.update() + + def assert_releases_gil(self, call): + call() # warm up, so the first call's setup cost is not measured + ticks = count_ticks_while(call) + # With the GIL held for the whole loop the other thread gets at most a tick or two. + self.assertGreater(ticks, 10) + + def test_is_state_valid(self): + self.assert_releases_gil(lambda: self.scene.is_state_valid(self.state, GROUP)) + + def test_is_state_colliding_current_state(self): + self.assert_releases_gil(lambda: self.scene.is_state_colliding(GROUP)) + + def test_is_state_colliding(self): + self.assert_releases_gil( + lambda: self.scene.is_state_colliding(self.state, GROUP) + ) + + def test_check_collision(self): + self.assert_releases_gil( + lambda: self.scene.check_collision(CollisionRequest(), CollisionResult()) + ) + + def test_check_collision_state(self): + self.assert_releases_gil( + lambda: self.scene.check_collision( + CollisionRequest(), CollisionResult(), self.state + ) + ) + + def test_check_self_collision_state(self): + self.assert_releases_gil( + lambda: self.scene.check_self_collision( + CollisionRequest(), CollisionResult(), self.state + ) + ) + + +if __name__ == "__main__": + unittest.main()