Responsible lecturer: Vít Krátký (kratkvit@fel.cvut.cz)

Your task will be to finish a prepared node for finding potential goal positions that should lead the robot to explore an unknown environment.

Where should the robot go to expand its map? The vast majority of approaches to this problem are based on the idea of “frontiers” introduced in [1] in 1997. A frontier is usually defined as the boundary between space that is known and free (containing no obstacles) and the space that is unknown. When dealing with a lidar-equipped robot using an occupancy grid in an indoor setting, simply moving the robot close to a frontier will nearly always uncover a new part of the environment and expand the occupancy grid. In this assignment, you will be implementing a node for finding these frontiers on a 2D occupancy grid (see HW06 for detailed info about the data type). We will use these specific definitions:

• Frontier Cell: a cell of the occupancy grid that is free and has at least 1 unknown cell next to it (in an 8-neighborhood).
• Frontier: a contiguous (connected in an 8-neighborhood) set of frontier cells
• Frontier Center: a frontier cell belonging to a frontier, which is closest to the geometric mean position of the cells. Since the mean position might lie in unsafe space, it is safer to rather navigate to a frontier cell, which is known to be free of obstacles.

As motivation, here is how more sophisticated exploration approaches, which still use the concept of frontiers, can work in 3D:

Searching for frontier cells and navigating to them on the input occupancy grid would work nicely if your robot was smaller than the cell of the grid. However, the robot is much larger than a single cell. This could thus cause the detected frontier cells to lie in places where the robot would collide with obstacles! For this reason, you need to inflate obstacles and unknown space (since it also might contain obstacles), similarly as you did in Homework 06 - Path Planning. Use the provided get_circular_dilation_footprint() kernel with the following function to perform the inflation:

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

You should then run the WFD algorithm on the modified grid. Make sure you merge the inflated obstacles and unknown space in the correct order!

Also, as in the Homework 06 - Path Planning, you should handle the case when the robot is too close to obstacles. If that happens, the start position can seem untraversable in the inflated grid, and then the first BFS of the WFD will immediately stop and return no frontiers. Setting the area around the robot as traversable, as in HW06, is the simplest solution, but beware - this could lead to finding deformed frontiers, depending on the specific way of implementation. We suggest starting the first BFS of the WFD with a larger starting 'open set' - not just the one cell corresponding to the robot's center position, but putting all the cells within the robot's dilation footprint into the starting open set.

To find the frontiers in the grid, we will use the WFD algorithm introduced in the lectures. In a nutshell, it is a 2-stage BFS algorithm on a 2D grid. We run it in two iterations:

• outer BFS - starting from the robot's position, search for frontier cells on the modified grid
• inner BFS - “frontier assembly” = group contiguous frontier cells into separate frontiers.

For the pseudocode and more details, please see the ARO lectures. Make sure to only accept frontier cells that are reachable, and filter out frontiers that have fewer cells than the value of the min_frontier_size parameter. You can find pseudocode and more information in the lectures and in [2]. An example result can be seen below. Green = frontier cells, Red = frontier centers, Blue = robot's position.

Your task will be to implement functionality for the detection of frontiers in an occupancy grid. You will need to finish parts of code in aro_exploration/aro_exploration/frontier/frontier.py and aro_exploration/src/aro_map/utils.py.

1) If you haven't already done it in the Path planning homework, implement the map_to_grid_coordinates() and grid_to_map_coordinates() functions in aro_exploration/src/aro_map/utils.py. These should transform a given position in the 'icp_map' frame (e.g., the robot's position) to discrete coordinates specifying a cell in the occupancy grid (see above for an image explaining the indexing we will use in this assignment).

2) In the find_frontiers function or when receiving an occupancy grid message, construct a modified grid where the obstacles and unknown space are inflated. Use the get_circular_dilation_footprint() in utils.py as the kernel for inflating the obstacles with the provided dilation function: https://docs.scipy.org/doc/scipy/reference/generated/scipy.ndimage.grey_dilation.html. For this assignment (you can change it for the semestral work), do not use a different kernel or different function, because the evaluation uses these and a mismatch might cause your solution to find frontiers at positions that could be unsafe according to the different kernel

3) Implement the Wavefront Frontier Detection algorithm and run it on the modified occupancy grid to obtain a list of reachable frontiers. We suggest you use the provided Frontier class.

4) From all found frontiers, select the one with the center closest to the robot and return its center in map coordinates in the get_closest_frontier service.

Make sure to:

• search for frontiers on a grid inflated by the given kernel and inflation function.
• return the closest Frontier Center, not the closest Frontier Cell
• filter out too small frontiers based on the given min_frontier_size parameter
• mark the cells in the modified grid near the robot's position (using the same kernel as for inflating the obstacles and unknown space) as free to avoid not generating any goals when the robot ends up close to a wall
• use the parameter occupancy_threshold for deciding how cells should be classified (higher values = occupied, lower or equal = free, negative = unknown).

Local evaluation of your solution can be run using

ros2 launch aro_exploration aro_frontier_evaluation.launch.xml

This starts a map publisher and an evaluation script which consequently sends frontier generation requests with current positions and goals loaded from a file aro_exploration/data/frontier/frontier_requests.csv. The file with frontier generation requests is a csv file with following format:

start_x,start_y,frontier_ref_x,frontier_ref_y
-0.30,0.70,-0.825,1.875
-0.30,-1.00,-0.275,-1.775
1.00,1.00,1.575,0.825
-1.65,0.30,-1.625,0.475
1.90,-1.90,1.525,-1.625

In this file each line specifies x, y coordinates of a start/robot position, and x, y coordinates of the expected position of a closest frontier. You can add lines or adapt existing lines in this file arbitrarily to include additional test cases for local testing of your solution.

Once the evaluation is finished, the script prints results of the evaluation in the command line. The output of the evaluation for correct implementation should look like:

In this table, sucess indicates whether all requirements for a given request were fulfilled, dist. to ref. shows distance of the generated frontier for the reference position of frontier, column frontier shows position of the generated frontier, column ref. frontier shows position of the correct closest frontier, robot pos. stands for position of robot that was used for choosing the closest frontier, timeout indicates whether maximum computation time was exceeded during generation of frontiers.

If you want to run your implementation of frontiers generation in a separate command line window in order to separate the command line outputs, you can run the Rviz and evaluation script by

ros2 launch aro_exploration aro_frontier_evaluation.launch.xml run_frontier_generator:=false

and then run your implementation of frontier_generator in another window using command

ros2 launch aro_exploration aro_frontier.launch.xml

Run script aro_exploration/homeworks/create_hw07_package.sh to create an archive containing your implementation (frontier.py and utils.py) and upload it to the upload system. The automatic evaluation uses a set of frontier generation requests similar to those provided in frontier_requests.csv. The points obtained for the solution are proportional to number of successfully generated frontiers closest to the current position of the robot. Please test your solution using the local evaluation script first. The results should be equivalent.

[1] YAMAUCHI Brian. A frontier-based approach for autonomous exploration. Proceedings 1997 IEEE International Symposium on Computational Intelligence in Robotics and Automation CIRA'97. 'Towards New Computational Principles for Robotics and Automation', 146-151

[2] TOPIWALA, Anirudh; INANI, Pranav; KATHPAL, Abhishek. Frontier Based Exploration for Autonomous Robot. arXiv preprint arXiv:1806.03581, 2018.