def test_set_tool_io(self): """Test setting tool IO values, and check wheter the values has been set.""" maximum_messages = 5 io_values = np.array([0, 1], dtype=np.float64) io_target_msg = Composite.create_composite_message( self.io_parser, io_values) self.app.publish("ur.subgraph", "interface", "io_command", io_target_msg) messages = 0 io_state = None while messages < maximum_messages: io_state_msg = wait_for_new_message(self.app, "ur.subgraph", "interface", "io_state") if io_state_msg is not None: io_state = Composite.parse_composite_message( io_state_msg, self.io_parser) if ((io_state == io_values).all()): break messages += 1 if messages >= maximum_messages: self.fail("Could not read desired state after {} messages.".format( maximum_messages)) self.assertEqual(io_state[0], io_values[0]) self.assertEqual(io_state[1], io_values[1]) io_values = np.array([1, 0], dtype=np.float64) io_target_msg = Composite.create_composite_message( self.io_parser, io_values) self.app.publish("ur.subgraph", "interface", "io_command", io_target_msg) messages = 0 while messages < maximum_messages: io_state_msg = wait_for_new_message(self.app, "ur.subgraph", "interface", "io_state") if io_state_msg is not None: io_state = Composite.parse_composite_message( io_state_msg, self.io_parser) if ((io_state == io_values).all()): break messages += 1 if messages >= maximum_messages: self.fail("Could not read desired state after {} messages.".format( maximum_messages)) self.assertEqual(io_state[0], io_values[0]) self.assertEqual(io_state[1], io_values[1])
def wait_for_trajectory_result(self, timeout=15): """Wait for result of executed trajectory.""" msg = wait_for_new_message(self.app, "ur.controller", "ScaledMultiJointController", "trajectory_executed_succesfully", timeout=timeout) if msg is None: self.fail("Could not read trajectory result within timeout {}".format(timeout)) return msg.proto
def wait_for_driver(self, timeout=30): """Make sure the driver is started and publishing robot states.""" state_msg = wait_for_new_message(self.app, "ur.subgraph", "interface", "arm_state", timeout=timeout) if state_msg is None: self.fail( "Could not receive message from driver. Make sure the driver is actually running" )
def wait_for_trajectory_result(self, timeout=15): """Wait for result of executed trajectory.""" msg = wait_for_new_message(self.app, "ur.subgraph", "interface", "trajectory_executed_succesfully", timeout=timeout) if msg is None: self.fail( "Could not read trajectory result within timeout {}".format( timeout)) return msg.proto
def test_external_control(self): """Testing that it is possible to switch between robot control and driver control.""" do_dashboard_command(self.app, "play") self.wait_for_robot_program_running(True) joint_values = [-1.0 for i in range(6)] target_pos = np.array(joint_values, dtype=np.float64) target_pos_msg = Composite.create_composite_message( self.position_parser, target_pos) self.app.publish("ur.subgraph", "interface", "joint_target", target_pos_msg) trajectory_executed_succesfully = self.wait_for_trajectory_result() self.assertTrue(trajectory_executed_succesfully.flag == True, "failed to execute trajectory succesfully") stop_control_msg = Message.create_message_builder("BooleanProto") stop_control_msg.proto.flag = True self.app.publish("ur.subgraph", "interface", "stop_control", stop_control_msg) self.wait_for_robot_program_running(False, timeout=1) self.wait_for_robot_program_running(True) joint_states_msg = wait_for_new_message(self.app, "ur.subgraph", "interface", "arm_state") joint_states = Composite.parse_composite_message( joint_states_msg, self.position_parser) time.sleep(2) joint_command_msg = wait_for_new_message(self.app, "ur.controller", "ScaledMultiJointController", "joint_command") if joint_command_msg is None: self.fail( "Could not read joint command within timeout {}".format(2)) joint_command = Composite.parse_composite_message( joint_command_msg, self.position_parser) self.assertTrue( np.allclose(joint_command, joint_states, rtol=0.001, atol=0.001), "current joint command {} is not equal to expected joint states {}" .format(joint_command, joint_states)) new_joint_command_msg = wait_for_new_message( self.app, "ur.controller", "ScaledMultiJointController", "joint_command") if joint_command_msg is None: self.fail( "Could not read joint command within timeout {}".format(2)) new_joint_command = Composite.parse_composite_message( new_joint_command_msg, self.position_parser) self.assertTrue( np.allclose(joint_command, new_joint_command, rtol=0.001, atol=0.001), "current joint command {} and last joint command is not equal {}". format(new_joint_command, joint_command))