======= Homework 06 - Path planning and frontier detection =======

**Responsible lecturer:** František Nekovář ([[mailto:nekovfra@fel.cvut.cz?subject=ARO HW06|nekovfra@fel.cvut.cz]])

The HW06 consists of two parts explained below.

====== Frontier detection ======

Your goal in this task will be to implement detection of frontiers in an occupancy grid.
Use the provided ''frontier.py'' template for implementation.
 
Several goal-seeking services should be implemented:
  * Return random frontier
  * Return closest frontier (Euclidean distance)
  * Return best frontier to visit (path length, frontier size?)
  * Return random valid point (fallback behaviour)

Only the second service will be evaluated, use provided ''frontier_evaluate.py'' file for example testing.

==== Wave-front detection (WFD) ====

In a nutshell, is a BFS algorithm on a 2D grid. We run it in two iterations:

  * outer BFS - search for frontier points connected to the robot position,
  * inner BFS - "frontier assembly", search for contiguous frontier points.

Make sure to filter the frontiers, i.e. make sure they are reachable. 

You can find pseudocode and more information in [1].

Frontier points close to the obstacles are untraversable, robot would collide when visiting them.
Make sure your returned frontiers point to traversable space.
Unknown map points might also contain obstacles!
Robot diameter is also given in the task.
Solution is to add a safety margin, inflate obstacles (and possible obstacles) in the grid.
Use the provided dilation function with your custom kernel:

https://docs.scipy.org/doc/scipy/reference/generated/scipy.ndimage.grey_dilation.html


==== Occupancy grid ====

The data received from localization is defined as an OccupancyGrid structure in: //nav_msgs/OccupancyGrid//

Indexing on a grid is row-column, ie. opposite from x-y in real-world. Make sure to address that.

data (int[8])
  * 0 == empty cell
  * 1 .. 100 == (probably) occupied cell
  * -1 == unknown (unseen) cell

info (//nav_msgs/MapMetaData//)
  * resolution (float32) - "size" of cell in meters
  * width - width of grid in cells
  * height - height of grid in cells
  * origin (//geometry_msgs/Pose//) - relation of origin [0,0] of the grid to the real world (e.g. map tf)

Robot position needs to be transformed to fit into the grid - translation and scaling.

{{ :courses:aro:tutorials:occupancy_grid.png?1280 |}}

The grid obstacles and unknows points should also be inflated. An example inflation footprint is below, dimension is an **odd number** as robot is itself on some grid cell:

{{ :courses:aro:tutorials:footprint.png?600 |}}

==== Testing ====

For running the example evaluation code, use provided ''frontier_evaluate.py'' file.

For visualization, you can use provided launch file: <code>roslaunch aro_exploration aro_frontier_planning_test_local.launch</code>. **These are not intended to be run together!**

You can publish robot position yourself using static transform:
<code>
rosrun tf2_ros static_transform_publisher x y z r p y icp_map base_footprint
</code> where 'x y z' specify position and 'r p y' orientation of the robot.

The services can be called using the command line, for example:
<code>
rosservice call /get_closest_frontier
</code>
and
<code>
rosservice call /get_random_frontier
</code>

Expected frontier output is below (planned path is an example).

{{ :courses:aro:tutorials:example_vis.png?480 |}}

==== Submission and evaluation ====

Frontier script will be evaluated on an example map. Returned points' closeness to nearest reachable frontier and traversability will be checked.
Expected output is in the same coordinates as the input, ie. the robot frame. Valid solutions will be awarded 2.5 points.

==== References ====

[1] [[ https://arxiv.org/pdf/1806.03581.pdf | TOPIWALA, Anirudh; INANI, Pranav; KATHPAL, Abhishek. Frontier Based Exploration for Autonomous Robot. arXiv preprint arXiv:1806.03581, 2018.]]



====== Path planning =====

Your task in the second part is to implement A* path planning on a grid. In a nutshell, you will extend your previous BFS with a heuristic function based on euclidean distance.

Path planning in the occupancy grid will be implemented in ''planner.py''. 
Implement a service which takes origin and goal poses in map frame as input and returns array of poses in map frame.
Conversion to and from grid frame has to be performed.
Generated path should be safe to traverse and as short as possible. It should also contain both start and goal poses.
Grid nodes should be connected using an 8-neighborhood for optimal performance, mind the corners' traversability though!

Also, make sure you are indexing the occupancy grid correctly. Reference data output can be found in the evaluation script. Visualize your data to make sure path does not cross any obstacles.

==== Testing ===

Locally, you can use same launch file as above for path visualization. Just make sure you publish the path from your planner.py node!

Example evaluation which also contains reference data is located in ''planner_evaluate.py''.

Call the planner service using:
<code>
rosservice call /plan_path "start:
  x: 0.0
  y: 0.0
  theta: 0.0
goal:
  x: -1.0
  y: 0.0
  theta: 0.0"
</code>



==== Submission and evaluation ====

Path planning will be evaluated using simulation.
Paths will be checked for length and validity.
Robot diameter will be kept the same during our testing.
Valid solution will be awarded 2.5 points.