def callback(data):
#run every time i see a TFMessage on tf_topic.
global tf_listener
global last_seen
global rbt
#header = data.header
#print(data.transforms[0])
#print(dir(data.transforms[0]))
tim = data.transforms[0]
tim = tim.header.stamp
#print(tim)
#print('fo')
if tf_listener.frameExists('ar_marker_63'):
now = rospy.Time.now()
#tf_listener.waitForTransform('ar_marker_63','left_gripper',now,rospy.Dure)
try:
(trans, rot) = tf_listener.lookupTransform('ar_marker_63', 'base',
tim)
(omega, theta) = eqf.quaternion_to_exp(rot)
rbt = eqf.return_rbt(trans, rot)
#print(rbt)
#print("")
#print("")
#print("Period was " + str(time.time() - last_seen))
if last_seen - time.time() > 10:
#print("updated rbt, i hadn't seen it in a while \n\r")
#i haven't seen the tag in a while
pass
last_seen = time.time()
#bring this down to 5hz, the camera rate. I don't wanna kill the processor on this...
time.sleep(0.2)
#time0 asks for the most recent one
except:
pass
def callback(data):
#run every time i see a TFMessage on tf_topic.
global tf_listener
global last_seen
global rbt
#header = data.header
#print(data.transforms[0])
#print(dir(data.transforms[0]))
tim = data.transforms[0]
tim = tim.header.stamp
#print(tim)
#print('fo')
if tf_listener.frameExists('ar_marker_63'):
now = rospy.Time.now()
#tf_listener.waitForTransform('ar_marker_63','left_gripper',now,rospy.Dure)
try:
(trans,rot) = tf_listener.lookupTransform('ar_marker_63','base',tim)
(omega,theta) = eqf.quaternion_to_exp(rot)
rbt = eqf.return_rbt(trans,rot)
#print(rbt)
#print("")
#print("")
#print("Period was " + str(time.time() - last_seen))
if last_seen - time.time() > 10:
#print("updated rbt, i hadn't seen it in a while \n\r")
#i haven't seen the tag in a while
pass
last_seen = time.time()
#bring this down to 5hz, the camera rate. I don't wanna kill the processor on this...
time.sleep(0.2)
#time0 asks for the most recent one
except:
pass
def movment_handle(data):
#run every time i get a service call
#print(data)
global gripper_status
#anything can be empty.
#if incrimental is not empty, assume that this is to be incrimental
if (len(data.trans) is not 3) and (len(data.trans) is not 0):
return("ERROR: trans is of the wrong length")
elif len(data.trans) is 3:
trans = data.trans
move = True
else:
move = True
#assume that i'm staying at the same location
loc = get_pose()
trans = loc['trans']
if data.grip == 'True':
gripper_status = False
#actuate_gripper(False)
#apparently, fase is actually what makes it suck. Weird.
elif data.grip == 'False':
gripper_status = True
#actuate_gripper(True)
else: #just let currnet state go on.
pass
if len(data.rot) is 0:
#assume that no rotation was intended... aka, keep the current rotation I want
pose = get_pose()
rot = pose['rot']
else:
rot = data.rot
if len(rot) is not 4:
return("Error. ROT should be a quaternion")
if len(data.incrimental) is 0:
incrimental = False
else:
incrimental = True
if len(data.target_rot) > 0:
target_rot = data.target_rot
target_trans = data.target_trans
target_rbt = eqf.return_rbt(target_trans,target_rot)
else:
target_rbt = np.eye(4)
if len(data.keep_orient) is 0:
keep_orient = False
else:
keep_orient = True
if len(data.change_height) is 0:
changeHeight = True
else:
changeHeight = False
print "I am currently at " +str(get_pose())
if move and incrimental:
print("Incrimental Move by " + str(trans))
#print("changeHeight?" + str(changeHeight))
incrimental_movement(trans,target_rbt,changeHeight = changeHeight, keep_oreint = keep_orient)
elif move:
print("Absolute Movement to " + str(trans) + "rot " + str(rot))
move_to_coord(trans,rot,keep_orient)
#if bool()
#construct a response. Currently, i return a string...1
resp = kinematics_requestResponse("I_did_stuff")
print("\n\r")
return resp
def closed_loop_pick(
which_arm,
arm,
destination='othello_piece',
):
#assume that i'm already close.
global tf_listener
global last_seen
limb = baxter_interface.Limb(which_arm)
while True:
pose = get_pose(limb)
execute = True
exit = raw_input(
"E to quit. K to start keyboard. D for dist to target. Otherwise, i'll move to 0 \n\r"
)
if (exit == 'e') or (exit == 'E'):
rospy.signal_shutdown("Shutdown signal recieved")
return
elif (exit == 'd') or (exit == 'D'):
execute = False
elif (exit == 'k') or (exit == 'K'):
keyboard_ctrl(which_arm, arm, left_gripper)
try:
#now = rospy.Time.now()
#tf_listener.waitForTransform('suction_cup','othello_piece',now,rospy.Duration(2.5))
#now = rospy.Time.now()
last_time = tf_listener.getLatestCommonTime(
'suction_cup', 'othello_piece')
(trans, rot) = tf_listener.lookupTransform('suction_cup',
'othello_piece',
last_time)
print("Saw the othello piece " +
str(last_time.to_sec() - rospy.Time.now().to_sec()) +
" secs ago \n\r")
#get the RBT from the suction_cup to the base so incrimental movement can deal with it.
last_time = tf_listener.getLatestCommonTime('suction_cup', 'base')
(trans_base,
rot_base) = tf_listener.lookupTransform('suction_cup', 'base',
last_time)
rbt_to_base = eqf.return_rbt(trans_base, rot_base)
#print("time diff")
#if time.time() - last_seen > 3:
# print("Warning. lag is over 3 seconds")
#last_seen = time.time()
except KeyboardInterrupt:
sys.exit()
except:
print("Could not find trans / rot \n\r")
traceback.print_exc()
else:
print("Error between suction cup and the othello piece is \n\r")
print(trans)
print("")
err_dist = np.linalg.norm(trans[:2]) #error in meters from
print("XY ERROR IS " + str(err_dist))
if execute:
if trans[2] > 0.05:
#i'm very high away, just get closer...
trans_goal = (.85 * trans[0], .85 * trans[1], trans[2] / 2)
change_height = True
keep_oreint = False
elif err_dist > 0.01: #1cm, 1 inch.
#final lateral alignment
trans_goal = (
trans[0], trans[1], 0
) #i want to move the suction cupt to 0,0,whatever height i'm at right now of hte othello piece.
change_height = False
keep_oreint = True
else:
#just go down % hit it
trans_goal = (.5 * trans[0], .5 * trans[1], trans[2])
change_height = True
keep_oreint = True
print("I want to move by, in the suction cup frame, \n\r")
print(str(trans_goal) + "\n\r")
incrimental_movement(trans_goal[0],
trans_goal[1],
trans_goal[2],
arm,
which_arm,
rbt=rbt_to_base,
changeHeight=change_height,
keep_oreint=keep_oreint)
def closed_loop_pick(which_arm,arm,destination='othello_piece',):
#assume that i'm already close.
global tf_listener
global last_seen
limb = baxter_interface.Limb(which_arm)
while True:
pose = get_pose(limb)
execute = True
exit = raw_input("E to quit. K to start keyboard. D for dist to target. Otherwise, i'll move to 0 \n\r")
if (exit == 'e') or (exit == 'E'):
rospy.signal_shutdown("Shutdown signal recieved")
return
elif(exit == 'd') or (exit == 'D'):
execute = False
elif(exit == 'k') or (exit == 'K'):
keyboard_ctrl(which_arm,arm,left_gripper)
try:
#now = rospy.Time.now()
#tf_listener.waitForTransform('suction_cup','othello_piece',now,rospy.Duration(2.5))
#now = rospy.Time.now()
last_time = tf_listener.getLatestCommonTime('suction_cup','othello_piece')
(trans,rot) = tf_listener.lookupTransform('suction_cup','othello_piece',last_time)
print("Saw the othello piece " + str(last_time.to_sec() - rospy.Time.now().to_sec()) + " secs ago \n\r")
#get the RBT from the suction_cup to the base so incrimental movement can deal with it.
last_time = tf_listener.getLatestCommonTime('suction_cup','base')
(trans_base,rot_base) = tf_listener.lookupTransform('suction_cup','base',last_time)
rbt_to_base = eqf.return_rbt(trans_base,rot_base)
#print("time diff")
#if time.time() - last_seen > 3:
# print("Warning. lag is over 3 seconds")
#last_seen = time.time()
except KeyboardInterrupt:
sys.exit()
except:
print("Could not find trans / rot \n\r")
traceback.print_exc()
else:
print("Error between suction cup and the othello piece is \n\r")
print(trans)
print("")
err_dist = np.linalg.norm(trans[:2]) #error in meters from
print("XY ERROR IS " +str(err_dist))
if execute:
if trans[2] > 0.05:
#i'm very high away, just get closer...
trans_goal = (.85*trans[0],.85*trans[1],trans[2]/2)
change_height = True
keep_oreint = False
elif err_dist > 0.01: #1cm, 1 inch.
#final lateral alignment
trans_goal = (trans[0],trans[1],0) #i want to move the suction cupt to 0,0,whatever height i'm at right now of hte othello piece.
change_height = False
keep_oreint = True
else:
#just go down % hit it
trans_goal = (.5*trans[0],.5*trans[1],trans[2])
change_height = True
keep_oreint = True
print("I want to move by, in the suction cup frame, \n\r")
print(str(trans_goal) + "\n\r")
incrimental_movement(trans_goal[0],trans_goal[1],trans_goal[2], arm, which_arm,rbt = rbt_to_base,changeHeight = change_height,keep_oreint=keep_oreint)
def movment_handle(data):
#run every time i get a service call
#print(data)
global gripper_status
#anything can be empty.
#if incrimental is not empty, assume that this is to be incrimental
if (len(data.trans) is not 3) and (len(data.trans) is not 0):
return ("ERROR: trans is of the wrong length")
elif len(data.trans) is 3:
trans = data.trans
move = True
else:
move = True
#assume that i'm staying at the same location
loc = get_pose()
trans = loc['trans']
if data.grip == 'True':
gripper_status = False
#actuate_gripper(False)
#apparently, fase is actually what makes it suck. Weird.
elif data.grip == 'False':
gripper_status = True
#actuate_gripper(True)
else: #just let currnet state go on.
pass
if len(data.rot) is 0:
#assume that no rotation was intended... aka, keep the current rotation I want
pose = get_pose()
rot = pose['rot']
else:
rot = data.rot
if len(rot) is not 4:
return ("Error. ROT should be a quaternion")
if len(data.incrimental) is 0:
incrimental = False
else:
incrimental = True
if len(data.target_rot) > 0:
target_rot = data.target_rot
target_trans = data.target_trans
target_rbt = eqf.return_rbt(target_trans, target_rot)
else:
target_rbt = np.eye(4)
if len(data.keep_orient) is 0:
keep_orient = False
else:
keep_orient = True
if len(data.change_height) is 0:
changeHeight = True
else:
changeHeight = False
print "I am currently at " + str(get_pose())
if move and incrimental:
print("Incrimental Move by " + str(trans))
#print("changeHeight?" + str(changeHeight))
incrimental_movement(trans,
target_rbt,
changeHeight=changeHeight,
keep_oreint=keep_orient)
elif move:
print("Absolute Movement to " + str(trans) + "rot " + str(rot))
move_to_coord(trans, rot, keep_orient)
#if bool()
#construct a response. Currently, i return a string...1
resp = kinematics_requestResponse("I_did_stuff")
print("\n\r")
return resp
