class TestAPIPause(unittest.TestCase):
_SLEEP_TIME_S = 0.01
_TOLERANCE_FOR_MOTION_DETECTION_RAD = 0.01
_WAIT_CMD_STOP_TIME_OUT_S = 3
def setUp(self):
self.test_data = XmlTestdataLoader(_TEST_DATA_FILE_NAME)
self.robot = Robot(API_VERSION)
self.robot_motion_observer = RobotMotionObserver(PLANNING_GROUP_NAME)
self.robot.move(
Ptp(goal=self.test_data.get_joints("ZeroPose",
PLANNING_GROUP_NAME)))
self.ptp = Ptp(goal=self.test_data.get_joints("PTPJointLarge",
PLANNING_GROUP_NAME))
def tearDown(self):
if hasattr(self, 'robot'):
self.robot._release()
self.robot = None
if hasattr(self, 'test_data'):
del self.test_data
def test_pause_move_resume_succeed(self):
""" Test the sequence of pause, move, resume, move succeed
Test sequence:
1. Trigger pause.
2. Start robot motion.
3. Trigger resume.
Test Results:
1. -
2. Robot does not move.
3. Robot starts motion and moves to goal.
"""
# 1. trigger pause
self.robot.pause()
# 2. start the robot motion
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
# no movement
self.assertFalse(
self.robot_motion_observer.is_robot_moving(
self._SLEEP_TIME_S, self._TOLERANCE_FOR_MOTION_DETECTION_RAD))
# 3. trigger resume
self.robot.resume()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
rospy.loginfo(
"Wait until thread/command finished to evaluate result...")
move_thread.join()
self.assertFalse(move_thread.exception_thrown)
def test_move_pause_resume_succeed(self):
""" Test the sequence of move, pause, resume, move succeed
Test sequence:
1. Start robot motion and wait for the motion to start.
2. Trigger pause and wait for the motion to stop.
3. Trigger resume.
Test Results:
1. Robot moves.
2. Robot pauses motion.
3. Robot starts motion again and moves to goal.
"""
# 1. start the robot motion
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
# 2. trigger pause
self.robot.pause()
self.assertTrue(
self.robot_motion_observer.wait_motion_stop(
self._WAIT_CMD_STOP_TIME_OUT_S,
self._TOLERANCE_FOR_MOTION_DETECTION_RAD))
# 3. trigger resume
self.robot.resume()
rospy.loginfo(
"Wait until thread/command finished to evaluate result...")
move_thread.join()
self.assertFalse(move_thread.exception_thrown)
def test_move_pause_stop_resume_move_succeed(self):
""" Test the sequence of move, pause, stop, resume, move, succeed.
Test sequence:
1. Start robot motion and wait until motion starts.
2. Trigger pause.
3. Trigger stop.
4. Trigger resume.
5. Start robot motion again
Test Results:
1. Robot moves.
2. Robot pauses motion.
3. -
4. Robot does not move
5. Robot moves again
"""
# 1. start the robot motion
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
# 2. trigger pause
self.robot.pause()
self.assertTrue(
self.robot_motion_observer.wait_motion_stop(
self._WAIT_CMD_STOP_TIME_OUT_S,
self._TOLERANCE_FOR_MOTION_DETECTION_RAD))
# 3. trigger stop
self.robot.stop()
# 4. trigger resume
self.robot.resume()
# no movement
self.assertFalse(
self.robot_motion_observer.is_robot_moving(
self._SLEEP_TIME_S * 3,
self._TOLERANCE_FOR_MOTION_DETECTION_RAD))
# wait thread ends
move_thread.join()
# move stopped results in exception
self.assertTrue(move_thread.exception_thrown)
# 5. start the robot motion again
move_thread2 = MoveThread(self.robot, self.ptp)
move_thread2.start()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
move_thread2.join()
self.assertFalse(move_thread2.exception_thrown)
def test_pause_resume_move_succeed(self):
""" Test the sequence of pause, resume, move, succeed.
Test sequence:
1. Trigger pause.
2. Trigger resume.
3. Start robot motion and wait until motion starts.
Test Results:
1. -
2. -
3. Robot moves again
"""
# 1 trigger pause
self.robot.pause()
# 2 trigger resume
self.robot.resume()
# 3 start the robot motion
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
# wait thread ends
move_thread.join()
# move stopped results in exception
self.assertFalse(move_thread.exception_thrown)
def test_pause_move_stop(self):
""" Test the sequence of pause, move, stop.
Test sequence:
1. Trigger pause.
2. Start robot motion.
3. Trigger stop.
Test Results:
1. -
2. Robot wait for resume, not moving
3. Robot stop moving, exception thrown
"""
# 1 trigger pause
self.robot.pause()
# 2 start the robot motion, robot not moving
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
self.assertFalse(
self.robot_motion_observer.is_robot_moving(
self._SLEEP_TIME_S, self._TOLERANCE_FOR_MOTION_DETECTION_RAD))
# 3 trigger stop
self.robot.stop()
# wait thread ends
move_thread.join()
# move stopped results in exception
self.assertTrue(move_thread.exception_thrown)
def test_pause_pause_move_resume_succeed(self):
""" Test the sequence of pause, pause, move, resume, succeed.
Only one pause is buffered.
Test sequence:
1. Trigger pause two times.
2. Start robot motion.
3. Trigger resume.
Test Results:
1. -
2. Robot wait for resume, not moving.
3. Robot moves successfully.
"""
# 1. trigger pause twice
self.robot.pause()
self.robot.pause()
# 2. start the robot motion
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
# no movement
self.assertFalse(
self.robot_motion_observer.is_robot_moving(
self._SLEEP_TIME_S, self._TOLERANCE_FOR_MOTION_DETECTION_RAD))
# 3. trigger resume
self.robot.resume()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
move_thread.join()
self.assertFalse(move_thread.exception_thrown)
def test_move_resume_succeed(self):
""" Test the sequence of move, resume, succeed.
Resume during motion has no effect.
Test sequence:.
1. Start robot motion.
2. Trigger resume.
Test Results:
1. Robot moves.
2. Robot moves successfully.
"""
# 1. start the robot motion
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
# 2. trigger resume
self.robot.resume()
move_thread.join()
self.assertFalse(move_thread.exception_thrown)
def test_pause_move_pause_resume_pause_resume_succeed(self):
""" Test the sequence of pause, move, pause, resume, pause, resume, succeed.
Test sequence:.
1. Trigger pause.
2. Start robot motion.
3. Trigger pause.
4. Trigger resume wait until robot moves.
5. Trigger pause.
6. Trigger resume wait until robot moves..
Test Results:
1. -
2. Robot does not move.
3. -
4. Robot starts to move.
5. Robot movement paused.
6. Robot continues to move.
"""
# 1. trigger pause
self.robot.pause()
# 2. start the robot motion
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
# no movement
self.assertFalse(
self.robot_motion_observer.is_robot_moving(
self._SLEEP_TIME_S, self._TOLERANCE_FOR_MOTION_DETECTION_RAD))
# 3. trigger pause
self.robot.pause()
# 4. trigger resume
self.robot.resume()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
# 5. trigger pause
self.robot.pause()
# 6. trigger resume
self.robot.resume()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
move_thread.join()
self.assertFalse(move_thread.exception_thrown)
def test_pause_stop_move_succeed(self):
""" Test the sequence of pause, stop, move, succeed.
Test stop will erase pause request
Test sequence:.
1. Trigger pause.
2. Trigger stop.
3. Start robot motion.
Test Results:
1. -
2. -
3. Robot moves successfully.
"""
# 1. trigger pause
self.robot.pause()
# 2. trigger stop
self.robot.stop()
# 3. start the robot motion
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
move_thread.join()
self.assertFalse(move_thread.exception_thrown)
def test_stop_move_succeed(self):
""" Test the sequence of stop, move, succeed.
Test stop before move has no effect
Test sequence:.
1. Trigger stop.
2. Start robot motion.
Test Results:
1. -
2. Robot moves successfully.
"""
# 1. trigger stop
self.robot.stop()
# 2. start the robot motion
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
move_thread.join()
self.assertFalse(move_thread.exception_thrown)
def test_resume_move_succeed(self):
""" Test the sequence of resume, move, succeed.
Test resume before move has no effect
Test sequence:.
1. Trigger resume.
2. Start robot motion.
Test Results:
1. -
2. Robot moves successfully.
"""
# 1. trigger resume
self.robot.resume()
# 2. start the robot motion
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
move_thread.join()
self.assertFalse(move_thread.exception_thrown)
def test_move_pause_move_exception_resume_succeed(self):
""" Test the sequence of move, pause, move, exception
Test multiple call of move will result in exception
Test sequence:.
1. Start robot motion in separate thread.
2. Trigger pause
3. Start robot motion in current thread.
4. Trigger Resume
Test Results:
1. Robot starts to move.
2. Robot motion paused.
3. Exception throw in current thread.
4. Robot continues to move.
"""
# 1. start the robot motion in separate thread
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
# 2. trigger pause
self.robot.pause()
# 3. start the robot motion in current thread
try:
self.robot.move(self.ptp)
except RobotMoveAlreadyRunningError:
pass
else:
self.fail('Parallel call to move is not allowed.')
# 4. trigger resume
self.robot.resume()
move_thread.join()
self.assertFalse(move_thread.exception_thrown)
def test_move_pause_resume_stop_via_service(self):
""" Test the sequence of move, pause(service call), resume(service call), stop(service call)
Test sequence:
1. Start robot motion and wait for the motion to start.
2. Trigger pause via service and wait for the motion to stop.
3. Trigger resume via service and wait for the motion to start.
4. Trigger stop via service.
Test Results:
1. Robot moves.
2. Robot pauses motion.
3. Robot starts motion again and moves to goal.
4. Robot motion stops.
"""
# 1. start the robot motion
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
# 2. trigger pause via service
rospy.wait_for_service(self.robot._PAUSE_TOPIC_NAME)
pause = rospy.ServiceProxy(self.robot._PAUSE_TOPIC_NAME, Trigger)
pause()
self.assertTrue(
self.robot_motion_observer.wait_motion_stop(
self._WAIT_CMD_STOP_TIME_OUT_S,
self._TOLERANCE_FOR_MOTION_DETECTION_RAD))
# 3. trigger resume via service
rospy.wait_for_service(self.robot._RESUME_TOPIC_NAME)
resume = rospy.ServiceProxy(self.robot._RESUME_TOPIC_NAME, Trigger)
resume()
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=self._SLEEP_TIME_S))
# 4. trigger stop via service
rospy.wait_for_service(self.robot._STOP_TOPIC_NAME)
stop = rospy.ServiceProxy(self.robot._STOP_TOPIC_NAME, Trigger)
stop()
self.assertRaises(RobotMoveFailed, move_thread.join())
def test_pause_and_new_command(self):
"""Tests a time critical edge case of the pause/stop behavior, the edge case is explained in detail below.
Note:
To avoid an unstable/time critical test, private functions are called to simulate the time critical
edge case of the pause/stop behavior.
Detailed explanation of edge case (activity sequence):
- The move() function of class Robot is called. The move() function starts working and checks
the MoveControlState in _move_execution_loop().
- Before the _execute() method on the concrete command is called, a pause/stop is triggered.
- The _move_execution_loop() function continues its execution and calls the execute method of the
concrete command.
- If everything works correctly, the MoveControlState is atomically checked and the command execution
is halted/stopped because the pause/stop is detected.
"""
self.robot.pause()
cmd_for_testing = Ptp(goal=self.test_data.get_joints(
"PTPJointValid", PLANNING_GROUP_NAME))
res = cmd_for_testing._execute(self.robot)
self.assertEqual(self.robot._STOPPED,
res) # Command is execution is halted/stopped.
class TestAPIPauseConcurrency(unittest.TestCase):
_SLEEP_TIME_S = 0.01
_TOLERANCE_FOR_MOTION_DETECTION_RAD = 0.01
_WAIT_CMD_STOP_TIME_OUT_S = 3
def setUp(self):
self.test_data = XmlTestdataLoader(_TEST_DATA_FILE_NAME)
self.robot = Robot(API_VERSION)
self.robot_motion_observer = RobotMotionObserver(PLANNING_GROUP_NAME)
self.robot.move(Ptp(goal=self.test_data.get_joints("ZeroPose", PLANNING_GROUP_NAME)))
self.ptp = Ptp(goal=self.test_data.get_joints("PTPJointLarge", PLANNING_GROUP_NAME))
def tearDown(self):
if hasattr(self, 'robot'):
self.robot._release()
self.robot = None
if hasattr(self, 'test_data'):
del self.test_data
def move_ptp(self):
self.robot.move(self.ptp)
def test_pause_move_concurrency(self):
""" Test the concurrent situation of pause and move
Test sequence:.
1. Trigger the pause and move at the same time
2. Trigger resume
Test Results:
1. Robot does not move.
2. Robot moves successfully
"""
lock = threading.Lock()
cv = threading.Condition(lock)
thread_pause = threading.Thread(target=waited_trigger, args=(cv, self.robot.pause))
thread_pause.start()
thread_move = threading.Thread(target=waited_trigger, args=(cv, self.move_ptp))
thread_move.start()
rospy.sleep(1)
with cv:
cv.notify_all()
self.assertFalse(self.robot_motion_observer.is_robot_moving(self._SLEEP_TIME_S,
self._TOLERANCE_FOR_MOTION_DETECTION_RAD))
self.robot.resume()
thread_pause.join()
thread_move.join()
def test_resume_stop_concurrency_when_motion_is_paused(self):
""" Test the concurrent situation of resume and stop when motion is paused
Test sequence:.
1. Trigger Move and wait until move starts
2. Trigger Pause and wait until move stops
3. Trigger resume and stop at the same time
Test Results:
1. Robot starts to move.
2. Robot move stops
3. Robot does not move, exception throw
"""
# 1. start the robot motion
move_thread = MoveThread(self.robot, self.ptp)
move_thread.start()
self.assertTrue(self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD, sleep_interval=self._SLEEP_TIME_S))
# 2. trigger pause
self.robot.pause()
self.assertTrue(self.robot_motion_observer.wait_motion_stop(self._WAIT_CMD_STOP_TIME_OUT_S,
self._TOLERANCE_FOR_MOTION_DETECTION_RAD))
# 3. trigger stop/resume at the same time
lock = threading.Lock()
cv = threading.Condition(lock)
thread_stop = threading.Thread(target=waited_trigger, args=(cv, self.robot.stop))
thread_stop.start()
thread_resume = threading.Thread(target=waited_trigger, args=(cv, self.robot.resume))
thread_resume.start()
rospy.sleep(1)
with cv:
cv.notify_all()
thread_stop.join()
thread_resume.join()
self.assertRaises(RobotMoveFailed, move_thread.join())
def test_resume_move_concurrency_when_paused_without_move(self):
""" Test the concurrent situation of resume and move pause is requested
Test sequence:.
1. Trigger pause
2. Trigger resume and move at the same time
Test Results:
1. -
2. Robot moves successfully
"""
# 1. trigger pause
self.robot.pause()
# 2. trigger stop/resume at the same time
lock = threading.Lock()
cv = threading.Condition(lock)
thread_move = threading.Thread(target=waited_trigger, args=(cv, self.move_ptp))
thread_move.start()
thread_resume = threading.Thread(target=waited_trigger, args=(cv, self.robot.resume))
thread_resume.start()
rospy.sleep(1)
with cv:
cv.notify_all()
thread_move.join()
thread_resume.join()