{{indexmenu_n>3}}
===== t1c-plan - Grid-based path planning ======
The main task is to implement a simple grid-based planning approach. You will learn how to plan on the grid such that the robot embodiment is considered.

|**Files**  |  ''Planner.py'' - File to be implemented \\ ''bresenham.py'' - Implementation of the Bresenham's line algorithm \\ ''utils.py'' - Helpers for evaluation, drawing, and saving figures \\ ''data'' - Scenarios and reference solutions in bag files \\ ''t1c-plan.py'' - Evaluation script |
|**Resources**  | {{ :courses:crl-courses:redcp:tasks:redcp.zip | redcp.zip}}, \\ {{ :courses:crl-courses:redcp:tasks:t1c-plan.zip |}} - **<fc #ff0000>Initial files and evaluation script</fc>** |

<note tip>Implement 
  * ''Planner.grow_obstacles()'' in ''Planner.py'',
  * ''Planner.plan_path()'' in ''Planner.py'',
  * ''Planner.simplify_path()'' in ''Planner.py''.
</note>

----

=== Assignment ===
In ''Planner.py'', implement the following steps of grid-based planning.
  - Implement the obstacle growing (method ''grow_obstacles'') to account for the robot embodiment.
  - Implement a grid-based planning method (method ''plan_path'') to find a path between the start and goal position (considering planning with 8-neighborhood).
  - Implement the trajectory smoothing method (method ''simplify_path'') that simplifies the path as a sequence of waypoints

----

====== Obstacle Growing ======
The ''grow_obstacles'' function processes the occupancy grid map into a binary map that encompasses areas inaccessible for the robot considering its embodiment.
Using obstacles growing by a disk representing the robot shape, we can use such a grid map for grid-based planning with a single-cell robot.
The robot shape can be modeled as a disk with $r_\mathrm{robot}$ radius ''robot_size''. 
The result of the obstacle growing is a binary map with the value 1 for occupied and unknown cells and 0 for traversable cells; thus, we threshold the occupancy grid in addition to obstacle growing.
For each cell $m_i$ of the ''grid_map'' the new binary value $b_i$ of the ''grid_map_grow'' is based on its Bayesian occupancy probability $P(m_i = occupied \vert z) = 0.5$ and distance to the nearby obstacles according to:
  - **Filtering all obstacles** - if $P(m_i = occupied \vert z) > 0.5$, then the cell is considered occupied $b_i$ = 1;
  - **Filtering all unknown areas** - if $P(m_i = occupied \vert z) = 0.5$, then the cell is considered occupied $b_i$ = 1;
  - **Filtering cells close to the obstacles** - if $P(m_i = occupied \vert z) < 0.5$ and the approximated Euclidean distance on the grid of the cell $m_i$ and the closest obstacle is shorter than $r_\mathrm{robot}$, then the cell is considered occupied $b_i$ = 1;
  - **Otherwise** the cell is considered free $b_i=0$.
We can further denote the mid probability of a cell being occupied as ''CELL_MID''.

The input parameters of the obstacle-growing function are: 
  - ''grid_map'' - [[:courses:crl-courses:redcp:tasks:data_types|OccupancyGrid]] - grid map representation;
  - ''robot_size'' - the robot radius in meters.

The function returns
  - [[:courses:crl-courses:redcp:tasks:data_types|OccupancyGrid]] with the processed grid map - binary map with filtered obstacles, unknown and inaccessible areas.

The ''grow_obstacles'' function in the ''Planner.py'' class has the following prescription.
<code python>
      def grow_obstacles(self, grid_map: OccupancyGrid, robot_size) -> OccupancyGrid:
        """ Method to grow the obstacles to take into account the robot embodiment
        Args:
            grid_map: OccupancyGrid - grid map for obstacle growing
            robot_size: float - the size of the robot
        Returns:
            grid_map_grow: OccupancyGrid - grid map with considered robot body embodiment
        """
</code>
The following figure depicts an example of the obstacle growing for the first evaluation scenario. From left to right:
  * Original map;
  * Map grown for robot diameter 0.3 m;
  * Map grown for robot diameter 0.5 m.

