def ar_callback(msg):
global hel, ar, last_100
for trans in msg.transforms:
if trans.child_frame_id == HEL_TAG:
hel = trans.transform
elif trans.child_frame_id == AR_TAG:
ar = trans.transform
if hel is not None and ar is not None:
hel_m = tf_to_rbt(hel)
ar_m = tf_to_rbt(ar)
trans = np.dot(np.linalg.inv(hel_m), ar_m)
last_100.append(trans)
if len(last_100) == 100:
print(np.average(np.array(last_100), axis=0))
last_100 = []
def helm_callback(ts):
if not lkp:
return
helm_tf = ts.transform
copter_tf = lkp
copter_to_helmar = tf_to_rbt(copter_tf)
origin_to_helm = tf_to_rbt(helm_tf)
helmar_to_copter = np.linalg.inv(copter_to_helmar)
origin_to_helmar = np.dot(origin_to_helm, HELM_TO_HELMAR)
origin_to_copter = np.dot(origin_to_helmar, helmar_to_copter)
origin_to_copter_crazy = np.dot(VICON_TO_CRAZY, origin_to_copter)
copter_loc = rbt_to_pose(origin_to_copter_crazy)
pub.publish(copter_loc)
def helm_callback(ts):
helm_tf = ts.transform
origin_to_helm = tf_to_rbt(helm_tf)
origin_to_desired = np.dot(origin_to_helm, HELM_TO_DESIRED)
origin_to_desired_crazy = np.dot(VICON_TO_CRAZY, origin_to_desired)
desired_loc = rbt_to_pose(origin_to_desired_crazy)
print 'publishing'
pub.publish(desired_loc)