if __name__ == '__main__':
rospy.init_node("homography_class")
pospub = []
pospub.append(
rospy.Publisher('/pidrone/homo_est1', PoseStamped, queue_size=1))
pospub.append(
rospy.Publisher('/pidrone/homo_est2', PoseStamped, queue_size=1))
pospub.append(
rospy.Publisher('/pidrone/homo_est3', PoseStamped, queue_size=1))
pospub.append(
rospy.Publisher('/pidrone/homo_est4', PoseStamped, queue_size=1))
rospy.Subscriber("/pidrone/est_pos", PoseStamped, seed_pos)
homography = Homography()
prev_img = streamPi().next()
position_flag = False
position = None
first_img = None
global first_pos
print 'Waiting for first position from optitrak'
while first_pos is None:
time.sleep(0.001)
print "found first pos"
homography.est_RT = decompose_pose(first_pos)
start_RT = decompose_pose(first_pos)
print "decomposed"
# cv2.namedWindow('preview')
for curr_img in streamPi():
if first_img is None: first_img = curr_img
elif first_img is not None and not position_flag:
if __name__ == '__main__':
rospy.init_node('velocity_flight')
rospy.Subscriber("/pidrone/est_pos", PoseStamped, vrpn_update_pos)
rospy.Subscriber("/pidrone/ultrasonic", Range, ultra_callback)
homography = Homography()
pid = PID()
first = True
board = MultiWii("/dev/ttyACM0")
while vrpn_pos is None:
if not rospy.is_shutdown():
print "No VRPN :("
time.sleep(0.01)
else:
print "Shutdown Recieved"
sys.exit()
stream = streamPi()
try:
while not rospy.is_shutdown():
curr_img = cv2.cvtColor(np.array(stream.next()), cv2.COLOR_RGB2GRAY)
if first:
homography.update_H(curr_img, curr_img)
first = False
homography.set_z(vrpn_pos.pose.position.z)
board.arm()
else:
homography.set_z(ultra_z)
error = axes_err()
if homography.update_H(curr_img):
vel, z = homography.get_vel_and_z()
if np.abs(np.linalg.norm(vel)) < 2500:
print "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"
def world_to_pixel_map(pt, global_map_min, global_map_max, map_size):
global_map_range = global_map_max - global_map_min
pt_pixel_coord = ((pt - global_map_min) / global_map_range *
map_size).astype(int)
return pt_pixel_coord
if __name__ == '__main__':
global start_RT
rospy.init_node('homography_mapper')
rospy.Subscriber("/pidrone/est_pos", PoseStamped, vrpn_callback)
prev_img = None
init_R = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])
homography = Homography()
for curr_img in streamPi():
curr_img = np.array(curr_img)
curr_img = cv2.cvtColor(np.array(curr_img), cv2.COLOR_RGB2GRAY)
if prev_img is None:
print "first prev"
prev_img = deepcopy(curr_img)
else:
if start_RT is not None:
# run homography on a new image and integrate H
homography.updateH(curr_img, prev_img=prev_img)
homo_RTn = homography.get_pose_alt(start_RT)
homo_RT = np.identity(4)
if homo_RTn is not None:
homo_R, homo_T, homo_norm = homo_RTn
def main():
#stream()
streamPi()