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