| {{::courses:crl-courses:redcp:tasks:map_base.png?300|}} | {{::courses:crl-courses:redcp:tasks:map_grown.png?300|}}| {{::courses:crl-courses:redcp:tasks:map_grown2.png?300|}} |

++++ Suggested approach (click to view)|
**Obstacle growing** - The robot has non-zero dimensions representing its embodiment, which must be considered when planning. Moreover, the plan execution in real experiments has a limited precision that further enlarges demands on the clearance to the obstacles for the robot to navigate the environment safely. We can grow the borders of the obstacles by the diameter of the robot and plan in the resulting map that ensures sufficient clearance to the obstacles. The obstacle growing can be achieved by one of the following approaches: 
  * Using convolution masks;
  * Using mathematical morphology;
  * Using distance transform, implemented using, e.g., ''scipy.ndimage.distance_transform_edt'' (suggested approach).
++++

====== Path planning ======
Path planning is to be implemented in the ''plan_path'' function that provides a path between the given start and goal poses on the grid map considering 8-neighborhood. The function returns a path on the output. If the path is not found, the planner returns ''None''.

The input parameters of the ''plan_path'' function are: 
  - ''grid_map'' - [[:courses:crl-courses:redcp:tasks:data_types|OccupancyGrid]] - pre-processed grid map representation (contains 1 and 0 only);
  - ''start'' - [[:courses:crl-courses:redcp:tasks:data_types|Pose]] - start robot pose;
  - ''goal'' - [[:courses:crl-courses:redcp:tasks:data_types|Pose]] - goal robot pose.

The function returns
  - [[:courses:crl-courses:redcp:tasks:data_types|Path]] with the found path or ''None'' when the path is not found. It is supposed that the path contains only 2D coordinates and not the heading. Only ''position.x'' and ''position.y'' entries of individual poses in the path are filled in. <fc #ff0000>The path is returned in the world coordinates!</fc>

The ''plan_path'' function in the ''Planner.py'' class has the following prescription.
<code python>
    def plan_path(self, grid_map: OccupancyGrid, start: Pose, goal: Pose) -> Path:
        """ Method to plan the path from the start to the goal pose on the grid
        Args:
            grid_map: OccupancyGrid - grid map for obstacle growing
            start: Pose - robot start pose
            goal: Pose - robot goal pose
        Returns:
            path: Path - path between the start and goal Pose on the map
        """
</code>
An example of the planned paths with the A* algorithm for different robot sizes is visualized in the following figure. From left to right, the planning for:
  * Original map;
  * Map grown for robot diameter 0.3 m;
  * Map grown for robot diameter 0.5 m;
| {{::courses:crl-courses:redcp:tasks:map_grown_plan.png?300|}} | {{::courses:crl-courses:redcp:tasks:map_grown_plan2.png?300|}} | {{::courses:crl-courses:redcp:tasks:map_grown_plan3.png?300|}} |

<note important>Note that the task concerns planning in robotics and not implementing an A* planner. Hence, with a proper citation of the source in the source files, you are encouraged to use any publicly available implementation of the planner you want. Rather, get the intuition behind looking at the resulting paths and recognizing sub-optimal paths, wrongly set heuristics, and so on.</note>
++++ Suggested approach (click to view)|
Considered planning methods
  *  Breadth First Search (BFS), Depth First Search (DFS)
  *  Dijkstra
  *  A*
  *  Jump Point Search (JPS)
  *  Theta* (any-angle path planning - returns the shortest path)
  *  ARA* (Any-time repairing A*)
  *  D* lite
  *  Floyd-Warshall
<note important>Pay attention to the planning algorithm used. If you decided to implement A* planner, it is assumed that the path is optimal!</note>
++++

----

====== Path Simplification ======
Path simplification can be done by excluding unnecessary cells (locations) from the path. The robot does not have to visit them precisely or will visit them along the navigation to the waypoint, where it needs to change direction. A possible approach to path simplification is to employ the ray-casting technique to connect the neighboring segments one by one using straight-line segments up to the point where the straight-line segment collides with an obstacle (grown obstacle) and then follows with another straight-line segment.

