Responsible lecturer: Vít Krátký (kratkvit@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 (red = start, green = goal) 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 (int8[])

• <0, occupancy_threshold> == empty cell
• (occupancy_threshold, infinity) == occupied cell
• -1 == unknown (unseen) cell

info (nav_msgs/msg/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/msg/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. Also, the map data contains occupancy probabilities as a number for each cell. To classify them into free/occupied, use the available parameter occupancy_threshold.

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_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 used in the occupancy grid) and vice versa.

Then, in aro_exploration/aro_exploration/planning/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 unknown space. For this, you have to inflate the obstacles and unknown space as described above,
• if the path starts in space that would be untraversable 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,
• the planning algorithm considers diagonal transitions in the grid to obtain shorter and smoother paths,
• the distance between two consequent way points in the path does not exceed 0.2 m,
• your path planning does not take longer than 10 seconds in the maps given in this assignment. Hints: Use look-up grids for booleans 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 fulfills these criteria.

Local evaluation of your solution can be run using

ros2 launch aro_exploration aro_planning_evaluation.launch.xml

This starts an evaluation script which consequently sends path planning requests with start and goals loaded from a file aro_exploration/data/planning/planning_requests.csv. The file with path definitions is the csv file with following format:

start_x,start_y,goal_x,goal_y,path_length,comment
-1.00,0.00,0.50,0.00,1.5,Standard path
1.00,1.00,-1.00,-1.00,2.828,Standard path
0.00,0.00,1.00,-1.00,1.414,Standard path
0.00,0.00,1.00,0.00,0.0,Goal unreachable (inside inflated)
3.00,0.00,-1.00,-1.00,0.0,Start outside of the map

In this file each line specifies x, y coordinates of a start position in the world frame, x, y coordinates of a goal position in the world frame, expected length of the path, and additional comment describing specificity of a given planning request. You can add or adapt 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, valid indicates whether all requirements for a given path were fulfilled, goal reached indicates whether a goal is the last way point of the path, start connected indicates whether the first point of the path is the given start position, length stands for the length of the generated path, ref. length stands for expected length of the path, column length ok indicates whether the length of the path matches the expected length, column follow time shows the time of following for individual paths, time diff indicates the difference of following time to time limit (negative numbers stand for the time below the limit, positive numbers stand for time over the limit), columns inc_too_big indicates whether the path includes too big jumps between individual positions, column safe indicates whether the planned path is collision-free.

If you want to run your implementation of the path following 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_planning_evaluation.launch.xml run_planner:=false

and then run your implementation of path planner in another window using command

ros2 launch aro_exploration aro_planning.launch.xml

If you prefer more interactive way of testing, you can run

ros2 launch aro_exploration aro_planning_evaluation.launch.xml run_evaluation:=false

, which runs your planner, visualization tools, and rosbag publishing the map, but does not send any path planning requests. For this purpose, you can run a python script aro_exploration/scripts/send_planning_request.py that allows you to repeatedly specify quadruplets of numbers defining start position and goal position that are subsequently sent as path planning request to your planner.

If you prefer to visualize and debug your solution with an evolving occupancy map, you can use the provided launch file:

ros2 launch aro_exploration aro_planning_sim.launch.xml gui:=false keyboard_teleop:=true

This will start the simulation including robot running localization, mapping modules, and “keyboard teleop” to navigate robot in the maze in order to update the map, and open Rviz which will automatically visualize any path published at the /path topic (red point = start, green point = goal). We recommend visualizing 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:

ros2 service call /plan_path_publish aro_msgs/srv/PlanPath "start:
  x: 1.0
  y: -1.0
  theta: 0.0
goal:
  x: 1.0
  y: 1.5
  theta: 0.0"

Run script aro_exploration/homeworks/create_hw06_package.sh to create an archive containing your implementation (planner.py and utils.py) and upload it to upload system. The automatic evaluation uses a set of path requests similar to paths provided in planning_requests.csv. The points obtained for the solution are proportional to number of successfully planned collision-free paths of a minimum length. Please test your solution using the local evaluation script first. The results should be equivalent.