Rover Search and Sampling Return

Last Updated on 21 October 2020

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
  • LINKGITHUB
Rover Project

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.

Rover View

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.

Rover View 2
Rover View 2

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 Rover

In 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 Rover

Launching 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

  1. The rover is having difficulty in navigating around with the big stones and small stones. It makes the rover keep swinging left and right.
  2. 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.
  3. Covering all the maps
  4. Using deep learning for stone detection to know better about the environment.
  5. Develop an algorithm to discover the unknown places.