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