Rover Search and Sampling Return
Project Info
This project is modeled after the NASA sample return challenge and it will give you first hand experience with the three essential elements of robotics, which are perception, decision making and actuation. You will carry out this project in a simulator environment built with the Unity game engine.
- DATE: 2017
- SKILLS: PYTHON / COMPUTER VISION / OPENCV
- LINK: GITHUB
The goals / steps of this project are the following:
Training / Calibration
• Download the simulator and took data in “Training Mode”
• Test out the functions in the Jupyter Notebook provided
• Add functions to detect obstacles and samples of interest (golden rocks)
• Fill in the process_image() function with the image processing steps
(perspective transform, color threshold etc.) To get from raw images to a map.
The output_image you create in this step should demonstrate that your mapping
pipeline works.
• Use moviepy to process the images in your saved dataset with
the process_image() function. Include the video you produce as part of your
submission.
Notebook Analysis
Run the functions provided in the notebook on test images (first with the test data provided, next in data you have recorded). Add/modify functions to allow for color selection of obstacles and rock samples. To find the rock samples, I created color_threshed for navigation, obstacle_threshed for
detection of obstacle and rocks_threshed for identifying the rock sample.
Populate the process_image() function with the analysis steps to map pixels identifying navigable terrain, obstacles and rock samples into a WorldMap. Run process_image() on your test data using the moviepy functions provided to create a video output of your result. After several tested with the code, I’ve got an average result of mapping around 70% and 44% of fidelity. This is the video.
Autonomous Navigation and Mapping
I wrapped the rover visions and applying some threshold to identify the navigation, obstacles and the rocks. For navigations, I’ve used the polar coordinates to find the mean of the angles that’s for navigating the best angle that the rover should go for and I’ve applied the same technique for the rover to navigate to the rocks once the rocks are tracked.
# Apply the above functions in succession and update the Rover state accordingly
def perception_step(Rover):
# Example of how to use the Databucket() object defined above
# to print the current x, y and yaw values
# print(data.xpos[data.count], data.ypos[data.count], data.yaw[data.count])
img = Rover.img
dst_size = 5
bottom_offset = 6
# 1) Define source and destination points for perspective transform
source = np.float32([[14, 140], [301,140], [200, 96], [118, 96]])
destination = np.float32([[img.shape[1]/2 - dst_size, img.shape[0] - bottom_offset],
[img.shape[1]/2 + dst_size, img.shape[0] - bottom_offset],
[img.shape[1]/2 + dst_size, img.shape[0] - 2*dst_size - bottom_offset],
[img.shape[1]/2 - dst_size, img.shape[0] - 2*dst_size - bottom_offset],
])
# 2) Apply perspective transform
warped = perspect_transform(img, source, destination)
# 3) Apply color threshold to identify navigable terrain/obstacles/rock samples
color_threshed = color_thresh(warped)
obstacle_threshed = obstacle_thresh(warped)
rocks_threshed = rocks_thresh(warped)
rocks_threshed = threshold_dilated(rocks_threshed, 5)
Rover.vision_image[:, :, 2] = color_threshed * 255
Rover.vision_image[:, :, 0] = obstacle_threshed * 255
# 4) Convert thresholded image pixel values to rover-centric coords
nav_xpix, nav_ypix = rover_coords(color_threshed)
obs_xpix, obs_ypix = rover_coords(obstacle_threshed)
# 5) Convert rover-centric pixel values to world coords
dist, angles = to_polar_coords(nav_xpix, nav_ypix)
mean_dir = np.mean(angles)
xpos = Rover.pos[0]
ypos = Rover.pos[1]
yaw = Rover.yaw
world_shape = Rover.worldmap.shape[0]
scale = 2 * dst_size
# 6) Update worldmap (to be displayed on right side of screen)
nav_x_world, nav_y_world = pix_to_world(nav_xpix, nav_ypix, xpos, ypos, yaw, world_shape, scale)
obs_x, obs_y = pix_to_world(obs_xpix, obs_ypix, xpos, ypos, yaw, world_shape, scale)
Rover.worldmap[nav_y_world, nav_x_world, 2] += 10
Rover.worldmap[obs_y, obs_x, 0] += 1
Rover.nav_angles = angles
Rover.nav_dists = dist
Rover.rock_nav_angles = None
Rover.rock_nav_dists = None
if rocks_threshed.any():
rock_xpix, rock_ypix = rover_coords(rocks_threshed)
rock_x, rock_y = pix_to_world(rock_xpix, rock_ypix, xpos, ypos, yaw, world_shape, scale)
rock_dist, rock_ang = to_polar_coords(rock_x, rock_y)
rock_dist2, rock_ang2 = to_polar_coords(rock_xpix, rock_ypix)
rock_idx = np.argmin(rock_dist)
if not isinstance(rock_x, np.ndarray):
rock_x = [rock_x]
rock_y = [rock_y]
rock_xcen = rock_x[rock_idx]
rock_ycen = rock_y[rock_idx]
Rover.worldmap[rock_ycen, rock_xcen, :] = 255
Rover.vision_image[:, :, 1] = rocks_threshed * 255
Rover.rock_found = True
Rover.rock_nav_angles = rock_ang2
Rover.rock_nav_dists = rock_dist2
else:
Rover.rock_found = False
Rover.vision_image[:, :, 1] = 0
return RoverIn decision step, I’ve 3 difference mode and there is stuck, forward and stop. In generally, the car will go forward mode and steer the direction for rovering the map. Once the rover detected the rocks, it will steer to the rocks angles. The rover will change to the stop mode once it’s don’t detect any navigations direction or it’s will change to stuck mode for unstuck the rover.
# This is where you can build a decision tree for determining throttle, brake and steer
# commands based on the output of the perception_step() function
def decision_step(Rover):
# Implement conditionals to decide what to do given perception data
# Here you're all set up with some basic functionality but you'll need to
# improve on this decision tree to do a good job of navigating autonomously!
# Example:
# Check if we have vision data to make decisions with
if Rover.nav_angles is not None:
# Check for Rover.mode status
if Rover.mode == 'stuck':
flip_coin = random.randint(0, 1)
Rover.throttle = 0
Rover.brake = 0
if flip_coin == 0:
if np.clip(np.mean(Rover.nav_angles * 180/np.pi), -15, 15) > 0:
Rover.steer = 15
else:
Rover.steer = -15
else:
Rover.throttle = 1.0
Rover.unstuck_turningfrequency = 0
Rover.mode = 'forward'
elif Rover.mode == 'forward':
# Check the extent of navigable terrain
if Rover.near_sample:
Rover.brake = Rover.brake_set
Rover.throttle = 0
Rover.steer = 0
Rover.mode = 'stop'
elif len(Rover.nav_angles) >= Rover.stop_forward:
# If mode is forward, navigable terrain looks good
# and velocity is below max, then throttle
if Rover.stuck is True:
Rover.brake = 0
Rover.throttle = 0
Rover.mode = 'stuck'
elif Rover.vel < Rover.max_vel:
# Set throttle value to throttle setting
Rover.throttle = Rover.throttle_set
else: # Else coast
Rover.throttle = 0
Rover.brake = 0
if Rover.rock_found is True and len(Rover.rock_nav_angles) > 1:
Rover.steer = np.clip(np.mean(Rover.rock_nav_angles * 180/np.pi), -15, 15)
else:
Rover.steer = np.clip(np.mean(Rover.nav_angles * 180/np.pi), -15, 15)
# If there's a lack of navigable terrain pixels then go to 'stop' mode
elif len(Rover.nav_angles) < Rover.stop_forward:
# Set mode to "stop" and hit the brakes!
Rover.throttle = 0
# Set brake to stored brake value
Rover.brake = Rover.brake_set
Rover.steer = 0
Rover.mode = 'stop'
# If we're already in "stop" mode then make different decisions
elif Rover.mode == 'stop':
# If we're in stop mode but still moving keep braking
if Rover.vel > 0.2:
Rover.throttle = 0
Rover.brake = Rover.brake_set
Rover.steer = 0
# If we're not moving (vel < 0.2) then do something else
elif Rover.vel <= 0.2:
# Now we're stopped and we have vision data to see if there's a path forward
if len(Rover.nav_angles) < Rover.go_forward:
Rover.throttle = 0
# Release the brake to allow turning
Rover.brake = 0
if Rover.near_sample:
Rover.steer = 0
else:
# Turn range is +/- 15 degrees, when stopped the next line will induce 4-wheel turning
Rover.steer = -15 # Could be more clever here about which way to turn
# If we're stopped but see sufficient navigable terrain in front then go!
if len(Rover.nav_angles) >= Rover.go_forward:
# Set throttle back to stored value
Rover.throttle = Rover.throttle_set
# Release the brake
Rover.brake = 0
# Set steer to mean angles
Rover.steer = np.clip(np.mean(Rover.nav_angles * 180/np.pi), -15, 15)
Rover.mode = 'forward'
# Just to make the rover do something
# even if no modifications have been made to the code
else:
Rover.throttle = Rover.throttle_set
Rover.steer = 0
Rover.brake = 0
# If in a state where want to pickup a rock send pickup command
if Rover.near_sample and Rover.vel == 0 and not Rover.picking_up:
Rover.send_pickup = True
Rover.mode = 'forward'
return RoverLaunching in autonomous mode, your rover can navigate and map autonomously.
Note: running the simulator with different choices of resolution and graphic quality may produce different results, particularly on different machines!
The rover is running well in the simulations with average 60 FPS but not perfect.
The Failure
- The rover is having difficulty in navigating around with the big stones and small stones. It makes the rover keep swinging left and right.
- I’ve already implemented the unstuck scenario, but it’s still possible to get stuck.
Future Enhancement
Nothing is perfect, there’ are always lots of fun works to improve. - Covering all the maps
- Using deep learning for stone detection to know better about the environment.
- Develop an algorithm to discover the unknown places.