{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Readme"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Notebook Analysis"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Describe in your writeup (and identify where in your code) how you modified or added functions to add obstacle and rock sample identification."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Replaced threshold function with cv2.inRange() and modified to include an upper bound as well."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def color_thresh(img, lower_rgb=(160, 160, 160), upper_rgb=(255,255,255)):\n",
    "    # Create an array of zeros same xy size as img, but single channel\n",
    "    return cv2.inRange(img, lower_rgb[::-1], upper_rgb[::-1])\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Used HSV for rock detection. RBG didn't seem to work very well, but I'm not sure why."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def segment_rock(img):\n",
    "    ROCK_MIN = np.array([90, 100, 120],np.uint8)\n",
    "    ROCK_MAX = np.array([100, 255, 255],np.uint8)\n",
    "    img_hsv = cv2.cvtColor(img,cv2.COLOR_BGR2HSV)\n",
    "    return cv2.inRange(img_hsv, ROCK_MIN, ROCK_MAX)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Simple rgb threshold for obstacles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "obstacles = color_thresh(warped, lower_rgb=(0, 0, 0), upper_rgb=(80,80,80))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Everything else was simply code from the lessons or already provided in the jupyter notebook."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Describe in your writeup how you modified the process_image() to demonstrate your analysis and how you created a worldmap. Include your video output with your submission."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For accuracy, only update the world map when roll and pitch are minimal."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def convert_angle(angle):\n",
    "    #convert from 0 to 360 to -180 to +180\n",
    "    if angle > 180:\n",
    "        angle -= 360\n",
    "    return angle\n",
    "\n",
    "#     if abs(convert_angle(data.pitch[data.count])) < 0.25 and abs(convert_angle(data.roll[data.count])) < 0.25:\n",
    "#         data.worldmap[obstacle_y_world, obstacle_x_world, 0] += 1\n",
    "#         data.worldmap[rock_y_world, rock_x_world, 1] += 1\n",
    "#         data.worldmap[navigable_y_world, navigable_x_world, 2] += 1\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Everything else was simply code from the lessons or already provided in the jupyter notebook."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Autonomous Navigation and Mapping"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### perception_step() and decision_step() functions have been filled in and their functionality explained in the writeup."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "perception_step() was just a copy/paste of notebook code."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "decision_step() was more interesting."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1. I re-wrote decision_step() as a finite state machine with several states (forward, picking up, stuck!, seek rock, stop before rock, etc)\n",
    "1. I added an output() function that clearly sets all FSM outputs in 1 line of code.\n",
    "1. the steer angle is the mean plus 0.75 of a standard deviation in order to make it (hopefully) follow the wall (leftside crawler). A full std dev seemed to cause him to become stuck against the wall, but < 0.5 and he wouldn't follow forks in the road.\n",
    "1. The rover keeps track of the change in x,y position (within tolerance) and the time of last change in order to detect if he's stuck. If the position hasn't change for 5 seconds, he moves to the stuck state.\n",
    "1. If we are in the stuck state we back up for 4 seconds.\n",
    "1. As soon as we detect a rock, we stop and go into a seeking rock state where the max velocity is half normal. This is in an effort to not over-shoot rocks.\n",
    "1. In the rock seeking state, we simply approach the rock at half speed and then pick it up.\n",
    "1. Reduced max velocity to 1 m/s as it helps with fidelity and keeping pitch and roll minimal. Also, I found that there were oscillations in the rover's path at 2 m/s.\n",
    "1. I implemented a FIR filter for the steering angles, but even with only 3 elements if made the rover undershoot (too sluggish).\n",
    "1. I never implemented any real obstacle avoidance. If there's navigable terrain of either side, he'll aim right for the rocks, so the success of a trial is quite sensitive to initial conditions.\n",
    "1. The rover tended to get stuck against the wall when picking up rocks that are also against the wall. There's a steering bias in the rock seek state where he keeps the rock to his left side, since he's a left side wallcrawler.\n",
    "1. The rover only seeks rocks that are on his left side so as not to get distracted and resume wall crawling before fully following the wall.\n",
    "1. The rover usually meets the goals of ~40% mapped with ~75% fidelity within 5 minutes.\n",
    "1. After about 15 minutes >95% is mapped, unless he gets stuck.\n",
    "1. Sometimes he has a hard time getting to the wall and just spins in circles in the starting open area.\n",
    "1. He tends to always get stuck in the same spot where there is a narrow opening."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "FYI: Running a MacBook Pro 2012, unity is 1440x900@25fps"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Improvements:\n",
    "1. I really wanted to use the world map generated by the rover instead of just using the instantaneous transformed view. For example, I was interested in using the gradient of the generated world map and using that to follow the contours a fixed distance from the wall instead of trying to maintain a bias from the mean navigable pixels. That approach would allow the rover to actually try and map out a continuous contour and allow it to enter narrow \"nooks and crannies\". Time constraints and limited experience with numpy stopped me from getting into this.\n",
    "\n",
    "1. "
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.2"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}