Responsible lecturer: František Nekovář (nekovfra@fel.cvut.cz)

The HW06 consists of two parts explained below.

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.

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

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

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.

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

For debugging purposes, you can also publish necessary data yourself:

• execute rosbag play -l map_dat.bag from the data/planning directory.
• provide robot position transform:

rosrun tf2_ros static_transform_publisher x y z r p y icp_map base_footprint

The services can be called using the command line, for example:

rosservice call /get_closest_frontier

rosservice call /get_random_frontier

Frontier script will be evaluated on an example map. Returned points' closeness to nearest reachable frontier and traversability will be checked. Valid solutions will be awarded 2.5 points.

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

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!

You can test the code manually by running rosbag play -l map_dat.bag and calling the planner service:

rosservice call /plan_path "start:
  x: 0.0
  y: 0.0
  theta: 0.0
goal:
  x: -1.0
  y: 0.0
  theta: 0.0"

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.