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)