예제 #1
0
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)
예제 #2
0
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)
예제 #4
0
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)
예제 #5
0
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)
예제 #6
0
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)
예제 #7
0
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)
예제 #9
0
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)