def telemetry(sid, data): global frame_counter, second_counter, fps frame_counter += 1 # Do a rough calculation of frames per second (FPS) if (time.time() - second_counter) > 1: fps = frame_counter frame_counter = 0 second_counter = time.time() # print("Current FPS: {}".format(fps)) if data: global Rover # Initialize / update Rover with current telemetry Rover, image = update_rover(Rover, data) if np.isfinite(Rover.vel): # Execute the perception and decision steps to update the Rover's state Rover = perception_step(Rover) Rover = decision_step(Rover) # Create output images to send to server out_image_string1, out_image_string2 = create_output_images(Rover) # The action step! Send commands to the rover! # Don't send both of these, they both trigger the simulator # to send back new telemetry so we must only send one # back in respose to the current telemetry data. # If in a state where want to pickup a rock send pickup command if Rover.send_pickup and not Rover.picking_up: send_pickup() # Reset Rover flags Rover.send_pickup = False else: # Send commands to the rover! commands = (Rover.throttle, Rover.brake, Rover.steer) send_control(commands, out_image_string1, out_image_string2) # In case of invalid telemetry, send null commands else: # Send zeros for throttle, brake and steer and empty images send_control((0, 0, 0), '', '') # If you want to save camera images from autonomous driving specify a path # Example: $ python drive_rover.py image_folder_path # Conditional to save image frame if folder was specified # image_folder = ../ if args.image_folder != '': timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3] image_filename = os.path.join(args.image_folder, timestamp) image.save('{}.jpg'.format(image_filename)) else: sio.emit('manual', data={}, skip_sid=True)
def telemetry(sid, data): global frame_counter, second_counter, fps frame_counter += 1 if (time.time() - second_counter) > 1: fps = frame_counter frame_counter = 0 second_counter = time.time() if data: global Rover Rover, image = update_rover(Rover, data) if np.isfinite(Rover.vel): Rover = perception_step(Rover) Rover = decision_step(Rover) out_image_string1, out_image_string2 = create_output_images(Rover) if Rover.send_pickup and not Rover.picking_up: send_pickup() Rover.send_pickup = False else: commands = (Rover.throttle, Rover.brake, Rover.steer) send_control(commands, out_image_string1, out_image_string2) else: send_control((0, 0, 0), '', '') if args.image_folder != '': timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3] image_filename = os.path.join(args.image_folder, timestamp) image.save('{}.jpg'.format(image_filename)) else: sio.emit('manual', data={}, skip_sid=True)
def telemetry(sid, data): global Rover, frame_counter, second_counter, fps, last_x, last_y frame_counter += 1 # Do a rough calculation of frames per second (FPS) if (time.time() - second_counter) > 1: fps = frame_counter frame_counter = 0 second_counter = time.time() Rover.tot_sec += 1 Rover.current_time += 1 # Calculate the distance traveled every 5 seconds if (time.time() - Rover.dist_sec_cnt) > 5: Rover.dist = np.sqrt((last_x - Rover.pos[0])**2 + (last_y - Rover.pos[1])**2) last_x, last_y = Rover.pos Rover.dist_sec_cnt = time.time() print(Rover.dist) #print("Current FPS: {}".format(fps)) if data: # Initialize / update Rover with current telemetry Rover, image = update_rover(Rover, data) if np.isfinite(Rover.vel): # Execute the perception and decision steps to update the Rover's state Rover = perception_step(Rover) Rover = decision_step(Rover) # Create output images to send to server out_image_string1, out_image_string2 = create_output_images(Rover) # The action step! Send commands to the rover! commands = (Rover.throttle, Rover.brake, Rover.steer) send_control(commands, out_image_string1, out_image_string2) # If in a state where want to pickup a rock send pickup command if Rover.send_pickup and not Rover.picking_up: send_pickup() # Reset Rover flags Rover.send_pickup = False # In case of invalid telemetry, send null commands else: # Send zeros for throttle, brake and steer and empty images send_control((0, 0, 0), '', '') # If you want to save camera images from autonomous driving specify a path # Example: $ python drive_rover.py image_folder_path # Conditional to save image frame if folder was specified if args.image_folder != '': timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3] image_filename = os.path.join(args.image_folder, timestamp) image.save('{}.jpg'.format(image_filename)) else: sio.emit('manual', data={}, skip_sid=True)
def telemetry(sid, data): global frame_counter, second_counter, fps frame_counter+=1 # Do a rough calculation of frames per second (FPS) if (time.time() - second_counter) > 1: fps = frame_counter frame_counter = 0 second_counter = time.time() print("Current FPS: {}".format(fps)) if data: global Rover # Initialize / update Rover with current telemetry Rover, image = update_rover(Rover, data) if np.isfinite(Rover.vel): # Execute the perception and decision steps to update the Rover's state Rover = perception_step(Rover) Rover = decision_step(Rover) # Create output images to send to server out_image_string1, out_image_string2 = create_output_images(Rover) # The action step! Send commands to the rover! # Don't send both of these, they both trigger the simulator # to send back new telemetry so we must only send one # back in respose to the current telemetry data. # If in a state where want to pickup a rock send pickup command if Rover.send_pickup and not Rover.picking_up: send_pickup() # Reset Rover flags Rover.send_pickup = False else: # Send commands to the rover! commands = (Rover.throttle, Rover.brake, Rover.steer) send_control(commands, out_image_string1, out_image_string2) # In case of invalid telemetry, send null commands else: # Send zeros for throttle, brake and steer and empty images send_control((0, 0, 0), '', '') # If you want to save camera images from autonomous driving specify a path # Example: $ python drive_rover.py image_folder_path # Conditional to save image frame if folder was specified if args.image_folder != '': timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3] image_filename = os.path.join(args.image_folder, timestamp) image.save('{}.jpg'.format(image_filename)) else: sio.emit('manual', data={}, skip_sid=True)
def test_perception(): image_name = '../calibration_images/example_rock1.jpg' image = mpimg.imread(image_name) rover = dr.RoverState() rover.img = image rover.yaw = 0.0 rover.pos = (99.66999, 85.58897) perception.perception_step(rover) # Display the original image and updated world f, axes = plt.subplots(2, 2, figsize=(21, 7), sharey=True) f.tight_layout() axes[0, 0].imshow(image) axes[0, 0].set_title('Original Image', fontsize=40) axes[0, 1].imshow(rover.vision_image) axes[0, 1].set_title('Your Result', fontsize=40) axes[1, 0].imshow(rover.worldmap) axes[1, 0].set_title('WorldMap', fontsize=40) plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.) plt.show() # Uncomment if running on your local machine
def telemetry(sid, data): if data: global Rover # Initialize / update Rover with current telemetry Rover, image = update_rover(Rover, data) if np.isfinite(Rover.vel): # Execute the perception and decision steps to update the Rover's state Rover = perception_step(Rover) Rover = decision_step(Rover) # Create output images to send to server out_image_string1, out_image_string2 = create_output_images(Rover) # The action step! Send commands to the rover! commands = (Rover.throttle, Rover.brake, Rover.steer) send_control(commands, out_image_string1, out_image_string2) # If in a state where want to pickup a rock send pickup command if Rover.pick_up: send_pickup() # Reset Rover flags Rover.pick_up = False # In case of invalid telemetry, send null commands else: # Send zeros for throttle, brake and steer and empty images send_control((0, 0, 0), '', '') # If you want to save camera images from autonomous driving specify a path # Example: $ python drive_rover.py image_folder_path # Conditional to save image frame if folder was specified if args.image_folder != '': timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3] image_filename = os.path.join(args.image_folder, timestamp) image.save('{}.jpg'.format(image_filename)) else: sio.emit('manual', data={}, skip_sid=True)
def telemetry(sid, data): global frame_counter, second_counter, fps frame_counter+=1 # Do a rough calculation of frames per second (FPS) if (time.time() - second_counter) > 1: fps = frame_counter frame_counter = 0 second_counter = time.time() print("Current FPS: {}".format(fps)) if data: global Rover # Initialize / update Rover with current telemetry Rover, image = update_rover(Rover, data) if np.isfinite(Rover.vel): # Execute the perception and decision steps to update the Rover's state #when a rock is found execute perception and rock_manuever picking up the rock if Rover.samples_located > Rover.samples_collected: Rover = perception_step(Rover) Rover = rock_manuever(Rover) if Rover.vel == 0: Rover = rover_stuck(Rover) if Rover.samples_located == Rover.samples_collected and Rover.reverse == 'False': #if Rover.reverse flag is false and no rocks to pickup operate normally Rover.manuever_flag == 'False' Rover.mode = 'forward' Rover = perception_step(Rover) Rover = decision_step(Rover) #flag that is true when rover is stuck and needs to reverse straight back if Rover.reverse == 'True': Rover.steer = 0 Rover.brake = 0 Rover.throttle = -1 #flag that is set to false when the rover velocity reaches -1m/s if Rover.reverse == 'True' and Rover.vel < -1: Rover.reverse = 'False' Rover.steer = 0 #Flag that is true when the rover needs to perform a 180 degree turn if Rover.turn180 == 'True'and Rover.manuever_flag == 'False': Rover.steer = 15 Rover.throttle = 0 Rover.brake = 0 #Set the turn180 flag back to false when there is navigable terrain in front of Rover if len(Rover.nav_angles) > Rover.stop_forward: Rover.turn180 = 'False' if Rover.vel<0: Rover.steer = 0 # Create output images to send to server out_image_string1, out_image_string2 = create_output_images(Rover) # The action step! Send commands to the rover! # Don't send both of these, they both trigger the simulator # to send back new telemetry so we must only send one # back in respose to the current telemetry data. # If in a state where want to pickup a rock send pickup command print('##rover pickup status##') print(Rover.send_pickup) if Rover.send_pickup and not Rover.picking_up: print('##met conditions to send pickup##') print(Rover.send_pickup) send_pickup() print('##sent the pickup command##') else: # Reset Rover flags # Send commands to the rover! Rover.send_pickup = False commands = (Rover.throttle, Rover.brake, Rover.steer) send_control(commands, out_image_string1, out_image_string2) # In case of invalid telemetry, send null commands else: # Send zeros for throttle, brake and steer and empty images send_control((0, 0, 0), '', '') # If you want to save camera images from autonomous driving specify a path # Example: $ python drive_rover.py image_folder_path # Conditional to save image frame if folder was specified if args.image_folder != '': timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3] image_filename = os.path.join(args.image_folder, timestamp) image.save('{}.jpg'.format(image_filename)) else: sio.emit('manual', data={}, skip_sid=True)
def telemetry(sid, data): global frame_counter, second_counter, fps frame_counter+=1 # Do a rough calculation of frames per second (FPS) if (time.time() - second_counter) > 1: fps = frame_counter frame_counter = 0 second_counter = time.time() print("Current FPS: {}".format(fps)) if data: global Rover # Initialize / update Rover with current telemetry Rover, image = update_rover(Rover, data) if Rover.start_pos is None: Rover.start_pos = Rover.pos Rover.stuck_time = Rover.total_time if np.isfinite(Rover.vel): # Execute the perception and decision steps to update the Rover's state Rover = perception_step(Rover) Rover = decision_step(Rover) # Visualize complementary information in the simulator - speed, mode and FPS if Rover.picking_up: mode = 'picking up' else: mode = Rover.mode #cv2.putText(Rover.vision_image, "navA{}".format(Rover.nav_angles), (2, Rover.vision_image.shape[0] - 104), # cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA) #cv2.putText(Rover.vision_image, "steer: {}".format(np.clip(np.mean(Rover.nav_angles * 180/np.pi), -15, 15)), (2, Rover.vision_image.shape[0] - 84), # cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA) #cv2.putText(Rover.vision_image, "total_time: {}".format(Rover.total_time), (2, Rover.vision_image.shape[0] - 64), # cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA) #cv2.putText(Rover.vision_image, "stuck_time: {}".format(Rover.stuck_time), (2, Rover.vision_image.shape[0] - 44), # cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA) #cv2.putText(Rover.vision_image, "Speed: {}".format(round(Rover.vel, 3)), (2, Rover.vision_image.shape[0] - 24), # cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA) #cv2.putText(Rover.vision_image, "Mode: {} Current FPS: {}".format(mode, fps), (2, Rover.vision_image.shape[0] - 5), cv2.putText(Rover.vision_image, "Mode: {}".format(mode), (2, Rover.vision_image.shape[0] - 24), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA) cv2.putText(Rover.vision_image, "Current FPS: {}".format(fps), (2, Rover.vision_image.shape[0] - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA) # Create output images to send to server out_image_string1, out_image_string2 = create_output_images(Rover) # The action step! Send commands to the rover! # Don't send both of these, they both trigger the simulator # to send back new telemetry so we must only send one # back in response to the current telemetry data. # If in a state where want to pickup a rock send pickup command if Rover.send_pickup and not Rover.picking_up: send_pickup() # Reset Rover flags Rover.send_pickup = False else: # Send commands to the rover! commands = (Rover.throttle, Rover.brake, Rover.steer) send_control(commands, out_image_string1, out_image_string2) # In case of invalid telemetry, send null commands else: # Send zeros for throttle, brake and steer and empty images send_control((0, 0, 0), '', '') # If you want to save camera images from autonomous driving specify a path # Example: $ python drive_rover.py image_folder_path # Conditional to save image frame if folder was specified if args.image_folder != '': timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3] image_filename = os.path.join(args.image_folder, timestamp) image.save('{}.jpg'.format(image_filename)) else: sio.emit('manual', data={}, skip_sid=True)
def telemetry(sid, data): global frame_counter, second_counter, fps frame_counter+=1 # Do a rough calculation of frames per second (FPS) if (time.time() - second_counter) > 1: fps = frame_counter frame_counter = 0 second_counter = time.time() # recalculate if FPS differ too much from the used FPS if (Rover.used_fps is not None) and (fps is not None): if (fps > 10) and (abs(Rover.used_fps - fps) > 5): Rover.used_fps = fps Rover.unstuck_target_yaw = Rover.unstuck_target_yaw_sec * fps Rover.unstuck_count_target = Rover.unstuck_count_target_sec * fps Rover.unstuck_getout_count_target = Rover.unstuck_getout_count_target_sec * fps Rover.unstuck_awhile_count_target = Rover.unstuck_awhile_count_target_sec * fps Rover.unstuck_torock_count_target = Rover.unstuck_torock_count_target_sec * fps print("Current FPS: {}".format(fps), 'Unstuck count = ', Rover.unstuck_count_target) if data: global Rover # Initialize / update Rover with current telemetry Rover, image = update_rover(Rover, data) if np.isfinite(Rover.vel): # Execute the perception and decision steps to update the Rover's state Rover = perception_step(Rover) Rover = decision_step(Rover) # Create output images to send to server out_image_string1, out_image_string2 = create_output_images(Rover) # The action step! Send commands to the rover! commands = (Rover.throttle, Rover.brake, Rover.steer) send_control(commands, out_image_string1, out_image_string2) # If in a state where want to pickup a rock send pickup command if Rover.send_pickup: send_pickup() # Reset Rover flags Rover.send_pickup = False # In case of invalid telemetry, send null commands else: # Send zeros for throttle, brake and steer and empty images send_control((0, 0, 0), '', '') # If you want to save camera images from autonomous driving specify a path # Example: $ python drive_rover.py image_folder_path # Conditional to save image frame if folder was specified if args.image_folder != '': timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3] image_filename = os.path.join(args.image_folder, timestamp) image.save('{}.jpg'.format(image_filename)) else: sio.emit('manual', data={}, skip_sid=True)
def telemetry(sid, data): if data: global Rover # Initialize start time and sample positions if Rover.start_time == None: Rover.start_time = time.time() Rover.total_time = 0 samples_xpos = np.int_([ np.float(pos.strip()) for pos in data["samples_x"].split(',') ]) samples_ypos = np.int_([ np.float(pos.strip()) for pos in data["samples_y"].split(',') ]) Rover.samples_pos = (samples_xpos, samples_ypos) Rover.samples_found = np.zeros( (len(Rover.samples_pos[0]))).astype(np.int) # Or just update elapsed time else: tot_time = time.time() - Rover.start_time if np.isfinite(tot_time): Rover.total_time = tot_time # Print out the fields in the telemetry data dictionary #####print(data.keys()) # The current speed of the rover in m/s Rover.vel = np.float(data["speed"]) # The current position of the rover Rover.pos = np.fromstring(data["position"], dtype=float, sep=',') # The current yaw angle of the rover Rover.yaw = np.float(data["yaw"]) # The current yaw angle of the rover Rover.pitch = np.float(data["pitch"]) # The current yaw angle of the rover Rover.roll = np.float(data["roll"]) # The current throttle setting Rover.throttle = np.float(data["throttle"]) # The current steering angle Rover.steer = np.float(data["steering_angle"]) print('speed =', Rover.vel, 'position =', Rover.pos, 'throttle =', Rover.throttle, 'steer_angle =', Rover.steer) # Get the current image from the center camera of the rover imgString = data["image"] image = Image.open(BytesIO(base64.b64decode(imgString))) Rover.img = np.asarray(image) if np.isfinite(Rover.vel): # Execute the perception and decision steps to update the Rover's state perception_step(Rover) print('chamada') decision_step(Rover) # Create output images to send to server out_image_string1, out_image_string2 = create_output_images(Rover) # The action step! Send commands to the rover! commands = (Rover.throttle, Rover.brake, Rover.steer) #print(commands) send_control(commands, out_image_string1, out_image_string2) else: # Send zeros for throttle, brake and steer and empty images # if we got a bad velocity value in telemetry send_control((0, 0, 0), '', '') # Save image frame if folder was specified if args.image_folder != '': timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3] image_filename = os.path.join(args.image_folder, timestamp) image.save('{}.jpg'.format(image_filename)) else: sio.emit('manual', data={}, skip_sid=True)
def telemetry(_, data): """Defines telemetry function for what to do with incoming data""" # pylint: disable=global-statement global FRAME_COUNTER, SECOND_COUNTER, FPS FRAME_COUNTER += 1 # Do a rough calculation of frames per second (FPS) if (time.time() - SECOND_COUNTER) > 1: FPS = FRAME_COUNTER FRAME_COUNTER = 0 SECOND_COUNTER = time.time() #print("Current FPS: {}".format(FPS)) if data: global ROVER # Initialize / update rover with current telemetry ROVER, image = update_rover(ROVER, data) if np.isfinite(ROVER.perception.vel): # Execute the perception and decision steps to update the rover's # state ROVER = perception_step(ROVER) ROVER = decision_step(ROVER) # Create output images to send to server out_image_string1, out_image_string2 = create_output_images(ROVER) # The action step! Send commands to the ROVER! # Don't send both of these, they both trigger the simulator # to send back new telemetry so we must only send one # back in respose to the current telemetry data. # If in a state where want to pickup a rock send pickup command if ROVER.control.send_pickup and not ROVER.control.picking_up: send_pickup() # Reset ROVER flags ROVER.control.send_pickup = False else: # Send commands to the ROVER! commands = (ROVER.control.throttle, ROVER.control.brake, ROVER.control.steer) send_control(commands, out_image_string1, out_image_string2) # In case of invalid telemetry, send null commands else: # Send zeros for throttle, brake and steer and empty images send_control((0, 0, 0), '', '') # To save camera images from autonomous driving, specify a path # Example: $ python drive_rover.py image_folder_path # Conditional to save image frame if folder was specified global IMAGE_FOLDER if IMAGE_FOLDER != '': timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3] image_filename = os.path.join(IMAGE_FOLDER, timestamp) image.save('{}.jpg'.format(image_filename)) else: SIO.emit('manual', data={}, skip_sid=True)
def robot_perception_thread_function(self): print "robot_perception_thread_function() starts.\n" self.camera = PiCamera() self.camera.resolution = (img_length, img_width)#(640, 480) self.camera.framerate = 32 self.rawCapture = PiRGBArray(self.camera, size=(img_length, img_width))#(640, 480) time.sleep(0.1) source = np.float32([[5, 117], [156 ,119],[117, 86], [44, 84]]) self.camera.capture('calibration_template.jpg') image = mpimg.imread('./calibration_template.jpg') destination = np.float32([[image.shape[1]/2-dst_size, image.shape[0]-bottom_offset], [image.shape[1]/2+dst_size, image.shape[0]-bottom_offset], [image.shape[1]/2+dst_size, image.shape[0]-2*dst_size-bottom_offset], [image.shape[1]/2-dst_size, image.shape[0]-2*dst_size-bottom_offset]]) self.M = cv2.getPerspectiveTransform(source, destination) self.dst_size = dst_size cv2.namedWindow("window",1) #cv2.namedWindow("map",1) cv2.namedWindow("view img",1) cv2.namedWindow("view threshold",1) for frame in self.camera.capture_continuous(self.rawCapture, format="bgr", use_video_port=True): # grab the raw NumPy array representing the image, then initialize the timestamp # and occupied/unoccupied text image = frame.array self.img = image self.botview = perception_step(self) #cv2.imshow('window', self.img) output_loc = "x:%d, y:%d, theta:%d " % (self.x, self.y, self.theta) cv2.putText(self.worldmap,output_loc, (10,20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,0)) cv2.imshow('view img', self.img) cv2.imshow('window', self.vision_image) #cv2.imshow('map', self.worldmap) cv2.imshow('view threshold', self.img_view_threshold) # show the frame #cv2.imshow("Frame", image) # fig = plt.figure(figsize=(12,9)) # plt.subplot(221) # plt.imshow(image) # plt.subplot(222) # plt.imshow(self.botview.vision_image) # plt.subplot(223) # plt.imshow(self.botview.worldmap) # plt.subplot(224) # plt.plot(self.botview.x, self.botview.y, '.') key = cv2.waitKey(1) & 0xFF # clear the stream in preparation for the next frame self.rawCapture.truncate(0) # update path information and publish self.bot_dir2path() self.bot_publish() # if the `q` key was pressed, break from the loop if key == ord("q"): self.function_sanity_flag = False break cv2.destroyAllWindows() print "robot_perception_thread_function() end.\n" return
# Define a function to send the "pickup" command def send_pickup(): print("Picking up") pickup = {} sio.emit( "pickup", pickup, skip_sid=True) eventlet.sleep(0) if __name__ == '__main__': # Initialize our rover Rover = RoverState() perception.perception_step(Rover) print("here") decision_step(Rover) print("after Decision") parser = argparse.ArgumentParser(description='Remote Driving') parser.add_argument( 'image_folder', type=str, nargs='?', default='', help='Path to image folder. This is where the images from the run will be saved.' ) args = parser.parse_args() # os.system('rm -rf IMG_stream/*') if args.image_folder != '':