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 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(_, 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)