++++ $\qquad\bullet\,$ Bresenham's line algorithm (click to view) | 
  *  <code python>
    def bresenham_line(self, start: (int, int), goal: (int, int)) -> (int, int):
        """Bresenham's line algorithm
        Args:
            start: (int, int) - start coordinate
            goal: (int, int) - goal coordinate
        Returns:
            Interlining points between the start and goal coordinate
        """
        (x0, y0) = start
        (x1, y1) = goal
        line = []
        dx = abs(x1 - x0)
        dy = abs(y1 - y0)
        x, y = x0, y0
        sx = -1 if x0 > x1 else 1
        sy = -1 if y0 > y1 else 1
        if dx > dy:
            err = dx / 2.0
            while x != x1:
                line.append((x,y))
                err -= dy
                if err < 0:
                    y += sy
                    err += dx
                x += sx
        else:
            err = dy / 2.0
            while y != y1:
                line.append((x,y))
                err -= dx
                if err < 0:
                    x += sx
                    err += dy
                y += sy
        x = goal[0]
        y = goal[1]
        return line
</code>
++++


<note tip>You can use the provided ''bresenham_line'' function from the ''bresenham.py''.</note>

The input parameters of the function are
  - ''grid_map'' - [[:courses:crl-courses:redcp:tasks:data_types|OccupancyGrid]] - pre-processed grid map representation (contains 1 and 0 only);
  - ''path'' - [[:courses:crl-courses:redcp:tasks:data_types|Path]] - path to be simplified.

The function returns:
  - the [[:courses:crl-courses:redcp:tasks:data_types|Path]] simplified path with the processed path. <fc #ff0000>The path is returned in the world coordinates!</fc>

The ''simplify_path'' function in the ''Planner.py'' class has a following prescription:
<code python>
      def simplify_path(self, grid_map: OccupancyGrid, path: Path) -> Path:
        """ Method to simplify the found path on the grid
        Args:
            grid_map: OccupancyGrid - grid map for obstacle growing
            path: Path - path to be simplified
        Returns:
            path_simple: Path - simplified path
        """

</code>
The result of path simplification may vary, but an example of the planned and simplified path using the suggested approach is visualized in the following figure. From left to right, the planning and simplification for
  * Original map;
  * Map grown for robot diameter 0.3 m;
  * Map grown for robot diameter 0.5 m.

| {{::courses:crl-courses:redcp:tasks:map_grown_plan_simple.png?300|}} | {{::courses:crl-courses:redcp:tasks:map_grown_plan_simple2.png?300|}} | {{::courses:crl-courses:redcp:tasks:map_grown_plan_simple3.png?300|}} |

++++ Suggested approach (click to view)|
  * Implementation of the path simplification can be relatively straightforward. In a pseudocode, it can be as follows.
<code python>
#add the start position
path_simple add path[0]

#iterate through the path and simplify the path
while not path_simple[end] == path[end]: #until the goal is not reached
  #find the connected segment
  previous_pose = path_simple[end]
  for each pose in the rest of the path:
    if bresenham_line(path_simple[end], pose) not collide: #there is no collision
      previous_pose = pose
      
      #the goal is reached
      if pose = path[end]:  
        path_simple append pose
        break
        
    else: #there is a collision
      path_simple append previous_pose
      break
</code>
++++

=== Evaluation ===
The evaluation focuses on the correct behavior of the above-described robot navigation pipeline; see ''t1c-plan.py'' for the full evaluation script.

<code python>
ROBOT_SIZE = 0.5

if __name__=="__main__":
    planner = Planner()
    scenarios =  pickle.load( open( "data/scenarios.bag", "rb" ) )
    for gridmap, start, goal in scenarios:
        gridmap_processed = planner.grow_obstacles(gridmap, radius)
        path = planner.plan_path(gridmap_processed, start, goal)
        path_simple = planner.simplify_path(gridmap_processed, path)
</code>