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()