class TestAsyncExecute(unittest.TestCase):
def setUp(self):
self.commander = ClopemaRobotCommander("arms")
self.commander.set_start_state_to_current_state()
self.commander.set_named_target("home_arms")
self.commander.go(wait=True)
self.commander.set_joint_value_target({'r1_joint_t':1.0})
self.traj = self.commander.plan()
def test_async(self):
"""Test whether the async execute returns """
self.commander.async_execute(self.traj)
state1 = self.commander.get_current_state()
rospy.sleep(0.1)
state2 = self.commander.get_current_state()
a = state1.joint_state.position[state1.joint_state.name.index('r1_joint_t')]
b = state2.joint_state.position[state2.joint_state.name.index('r1_joint_t')]
self.assertNotEqual(a,b)
def test_stop(self):
"""
Test whether is possible to stop the execution of a trajectory in the
middle of execution.
"""
self.commander.async_execute(self.traj)
self.commander.stop()
rospy.sleep(0.1)
state1 = self.commander.get_current_state()
rospy.sleep(0.1)
state2 = self.commander.get_current_state()
a = state1.joint_state.position[state1.joint_state.name.index('r1_joint_t')]
b = state2.joint_state.position[state2.joint_state.name.index('r1_joint_t')]
self.assertEqual(a,b)
self.assertNotEqual(a, 1.0)
self.assertNotEqual(a, 0.0)
if args['T'] is not None and args['T'] is not '-':
robot.set_joint_value_target("r2_joint_t", radians(float(args['T'])))
traj = robot.plan()
# Print some info
print "From: ", " ".join([
"% +3.2f" % degrees(r)
for r in traj.joint_trajectory.points[0].positions
])
print "To: ", " ".join([
"% +3.2f" % degrees(r)
for r in traj.joint_trajectory.points[-1].positions
])
print "Length: ", len(traj.joint_trajectory.points)
if args['--speed'] is not None:
print "Speed ", "%+3.2f" % float(args['--speed'])
else:
print "Speed ", "%+3.2f" % robot.get_robot_speed()
r = ask("Execute trajectory? ", {'Y': 'Yes', 'n': 'No'})
if r == 'y':
if args['--speed'] is not None:
robot.set_robot_speed(float(args['--speed']))
robot.go()
if args['--sof']:
robot.set_servo_power_off(True)
#!/usr/bin/env python
# Copyright (c) CloPeMa, EU-FP7 - All Rights Reserved
#
# Author: Libor Wagner <*****@*****.**>
# Institute: Czech Technical University in Prague
# Created on: Oct 15, 2013
from clopema_libs.ui import ask
from clopema_robot import ClopemaRobotCommander
if __name__ == '__main__':
group = ClopemaRobotCommander("arms")
group.set_named_target('home_arms')
group.plan()
r = ask("Execute trajectory", {'Y': "Yes", "n": "No"})
if r == 'y':
group.go()
group = ClopemaRobotCommander("ext")
group.set_named_target('home_ext')
group.plan()
r = ask("Execute trajectory", {'Y': "Yes", "n": "No"})
if r == 'y':
group.go()
