Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion moveit_py/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
64 changes: 54 additions & 10 deletions moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -271,7 +271,8 @@ void initPlanningScene(py::module& m)
.def("is_state_valid",
py::overload_cast<const moveit::core::RobotState&, const std::string&, bool>(
&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<py::gil_scoped_release>())
.def("is_state_colliding",
py::overload_cast<const std::string&, bool>(&planning_scene::PlanningScene::isStateColliding),
py::arg("joint_model_group_name"), py::arg("verbose") = false,
Expand All @@ -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<py::gil_scoped_release>())

.def("is_state_colliding",
py::overload_cast<const moveit::core::RobotState&, const std::string&, bool>(
Expand All @@ -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<py::gil_scoped_release>())

.def("is_state_constrained",
py::overload_cast<const moveit::core::RobotState&, const moveit_msgs::msg::Constraints&, bool>(
Expand Down Expand Up @@ -331,7 +340,8 @@ void initPlanningScene(py::module& m)

Returns:
bool: true if the path is valid otherwise false.
)")
)",
py::call_guard<py::gil_scoped_release>())

// 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
Expand All @@ -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<py::gil_scoped_release>())

.def("check_collision",
py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&,
Expand All @@ -364,7 +381,12 @@ void initPlanningScene(py::module& m)

Returns:
bool: true if state is in collision otherwise false.
)")

Note:
Releases the GIL. Do not use the passed robot state from another Python thread during the call:
its collision transforms may be updated.
)",
py::call_guard<py::gil_scoped_release>())

.def("check_collision",
py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&,
Expand All @@ -382,7 +404,12 @@ void initPlanningScene(py::module& m)

Returns:
bool: true if state is in collision otherwise false.
)")

Note:
Releases the GIL. Do not use the passed robot state from another Python thread during the call:
its collision transforms may be updated.
)",
py::call_guard<py::gil_scoped_release>())
// DEPRECATED! Use check_collision instead
.def("check_collision_unpadded",
py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&>(
Expand Down Expand Up @@ -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<py::gil_scoped_release>())

.def("check_self_collision",
py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&,
Expand All @@ -462,7 +496,12 @@ void initPlanningScene(py::module& m)

Returns:
bool: true if state is in self collision otherwise false.
)")

Note:
Releases the GIL. Do not use the passed robot state from another Python thread during the call:
its collision transforms may be updated.
)",
py::call_guard<py::gil_scoped_release>())

.def("check_self_collision",
py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&,
Expand All @@ -480,7 +519,12 @@ void initPlanningScene(py::module& m)

Returns:
bool: true if state is in self collision otherwise false.
)")

Note:
Releases the GIL. Do not use the passed robot state from another Python thread during the call:
its collision transforms may be updated.
)",
py::call_guard<py::gil_scoped_release>())

.def("save_geometry_to_file", &saveGeometryToFile, py::arg("file_path_and_name"),
R"(
Expand Down
7 changes: 6 additions & 1 deletion moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<py::gil_scoped_release>())

// Setting entire state values
.def("set_to_default_values", py::overload_cast<>(&moveit::core::RobotState::setToDefaultValues),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<py::gil_scoped_release>(),
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"),
Expand Down
107 changes: 107 additions & 0 deletions moveit_py/test/unit/test_planning_scene.py
Original file line number Diff line number Diff line change
@@ -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()
Loading