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 _test_stop(robot):
"""Tests stopping a robot motion.
Test Sequence:
1. Start robot motion and wait for the motion to start.
2. Trigger stop.
3. Start robot motion with different goal.
Expected Results:
1. Robot moves.
2. Robot stops motion before reaching goal.
3. Robot starts motion and moves to the other goal.
"""
if _askPermission(_test_stop.__name__) == 0:
return
_robot_motion_observer = RobotMotionObserver(_PLANNING_GROUP)
# 1. Create simple ptp command and start thread for movement
ptp = Ptp(goal=[0, -0.78, 0.78, 0, 1.56, 0], vel_scale=_DEFAULT_VEL_SCALE)
move_thread = MoveThread(robot, ptp)
move_thread.start()
_robot_motion_observer.wait_motion_start(
sleep_interval=_SLEEP_TIME_S,
move_tolerance=_TOLERANCE_FOR_MOTION_DETECTION_RAD)
# 2. Trigger stop
robot.stop()
# Wait for thread to finish
move_thread.join()
# 3. Trigger motion with different goal
robot.move(
Ptp(goal=[0.78, 0.0, 0.0, 0, 0, 0],
relative=True,
vel_scale=_DEFAULT_VEL_SCALE))
_askSuccess(
_test_stop.__name__,
'The robot should have stopped before reaching the upper pick position and continued moving in '
'another direction.')
def _test_pause_resume(robot):
"""Tests pausing and resuming a robot motion.
Test Sequence:
1. Start robot motion and wait for the motion to start.
2. Trigger pause and wait 5 seconds.
3. Trigger resume.
Expected Results:
1. Robot moves.
2. Robot stops motion.
3. Robot starts motion again and moves to goal.
"""
if _askPermission(_test_pause_resume.__name__) == 0:
return
_robot_motion_observer = RobotMotionObserver(_PLANNING_GROUP)
# 1. Create simple ptp command and start thread for movement
ptp = Ptp(goal=[0, 0.39, -0.39, 0, 0.78, 0], vel_scale=_DEFAULT_VEL_SCALE)
move_thread = MoveThread(robot, ptp)
move_thread.start()
_robot_motion_observer.wait_motion_start(
sleep_interval=_SLEEP_TIME_S,
move_tolerance=_TOLERANCE_FOR_MOTION_DETECTION_RAD)
# 2. Trigger pause
robot.pause()
rospy.sleep(5.0)
# 3. Trigger resume
robot.resume()
# Wait for thread to finish
move_thread.join()
_askSuccess(
_test_pause_resume.__name__,
'The robot should have paused his movement and continued after approximately 5 seconds.'
)
def _test_pause_stop(robot):
"""Tests pausing and canceling a robot motion.
Test Sequence:
1. Start robot motion and wait for the motion to start.
2. Trigger pause.
3. Trigger stop.
Expected Results:
1. Robot moves.
2. Robot stops motion.
3. Move thread terminates.
"""
if _askPermission(_test_pause_stop.__name__) == 0:
return
_robot_motion_observer = RobotMotionObserver(_PLANNING_GROUP)
# 1. Create simple ptp command and start thread for movement
ptp = Ptp(goal=[0, -0.78, 0.78, 0, 1.56, 0], vel_scale=_DEFAULT_VEL_SCALE)
move_thread = MoveThread(robot, ptp)
move_thread.start()
_robot_motion_observer.wait_motion_start(
sleep_interval=_SLEEP_TIME_S,
move_tolerance=_TOLERANCE_FOR_MOTION_DETECTION_RAD)
# 2. Trigger pause
robot.pause()
# 3. Trigger stop
robot.stop()
# Wait for thread to finish
move_thread.join()
_askSuccess(
_test_pause_stop.__name__,
'The robot should have paused his movement before reaching the upper pick position '
'and the test should have terminated immediately.')
def setUp(self):
self.robot = Robot(API_VERSION)
self.robot_motion_observer = RobotMotionObserver(group_name="gripper")
self.test_data = XmlTestdataLoader(_TEST_DATA_FILE_NAME)
class TestAPIGripper(unittest.TestCase):
""" Test the gripper command"""
_SLEEP_TIME_S = 0.01
_TOLERANCE_FOR_MOTION_DETECTION_RAD = 0.01
def setUp(self):
self.robot = Robot(API_VERSION)
self.robot_motion_observer = RobotMotionObserver(group_name="gripper")
self.test_data = XmlTestdataLoader(_TEST_DATA_FILE_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_gripper_cmd_convert_invalid_goal(self):
""" Test the gripper convert function with invalid goal.
Test sequence:
1. Call gripper convert function with a list.
Test Results:
1. raises Exception.
"""
gripper_cmd = Gripper(goal=[1, 2])
self.assertRaises(RuntimeError, gripper_cmd._cmd_to_request,
self.robot)
def test_gripper_cmd_convert(self):
""" Test the gripper convert function works correctly.
Test sequence:
1. Call gripper convert function with a valid floating point goal.
Test Results:
1. Correct motion plan request is returned.
"""
gripper_cmd = Gripper(goal=0.02, vel_scale=0.1)
req = gripper_cmd._cmd_to_request(self.robot)
self.assertIsNotNone(req)
self.assertEqual("gripper", req.group_name)
# gripper command is internal a ptp command
self.assertEqual("PTP", req.planner_id)
self.assertEqual(0.02,
req.goal_constraints[0].joint_constraints[0].position)
self.assertEqual(0.1, req.max_velocity_scaling_factor)
def test_gripper_cmd_convert_int(self):
""" Test the gripper convert function works correctly.
Test sequence:
1. Call gripper convert function with a valid integer goal.
Test Results:
1. Correct motion plan request is returned.
"""
gripper_cmd = Gripper(goal=0, vel_scale=0.1)
req = gripper_cmd._cmd_to_request(self.robot)
self.assertIsNotNone(req)
self.assertEqual("gripper", req.group_name)
# gripper command is internal a ptp command
self.assertEqual("PTP", req.planner_id)
self.assertEqual(0.0,
req.goal_constraints[0].joint_constraints[0].position)
self.assertEqual(0.1, req.max_velocity_scaling_factor)
def test_gripper_execution(self):
""" Test execution of valid gripper command works successfully.
Test sequence:
1. Execute a valid gripper command
Test results:
1. Move function returns without throwing an exception.
"""
self.robot.move(Gripper(goal=0.01))
def test_invalid_gripper_execution(self):
""" Test execution of invalid gripper command works successfully.
Test sequence:
1. Execute an invalid gripper command with out of bounds joints
Test results:
1. Move function throws exception.
"""
self.assertRaises(RobotMoveFailed, self.robot.move, Gripper(goal=1))
def test_gripper_stop_during_command(self):
"""Test stop during gripper command execution.
Test sequence:
1. Move the gripper to zero position
2. Start given command in separate thread.
3. Wait till robot is moving then execute stop function in main test thread.
Test Results:
1. Robot's gripper closes.
2. Robot's gripper starts moving.
3. Execute methods ends and throws correct exception.
"""
# 1
self.robot.move(Gripper(goal=0.00))
# 2
move_thread = MoveThread(self.robot, Gripper(goal=0.02))
move_thread.start()
# 3
# Wait till movement started
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("Call stop function.")
self.robot.stop()
rospy.loginfo(
"Wait till thread/command finished to evaluate result...")
move_thread.join()
# Check return value of execute method
self.assertTrue(move_thread.exception_thrown)
def test_get_current_gripper_joint(self):
""" Check if the current joints can be retrieved correctly
Test sequence:
1. Move robot to a goal position via ptp.
2. Check the current pose by robot.get_current_joints().
Test Results:
1. Move function returns without throwing an exception.
2. The current joints are same as the goal joints.
"""
# 1
goal_joint = 0.03
self.robot.move(Gripper(goal=goal_joint))
# 2
current_joints = self.robot.get_current_joint_states("gripper")
self.assertEqual(1, len(current_joints))
self.assertAlmostEqual(goal_joint, current_joints[0])
def setUp(self):
rospy.loginfo("SetUp called...")
self.test_data = XmlTestdataLoader(_TEST_DATA_FILE_NAME)
self.robot = Robot(API_VERSION)
self.robot_motion_observer = RobotMotionObserver(PLANNING_GROUP_NAME)
class TestAPIExecutionStop(unittest.TestCase):
"""
Test the Api Adapter Node. Result is evaluated from the error code in action result
Strategy for error cases that are caught inside CommandPlanner: We only test for one
error case, that the returned error is passed on to the user.
Not every single error case is covered by a test here; referring to the tests on lower
levels instead.
"""
_SLEEP_TIME_S = 0.01
_TOLERANCE_FOR_MOTION_DETECTION_RAD = 0.01
def setUp(self):
rospy.loginfo("SetUp called...")
self.test_data = XmlTestdataLoader(_TEST_DATA_FILE_NAME)
self.robot = Robot(API_VERSION)
self.robot_motion_observer = RobotMotionObserver(PLANNING_GROUP_NAME)
def tearDown(self):
rospy.loginfo("TearDown called...")
if hasattr(self, 'robot'):
self.robot._release()
self.robot = None
if hasattr(self, 'test_data'):
del self.test_data
def getCmds(self, name, cmd_type='ptp', start_type='joint', goal_type='joint', circ_type='center'):
if cmd_type == 'ptp' and goal_type == 'joint':
return Ptp()
if cmd_type == 'lin':
lin_cmd = self.test_data.get_lin(name)
# start cmd
if start_type == 'joint':
start_ptp = Ptp(goal=self.test_data.get_joints(lin_cmd[START_POS_STR], lin_cmd[PLANNING_GROUP_STR]))
else:
start_ptp = Ptp(goal=self.test_data.get_pose(lin_cmd[START_POS_STR], lin_cmd[PLANNING_GROUP_STR]))
if goal_type == 'joint':
lin = Lin(goal=self.test_data.get_joints(lin_cmd[END_POS_STR], lin_cmd[PLANNING_GROUP_STR]),
vel_scale=lin_cmd[VEL_STR], acc_scale=lin_cmd[ACC_STR])
else:
lin = Lin(goal=self.test_data.get_pose(lin_cmd[END_POS_STR], lin_cmd[PLANNING_GROUP_STR]),
vel_scale=lin_cmd[VEL_STR], acc_scale=lin_cmd[ACC_STR])
return start_ptp, lin
if cmd_type == "circ":
# circ cmd
if circ_type == 'interim':
circ_cmd = self.test_data.get_circ(name, INTERIM_POS_STR)
circ = Circ(goal=self.test_data.get_pose(circ_cmd[END_POS_STR], circ_cmd[PLANNING_GROUP_STR]),
interim=self.test_data.get_pose(circ_cmd[INTERIM_POS_STR],
circ_cmd[PLANNING_GROUP_STR]).position,
vel_scale=circ_cmd[VEL_STR], acc_scale=circ_cmd[ACC_STR])
else:
circ_cmd = self.test_data.get_circ(name, CENTER_POS_STR)
circ = Circ(goal=self.test_data.get_pose(circ_cmd[END_POS_STR], circ_cmd[PLANNING_GROUP_STR]),
center=self.test_data.get_pose(circ_cmd[CENTER_POS_STR],
circ_cmd[PLANNING_GROUP_STR]).position,
vel_scale=circ_cmd[VEL_STR], acc_scale=circ_cmd[ACC_STR])
# start cmd
if start_type == 'joint':
start_ptp = Ptp(goal=self.test_data.get_joints(circ_cmd[START_POS_STR], circ_cmd[PLANNING_GROUP_STR]))
else:
start_ptp = Ptp(goal=self.test_data.get_pose(circ_cmd[START_POS_STR], circ_cmd[PLANNING_GROUP_STR]))
return start_ptp, circ
if cmd_type == "sequence":
sequence_cmd = self.test_data.get_sequence(name)
seq = Sequence()
# first cmd
first_cmd = sequence_cmd.pop(0)
start_ptp, lin = self.getCmds(name=first_cmd[NAME_STR], cmd_type=first_cmd[TYPE_STR], start_type="joint",
goal_type="pose")
seq.append(lin, float(first_cmd[BLEND_RADIUS_STR]))
for cmd in sequence_cmd:
start_, lin_ = self.getCmds(name=cmd[NAME_STR], cmd_type=cmd[TYPE_STR], start_type="joint",
goal_type="pose")
seq.append(lin_, float(cmd[BLEND_RADIUS_STR]))
return start_ptp, seq
def stop_during_execution(self, cmd_for_testing):
""" Tests what happens if stop is triggered during execution of given command.
Test sequence:
1. Start given command in separate thread.
2. Wait till robot is moving then execute stop function in main test thread.
Test Results:
1. Robot starts moving.
2. Execute methods ends and throws correct exception.
"""
# +++++++++++++++++++++++
rospy.loginfo("Step 1")
# +++++++++++++++++++++++
move_thread = MoveThread(self.robot, cmd_for_testing)
move_thread.start()
# +++++++++++++++++++++++
rospy.loginfo("Step 2")
# +++++++++++++++++++++++
# Wait till movement started
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("Call stop function.")
self.robot.stop()
rospy.loginfo("Wait till thread/command finished to evaluate result...")
move_thread.join()
# Check return value of execute method
self.assertTrue(move_thread.exception_thrown)
def test_none_robot_cmd(self):
""" Check what happens if somebody tries to execute a none move command.
Test sequence:
1. Create none move command.
2. Call the execute function directly
Test Results:
1. Move function throws exception.
2. Error is returned.
"""
self.assertRaises(RobotUnknownCommandType, self.robot.move, None)
ptp = Ptp()
self.assertEquals(ptp._execute(self.robot), Robot._FAILURE)
def test_ptp_execution(self):
""" Test execution of valid ptp command works successfully.
Test sequence:
1. Execute a valid ptp command
Test results:
1. Move function returns without throwing an exception.
"""
self.robot.move(Ptp(goal=self.test_data.get_joints("PTPJointValid", PLANNING_GROUP_NAME)))
def test_invalid_ptp_execution(self):
""" Test execution of invalid ptp command works successfully.
Test sequence:
1. Execute an invalid ptp command with out of bounds joints
Test results:
1. Move function throws exception.
"""
self.assertRaises(RobotMoveFailed, self.robot.move,
Ptp(goal=self.test_data.get_joints("PTPJointInvalid", PLANNING_GROUP_NAME)))
def test_lin_execution(self):
""" Test execution of valid lin command works successfully.
Test sequence:
1. Move the robot to start pose via ptp
2. Execute a valid lin command
Test results:
1. Move function returns without throwing an exception.
2. Move function returns without throwing an exception.
"""
start_ptp, lin = self.getCmds(name="ValidLINCmd", cmd_type="lin", start_type="joint", goal_type="pose")
self.robot.move(start_ptp)
self.robot.move(lin)
def test_invalid_lin_execution(self):
""" Test execution of invalid lin command will fail.
Test sequence:
1. Move the robot to start pose via ptp
2. Execute an invalid lin command which has an unreachable goal pose
Test results:
1. Move function returns without throwing an exception.
2. Move function throws exception.
"""
start_ptp, lin = self.getCmds(name="InvalidLINCmd", cmd_type="lin", start_type="joint", goal_type="pose")
self.robot.move(start_ptp)
self.assertRaises(RobotMoveFailed, self.robot.move, lin)
def test_circ_center_execution(self):
""" Test execution of valid circ center command works successfully.
Test sequence:
1. Move the robot out of zero pose via ptp joint
2. Move the robot to start pose via ptp
3. Execute a valid circ center command
Test results:
1. Move function returns without throwing an exception.
2. Move function returns without throwing an exception.
3. Move function returns without throwing an exception.
"""
self.robot.move(Ptp(goal=self.test_data.get_joints("PTPJointValid", PLANNING_GROUP_NAME)))
start_ptp, circ = self.getCmds("ValidCircCmd", cmd_type="circ", start_type="pose", circ_type="center")
self.robot.move(start_ptp)
self.robot.move(circ)
def test_invalid_circ_center_execution(self):
""" Test execution of invalid circ center command will fail.
Test sequence:
1. Move the robot out of zero pose via ptp joint
2. Move the robot to start pose via ptp
3. Execute an invalid circ center command
Test results:
1. Move function returns without throwing an exception.
2. Move function returns without throwing an exception.
3. Move function throws exception.
"""
self.robot.move(Ptp(goal=self.test_data.get_joints("PTPJointValid", PLANNING_GROUP_NAME)))
start_ptp, circ = self.getCmds("InvalidCircCmd", cmd_type="circ", start_type="pose", circ_type="center")
self.robot.move(start_ptp)
self.assertRaises(RobotMoveFailed, self.robot.move, circ)
def test_circ_interim_execution(self):
""" Test execution of valid circ interim command works successfully.
Test sequence:
1. Move the robot out of zero pose via ptp joint
2. Move the robot to start pose via ptp
3. Execute a valid circ interim command
Test results:
1. Move function returns without throwing an exception.
2. Move function returns without throwing an exception.
3. Move function returns without throwing an exception.
"""
self.robot.move(Ptp(goal=self.test_data.get_joints("PTPJointValid", PLANNING_GROUP_NAME)))
start_ptp, circ = self.getCmds("ValidCircCmd", cmd_type="circ", start_type="pose", circ_type="interim")
self.robot.move(start_ptp)
self.robot.move(circ)
def test_lin_lin_blend_execution(self):
""" Test execution of valid sequence command, consisting of lin-lin, works successfully.
Test sequence:
1. Move the robot to start pose via ptp
2. Execute a valid circ interim command
Test results:
1. Move function returns without throwing an exception.
2. Move function returns without throwing an exception.
"""
rospy.loginfo("\n\nStart of test...\n")
start_ptp, sequence = self.getCmds("LINLINBlend", cmd_type="sequence")
self.robot.move(start_ptp)
self.robot.move(sequence)
def test_invalid_lin_lin_blend_execution(self):
""" Test execution of an invalid sequence command with too large velocity scale fails.
Test sequence:
1. Move the robot to start pose via ptp
2. Execute an invalid sequence command with too large velocity scale
Test results:
1. Move function returns without throwing an exception.
2. Move function throws exception.
"""
rospy.loginfo("\n\nStart of test...\n")
start_ptp, sequence = self.getCmds("InvalidLINLINBlend", cmd_type="sequence")
self.robot.move(start_ptp)
self.assertRaises(RobotMoveFailed, self.robot.move, sequence)
def test_execute_abstract_cmd(self):
""" An AbstractCmd (base class) object should not be executable
Test sequence:
1. Execute an invalid AbstractCmd command
Test results:
1. Move function throws exception.
"""
import pilz_robot_programming.commands as cmd
abstract_cmd = cmd._AbstractCmd() # private abstract base
self.assertRaises(RobotMoveFailed, self.robot.move, abstract_cmd)
def test_ptp_stop_during_command(self):
"""Test stop during ptp command execution.
For more details see function stop_during_execution.
"""
# move to start pose
self.robot.move(Ptp(goal=self.test_data.get_joints("ZeroPose", PLANNING_GROUP_NAME)))
cmd_for_testing = Ptp(goal=self.test_data.get_joints("PTPJointValid", PLANNING_GROUP_NAME))
self.stop_during_execution(cmd_for_testing)
def test_lin_stop_during_command(self):
"""Test stop during lin command execution.
For more details see function stop_during_execution.
"""
start_ptp, lin = self.getCmds(name="ValidLINCmd", cmd_type="lin", start_type="joint", goal_type="pose")
self.robot.move(start_ptp)
self.stop_during_execution(lin)
def test_circ_stop_during_command(self):
"""Test stop during circ command execution.
For more details see function stop_during_execution.
"""
self.robot.move(Ptp(goal=self.test_data.get_joints("PTPJointValid", PLANNING_GROUP_NAME)))
start_ptp, circ = self.getCmds("ValidCircCmd", cmd_type="circ", start_type="pose", circ_type="center")
self.robot.move(start_ptp)
self.stop_during_execution(circ)
def test_sequence_stop_during_command(self):
"""Test stop during sequence command execution.
For more details see function stop_during_execution.
"""
start_ptp, sequence = self.getCmds("LINLINBlend", cmd_type="sequence")
self.robot.move(start_ptp)
self.stop_during_execution(sequence)
def test_move_external_stop(self):
""" Tests what happens if stop is triggered during execution of given command.
Test sequence:
1. Start given command in separate thread.
2. Wait till robot is moving then execute client.cancel_all_goals() in main test thread.
Test Results:
1. Robot starts moving.
2. Execute methods ends and throws correct exception.
"""
# Create move_group action client for external stop
move_client = SimpleActionClient(SEQUENCE_TOPIC, MoveGroupSequenceAction)
rospy.loginfo("Waiting for connection to action server " + SEQUENCE_TOPIC + "...")
move_client.wait_for_server()
rospy.logdebug("Connection to action server " + SEQUENCE_TOPIC + " established.")
# Move to zero pose
self.robot.move(Ptp(goal=self.test_data.get_joints("ZeroPose", PLANNING_GROUP_NAME)))
# +++++++++++++++++++++++
rospy.loginfo("Step 1")
# +++++++++++++++++++++++
ptp = Ptp(goal=self.test_data.get_joints("PTPJointValid", PLANNING_GROUP_NAME), vel_scale=0.05)
move_thread = MoveThread(self.robot, ptp)
move_thread.start()
# +++++++++++++++++++++++
rospy.loginfo("Step 2")
# +++++++++++++++++++++++
# Wait till movement started
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("Call stop function from external client.")
move_client.cancel_all_goals()
rospy.loginfo("Wait till thread/command finished to evaluate result...")
move_thread.join()
# Check return value of execute method
self.assertTrue(move_thread.exception_thrown)
def test_move_multi_threading_prevention(self):
""" Tests what happens if two move() are started in two threads.
Test sequence:
1. Start a command in separate thread.
2. Start another command in main test thread.
Test Results:
1. Robot starts moving.
2. Second move throws RobotMoveAlreadyRunningError. First move finishes successfully.
"""
# Move to zero pose
self.robot.move(Ptp(goal=self.test_data.get_joints("ZeroPose", PLANNING_GROUP_NAME)))
# +++++++++++++++++++++++
rospy.loginfo("Step 1")
# +++++++++++++++++++++++
ptp = Ptp(goal=self.test_data.get_joints("PTPJointValid", PLANNING_GROUP_NAME), vel_scale=0.1)
move_thread = MoveThread(self.robot, ptp)
move_thread.start()
# +++++++++++++++++++++++
rospy.loginfo("Step 2")
# +++++++++++++++++++++++
# Wait till movement started
self.assertTrue(self.robot_motion_observer.wait_motion_start(
move_tolerance=self._TOLERANCE_FOR_MOTION_DETECTION_RAD, sleep_interval=self._SLEEP_TIME_S))
# start another command
try:
self.robot.move(Ptp(goal=self.test_data.get_joints("ZeroPose", PLANNING_GROUP_NAME)))
except RobotMoveAlreadyRunningError:
pass
else:
self.fail('robot.move() can run in two threads.')
rospy.loginfo("Wait till thread/command finished to evaluate result...")
move_thread.join()
# Check return value of execute method
self.assertFalse(move_thread.exception_thrown)
class TestAPIProgramKill(unittest.TestCase):
"""
Test the API behaviour when the python program is killed.
"""
_PTP_TEST_NAME = "PTPJointValid"
_WAIT_TIME_FOR_MOTION_DETECTION_SEC = 8.0
def setUp(self):
rospy.loginfo("SetUp called...")
self.test_data = XmlTestdataLoader(_TEST_DATA_FILE_NAME)
self.robot_motion_observer = RobotMotionObserver(_GROUP_NAME)
def tearDown(self):
rospy.loginfo("TearDown called...")
if hasattr(self, 'test_data'):
del self.test_data
def _get_robot_move_command(self):
"""Build command to move the robot (to be called as subprocess)
:returns list containing executable with path and appropriate arguments
"""
ptp_goal = [
str(x) for x in self.test_data.get_joints(self._PTP_TEST_NAME,
_GROUP_NAME)
]
movecmd = RosPack().get_path(
"pilz_robot_programming"
) + '/test/integrationtests/movecmd.py ptp joint'
return movecmd.split(" ") + ptp_goal
def test_stop_at_program_kill(self):
"""
Test if robot movement is stopped when program is killed.
Test Sequence:
1. Request robot movement in a subprocess.
2. Send kill signal.
Expected Results:
1. -
2. Robot does not move after subprocess has terminated.
"""
# 1. Start robot movement
proc = subprocess.Popen(self._get_robot_move_command())
# Wait until movement is detected
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
wait_time_out=self._WAIT_TIME_FOR_MOTION_DETECTION_SEC))
# 2. Send kill signal
proc.send_signal(signal.SIGINT)
# Wait until process has terminated.
proc.wait()
self.assertFalse(self.robot_motion_observer.is_robot_moving())
class TestAPIProgramTermination(unittest.TestCase):
"""
Test the API behaviour when the python program is terminated.
"""
_PTP_TEST_NAME = "PTPJointValid"
_WAIT_TIME_FOR_MOTION_DETECTION_SEC = 8.0
def setUp(self):
rospy.loginfo("SetUp called...")
self.test_data = XmlTestdataLoader(_TEST_DATA_FILE_NAME)
self.robot_motion_observer = RobotMotionObserver(_GROUP_NAME)
def tearDown(self):
rospy.loginfo("TearDown called...")
if hasattr(self, 'test_data'):
del self.test_data
def _get_robot_move_command(self):
"""Build command to move the robot (to be called as subprocess)
:returns list containing executable with path and appropriate arguments
"""
ptp_goal = [
str(x) for x in self.test_data.get_joints(self._PTP_TEST_NAME,
_GROUP_NAME)
]
movecmd = RosPack().get_path(
"pilz_robot_programming"
) + '/test/integrationtests/movecmd.py ptp joint'
return movecmd.split(" ") + ptp_goal
def test01_stop_at_program_interrupt(self):
"""
Test if robot movement is stopped when program is interrupted (Ctrl-c).
Test Sequence:
1. Request robot movement in a subprocess.
2. Send interrupt signal.
Expected Results:
1. -
2. Robot does not move after subprocess has terminated.
"""
# 1. Start robot movement
proc = subprocess.Popen(self._get_robot_move_command())
# Wait until movement is detected
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
wait_time_out=self._WAIT_TIME_FOR_MOTION_DETECTION_SEC))
# 2. Send interrupt signal
proc.send_signal(signal.SIGINT)
# Wait until process has terminated.
proc.wait()
self.assertFalse(self.robot_motion_observer.is_robot_moving())
def test09_instantiation_after_program_kill(self):
"""
Test if robot can be instantiated after a program was killed. In case of a failure, the robot can no longer be
instantiated and a robot movement may still be executed. So this should be the last test case.
Test Sequence:
1. Request robot movement in a subprocess.
2. Send kill signal and wait for termination of the subprocess.
3. Try to create an instance of Robot.
Expected Results:
1. -
2. -
3. No exception is thrown
"""
# 1. Start robot movement
proc = subprocess.Popen(self._get_robot_move_command())
# Wait until movement is detected
self.assertTrue(
self.robot_motion_observer.wait_motion_start(
wait_time_out=self._WAIT_TIME_FOR_MOTION_DETECTION_SEC))
# 2. Send kill signal
proc.send_signal(signal.SIGKILL)
# Wait until process has terminated.
proc.wait()
try:
r = Robot(_API_VERSION)
except RobotMultiInstancesError:
self.fail('Instantiation after program kill does throw exception.')
else:
del r
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()