def main():
# Process cammand line arguments
argv = rospy.myargv(argv=sys.argv)
opt = docopt(__doc__, argv=argv[1:])
opt_debug = opt['--debug']
opt_trajectory_file = opt['<trajectory_file>']
opt_force = opt['--force']
opt_no_confirm = opt['--no-confirm']
opt_xtion = opt['--xtion']
opt_output = opt['--output']
if opt_debug:
print opt
# Initialise node
rospy.init_node(NODE_NAME, anonymous=NODE_ANONYMOUS)
# Initialise robot
robot = ClopemaRobotCommander(ROBOT_GROUP)
# Initialise tf buffer
tfb = get_tf2_buffer()
# Load trajectories
trajs = read_msgs(opt_trajectory_file, RobotTrajectory)
# Infer what xtion we are calibrating
if opt_xtion is None:
xtion = get_xtion(infer_xtion(trajs))
else:
xtion = get_xtion(int(opt_xtion))
# Check output directory
if opt_output is not None:
output_path = opt_output
if not prepare_output_directory(output_path, opt_force):
rospy.logerr(
"Output directory is not empty, try adding -f if you want to overwrite it."
)
return
else:
output_path = prepare_unique_directory("%s_calib" % xtion.name)
# Initialise measurement
calib = CameraCalib(robot, tfb, xtion, output_path)
calib.confirm = not opt_no_confirm
# Run calibration capture
calib.run(trajs)
# Turn servo off
robot.set_servo_power_off()
def exec_cmd(filename):
with open(filename, "r") as f:
data = yaml.load_all(f)
s = next(data)
js = JointState()
genpy.message.fill_message_args(js, s)
robot = ClopemaRobotCommander("arms")
rs = robot.get_current_state()
if has_arms(rs, js):
robot = ClopemaRobotCommander("arms")
robot.set_joint_value_target(js)
traj = robot.plan()
if args['--speed'] is not None:
print "Speed ", "%+3.2f" % float(args['--speed'])
else:
print "Speed ", "%+3.2f" % robot.get_robot_speed()
if len(traj.joint_trajectory.points) <= 0:
print "No valid trajectory found please check the robot output."
else:
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.execute(traj)
if has_ext(rs, js):
robot = ClopemaRobotCommander("ext")
robot.set_joint_value_target(js)
traj = robot.plan()
if args['--speed'] is not None:
print "Speed ", "%+3.2f" % float(args['--speed'])
else:
print "Speed ", "%+3.2f" % robot.get_robot_speed()
if len(traj.joint_trajectory.points) <= 0:
print "No valid trajectory found please check the robot output."
else:
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.execute(traj)
if args['--sof']:
robot.set_servo_power_off(True)
print args
# Initialize node
rospy.init_node(_NODE_NAME, anonymous=True)
# Initialize robot
robot = ClopemaRobotCommander("arms")
trajs = read_msgs(args['<file_name>'], RobotTrajectory)
# Check speed
if args['--speed'] is not None:
print "Speed ", "%+3.2f" % float(args['--speed'])
else:
print "Speed ", "%+3.2f" % robot.get_robot_speed()
if not args['--force']:
r = ask("Execute trajectory? ", {'Y':'Yes','n':'No'})
else:
r = 'y'
if r == 'y':
if args['--speed'] is not None:
robot.set_robot_speed(float(args['--speed']))
for traj in trajs:
robot.execute(traj)
if not args['--no-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_robot import ClopemaRobotCommander
if __name__ == '__main__':
ClopemaRobotCommander.set_servo_power_off()