moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
test_planning_scene.py
Go to the documentation of this file.
1import os
2import sys
3import threading
4import time
5import unittest
6
7from test_moveit.core.collision_detection import CollisionRequest, CollisionResult
8from test_moveit.core.planning_scene import PlanningScene
9from test_moveit.core.robot_model import RobotModel
10from test_moveit.core.robot_state import RobotState
11
12dir_path = os.path.dirname(os.path.realpath(__file__))
13URDF_FILE = "{}/fixtures/panda.urdf".format(dir_path)
14SRDF_FILE = "{}/fixtures/panda.srdf".format(dir_path)
15
16GROUP = "panda_arm"
17
18
20 """Helper function that returns a RobotModel instance."""
21 return RobotModel(urdf_xml_path=URDF_FILE, srdf_xml_path=SRDF_FILE)
22
23
24def count_ticks_while(call, duration=0.5):
25 """
26 Run call() in a loop for duration seconds and count how often another Python thread gets to run meanwhile.
27
28 The switch interval is raised far above duration, so the interpreter does not hand the GIL to the other
29 thread on its own: the other thread only runs while call() releases the GIL.
30 """
31 stop = threading.Event()
32 ticks = 0
33
34 def tick():
35 nonlocal ticks
36 while not stop.is_set():
37 ticks += 1
38 time.sleep(0.0001) # releases the GIL, so call() can take it back
39
40 old_interval = sys.getswitchinterval()
41 sys.setswitchinterval(30.0)
42 ticker = threading.Thread(target=tick, daemon=True)
43 try:
44 ticker.start()
45 deadline = time.monotonic() + duration
46 while time.monotonic() < deadline:
47 call()
48 result = ticks
49 finally:
50 stop.set()
51 sys.setswitchinterval(old_interval)
52 ticker.join()
53 return result
54
55
56class TestPlanningSceneGilRelease(unittest.TestCase):
57 """The collision checks release the GIL, so other Python threads keep running during bulk checks."""
58
59 @classmethod
63 def setUp(self):
64 self.scene = PlanningScene(self.robot_model)
65 self.state = RobotState(self.robot_model)
66 self.state.set_to_default_values()
67 self.state.update()
68
69 def assert_releases_gil(self, call):
70 call() # warm up, so the first call's setup cost is not measured
71 ticks = count_ticks_while(call)
72 # With the GIL held for the whole loop the other thread gets at most a tick or two.
73 self.assertGreater(ticks, 10)
74
76 self.assert_releases_gil(lambda: self.scene.is_state_valid(self.state, GROUP))
77
79 self.assert_releases_gil(lambda: self.scene.is_state_colliding(GROUP))
80
83 lambda: self.scene.is_state_colliding(self.state, GROUP)
84 )
85
88 lambda: self.scene.check_collision(CollisionRequest(), CollisionResult())
89 )
90
93 lambda: self.scene.check_collision(
94 CollisionRequest(), CollisionResult(), self.state
95 )
96 )
97
100 lambda: self.scene.check_self_collision(
101 CollisionRequest(), CollisionResult(), self.state
102 )
103 )
104
105
106if __name__ == "__main__":
107 unittest.main()
count_ticks_while(call, duration=0.5)