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