===== Task03 - Grid-based path planning ======
The main task is to implement a simple grid-based planning approach.

|**Deadline**  |  27. October 2018, 23:59 PST |
|**Points**  |  3 |
|**Label in BRUTE**  |  Task03 |
|**Files to submit**  |  archive with ''gridmap'' directory and ''gridplanner'' directory |
| |  <fc #ff0000>Minimal content of the archive:</fc>  ''gridmap/GridMap.py'', ''gridplanner/GridPlanner.py''| 
|**Resources**  |  {{ :courses:b4m36uir:hw:task03.zip | Task03 resource package}}|

===Assignment===
Implement the following basic steps of the grid-based navigation pipeline:
  - In file ''GridMap.py'' implement the obstacle growing (method ''grow_obstacles'') to take into account the robot embodiment
  - In file ''Planner.py'' implement a grid-based planning method (method ''plan'') to find a path between the start and goal position
  - In file ''Planner.py'' implement the trajectory smoothing method (method ''simplify_path'') which purpose is to provide smoother path for the path following controller that drives the real robot  

=== Approach ===
    - **Obstacle growing** - can be achieved by different means, typically methods of mathematical morphology are being used. In particular, binary dilation of the obstacles for a predefined ''distance'' which is set by user to take into account the robot embodiment. Typically, half of the size of the being used as the ''distance''. Further, methods based on distance transform can be also used to grow the obstacles or as a heuristic function for the planner to stay away from walls.
    - **Planning** - grid-based path planning takes the gridmap, starting position(''start'') and goal position(''goal'') on the input and provide a list of cell coordinates on the output. If the path is not found, the planner returns ''None'' 
    - **Path simplification** - path simplification is usually done by excluding navigation points from the path, that are not necessary in a sense, that the robot does not have to visit them precisely. Typical approach to trajectory smoothing is to connect the neighboring segments one by one using straight-line segments (using Bresenham line algorithm) up to the point where the straight-line segment collide with an obstacle (grown obsttacle) and then follow with another straight-line segment.

=== Evaluation ===
The code can be evaluated using the following script
<code python>
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import sys
import math
import time
import numpy as np
import matplotlib.pyplot as plt

sys.path.append('robot')
sys.path.append('gridmap')
sys.path.append('gridplanner')
 
import Robot as rob
import GridMap as gmap
import GridPlanner as gplanner

PLOT_ENABLE = True

if __name__=="__main__":
 
    #define planning problems:
    #  map file 
    #  map scale [m] (how big is a one map cell in comparison to the real world)
    #  start position [m]
    #  goal position [m]
    #  execute flag
    scenarios = [("maps/maze01.csv", 0.1, (8.5, 8.5), (1, 1), False),
                 ("maps/maze02.csv", 0.1, (8.5, 8.5), (5.6, 3), False),
                 ("maps/maze02.csv", 0.1, (8.5, 8.5), (5.8, 2.5), False),
                 ("maps/maze03.csv", 0.1, (19.5, 23.0), (6.2, 26.4), False),
                 ("maps/maze04.csv", 0.1, (51.6, 53.3), (19.2, 12.4), False),
                 ("maps/maze05.csv", 0.1, (78.8, 79.2), (7.0, 14.6), False),
                 ("maps/maze02.csv", 0.1, (8.5, 8.5), (5.6, 3), True)]

    #fetch individual scenarios
    for scenario in scenarios:
        mapfile = scenario[0] #the name of the map
        scale = scenario[1]
        start = scenario[2]   #start point
        goal = scenario[3]    #goal point
        execution_flag = scenario[4] #execute the trajectory in vrep

        #instantiate the map
        gridmap = gmap.GridMap()
        
        #load map from file
        gridmap.load_map(mapfile, 0.1)

        #plot the map with the start and goal positions
        if PLOT_ENABLE:
            gmap.plot_map(gridmap)
            gmap.plot_path(gridmap.world_to_map([start, goal]))
            plt.show()

            #show the free/occupied space
            gmap.plot_map(gridmap, clf=True, data='free')
            plt.show()
    
        #blow the obstacles to avoid collisions
        gridmap.grow_obstacles(0.4)
        
        #show the map after obstacle blowing
        if PLOT_ENABLE:
            gmap.plot_map(gridmap, clf=True, data='free')
            plt.show()
        
        #plan the route from start to goal
        planner = gplanner.GridPlanner()
        path = planner.plan(gridmap, gridmap.world_to_map(start), gridmap.world_to_map(goal))

        if path == None:
            print("Destination unreachable")
            continue

        #show the planned path
        if PLOT_ENABLE:
            gmap.plot_map(gridmap)
            gmap.plot_path(path)
            plt.show()

        #simplify the path
        path_s = planner.simplify_path(gridmap, path)

        #show the simplified path
        if PLOT_ENABLE:
            gmap.plot_map(gridmap)
            gmap.plot_path(path)
            gmap.plot_path(path_s, color='blue')
            plt.show()

        if execution_flag:
            #instantiate the robot
            robot = rob.Robot()

            #execute the path
            for waypoint in path_s:
                #navigate the robot towards the target
                status1 = robot.goto(gridmap.map_to_world(waypoint))

                #check for the execution problems
                if not status1:
                    print("The robot has collided en route")
                    break


</code>