Responsible lecturer: Tomáš Musil (musilto8@fel.cvut.cz)

Your task will be to finish a node that finds a collision-free path to a given goal by running the A* algorithm on an occupancy grid. This will later come in useful for both finding paths towards exploration goals and for safely returning home in the semestral work.

A* is a graph search algorithm that was originally developed for making the robot Shakey navigate and is nowadays a very popular algorithm for pathfinding in robotics and game development. For its in-depth explanation, please see the lecture slides. The A* algorithm is also very well explained in these videos from Computerphile and Sebastian Lague:

Your task will be to implement the algorithm for pathfinding in the case of a circular robot and using a 2D occupancy grid. This will require modifying the grid to take into account the robot's radius to avoid colliding with obstacles. An example resulting path can be seen here:

The most common representation for path planning and exploration in mobile robotics is an occupancy grid. The core idea of the occupancy grid is to divide the world into discrete 2D or 3D cells that can be occupied (= containing a physical object), free (= known to contain no objects) or unknown (= initial state). These values are updated at a high rate using the sensor data. In the case of a LIDAR, you essentially trace a ray from the robot through the grid towards each scan point and increase the likelihood of traversed cells being free, and increase the likelihood of the cell being occupied at the point where the LIDAR hit. In 3D, a more efficient octree representation is often used (e.g. OctoMap), but it still represents occupancy. An occupancy grid-based representation is also often used to construct a more abstract, topological map - for example to extract a graph of rooms or arbitrary convex segments for faster pathfinding or obtaining semantics.

In ARO, we will be using a simple 2D occupancy grid for both path planning and finding exploration goals. The updating of the grid is already implemented and done by the occupancy_mapper node and need not be changed. This node publishes a message of type nav_mgs/OccupancyGrid which you will work with.

The message is structured as follows:

data (int[8])

• 0 == empty cell
• 1 .. 100 == (probably) occupied cell
• -1 == unknown (unseen) cell

info (nav_msgs/MapMetaData)

• resolution (float32) - side length of each cell in meters
• width - width of grid in cells
• height - height of grid in cells
• origin (geometry_msgs/Pose) - the position and orientation of the grid's origin (bottom-left corner) relative to the frame in the message's header/frame_id (will be /icp_map in this task).

Below is a visualization of the indexing used in this message. The cell data of the image is a single-dimension array indexed from the bottom left and going up. In the provided template, the data is already reshaped into a numpy array.

For simplicity of debugging, we encourage you to use the following coordinate system: cell_x: the index of the cell in the X direction (red arrow, left to right in the image) and cell_y: index of the cell in the Y direction (green arrow, bottom to top). Since the numpy array is indexed as [row, column], it must then be indexed as value = occupancy_grid[cell_y, cell_x]. Also note that in this image, the grid is not rotated relative to the map frame (the origin pose rotation is zero). In your code, you should use the rotation specified in the origin pose. You can imagine the rotation as rotating the grid around the bottom-left corner of the grid, so for example if the grid is rotated by 180 degrees around the Z axis, the resulting grid coordinates would be cell_x_rotated = -cell_x, cell_y_rotated = -cell_y.

In this assignment (and basically all real-world cases), the robot is much bigger (~0.6 m radius) than a cell of the grid (~0.05 m wide). Thus, we cannot simply do pathfinding on the input grid, because the shortest paths could lead so close to obstacles that the robot would crash into them. One solution would be to check the distance to obstacles at each expanded node during the search for path, but that is computationally expensive. Instead, in this assignment, we will construct a better representation for planning. We will construct a new grid (with the same size) where each cell is either traversable (meaning the robot's center can be inside it without colliding) or untraversable, and plan on this new representation.

Since our robot is circular, obtaining the traversability grid is quite simple - you just need to inflate the obstacles (and unknown space, since it might contain obstacles) by the robot's radius. All remaining free-space cells should then be traversable. In the template files, please use the prepared get_circular_dilation_footprint() function for this, since the same one is used for evaluating the path and using a different kernel could fail the evaluation. We suggest using the following function with this kernel:

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

In the real-world, especially on flying robots, you would normally use an inflation radius that is a bit larger than the robot's true radius to avoid crashing due to, e.g., wind. But what if a robot ends up closer to the obstacles than is its inflation radius? For example, if the robot's radius is 0.2 m while using a 0.4 m footprint to inflate the obstacles and the robot gets too close to the wall due to the path following overshooting, then suddenly the robot would lie in untraversable space! And when starting in space classified as untraversible, the robot would not be able to plan anywhere or find any reachable goals, leading to it staying stuck there forever. Fortunately, this has a relatively simple solution in our case - simply use your dilation footprint to mark the space around the robot's position as traversable! This will connect the robot's starting cell with the nearby traversable space and if you use the same kernel as for inflating the obstacles, this approach should be safe (you can try drawing this on paper).

Your task is to implement A* path planning on a grid where the robot's radius is taken into account.

First, implement the map_to_grid_coordinates() and grid_to_map_coordinates() functions in aro_exploration/src/aro_exploration/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 used in the occupancy grid) and vice versa.

Then, in aro_exploration/nodes/planner.py, finish the plan_path function that returns a path as a PlanPathResponse() object. The path should fulfill the following criteria:

• the path has to be in the map frame, so after planning on the grid, transform the coordinates using grid_to_map_coordinates().
• the path must contain the start and end positions
• all points on the path must lie in safe space - meaning the robot will not intersect occupied or uknown space. For this, you have to inflate the obstacles and unknown space as described above.
• if the path starts in space that would be untraversible according to the inflated grid, you should clear the space around the starting position, as described above.
• given these constraints, the path should have the smallest possible euclidean length.
• you can move diagonally in the grid to obtain shorter and smoother paths.
• your path planning should not take longer than approx. 5 seconds in the maps given in this assignment. Hints: Use look-up grids for bools and cost values, do not store visited nodes in a list, and consider using queue.PriorityQueue
• if no path is found, the service should return a PlanPathResponse([]). This is already prepared in the template code.

Use the local evaluation script described below, it will automatically check if your solution fuliflls these criteria.

Run the script aro_exploration/homeworks/create_hw06_package.sh to create an archive containing your implementation (planner.py and utils.py) and upload it to the upload system. Path planning will be evaluated on multiple path service requests in BRUTE. Paths will be checked for the criteria specified in the assignment. Valid solutions will be awarded 1 point per path, up to 5 points total. Please also test your solution using the local evaluation script first.

For visualizing and debugging your solution, you can use the provided launch file:

roslaunch aro_exploration aro_frontier_planning_test_local.launch

This will start both the planner and frontier nodes, play a repeating rosbag containing OccupancyGrid messages, and open RVIZ which will automatically visualize any path published at the /path topic. We recommend visualizating your solution with the /plan_path_publish service, as it will do the same as the /plan_path service but also publish the path on the /path topic for visualization. You can call the path planning service from the terminal between any two positions as:

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

To evaluate your solution locally (using the same evaluation criteria as in BRUTE) on multiple paths, start the local evaluator as

rosrun aro_exploration planner_evaluate.py