else: time_elapsed = time.clock() - prior_time + delay_time print 'Time Elapsed:' + str(time_elapsed) prior_time = time.clock() if(vel == 0): kalfilter.prediction_update(0,.4*ang_vel,time_elapsed,True) else: kalfilter.prediction_update(.55*vel,.45*ang_vel,time_elapsed,True) if(marker_dist != -1): #-1 means invalid measurements print 'Marker Dist:' + str(marker_dist) print 'Marker Angle:' + str(marker_angle) marker_x,marker_y = kalfilter.guess_marker(marker_dist,marker_angle) if(not colorseg_debug): x,y,theta = kalfilter.get_pose() kalfilter.measurement_update(marker_x,marker_y,marker_dist,marker_angle,second_dist,second_angle) x,y,theta = kalfilter.get_pose() #wrapper.packet = str((x,y)) #Obstacle avoidance stuff current_x = int(round((x-GRID_SIZE)/GRID_SIZE)) current_y = int(round((y-GRID_SIZE)/GRID_SIZE)) #wrapper.packet = str((x-GRID_SIZE)/(GRID_SIZE)) + ',' + str((y - GRID_SIZE)/(GRID_SIZE))+ ',' + str(time.clock()) + ',' + str(theta*(180/math.pi)) #print 'Android Coordinates:', wrapper.packet #get distances #print wrapper.ss, '!!!!!!!' #try: #print processQ.get(False), "!!!!!!!!!!!!" #except Queue.Empty, e:
