Intro

The Robomasters game has changed its rules again, and the implementation of the Sentry robot has gone from confined to a rail to autonomously driving around the entire game field.

Our [the ARUW team] previous implementation of the Sentry robot was required to autonomously target, aim, and launch spheres at enemy robots to deactivate them and score points. This previous Sentry robot was also required to autonomously drive back and forth, hanging from a monorail.

With the Sentry’s navigational skills moving from one dimension to two dimensions, we needed to engineer a system in order to navigate the robot from any point to any other point on the game field. This is the problem that I have been working on for the past few months, and what I wish to document here.

Generating our Static Map (to pathfind on)

Parsing 3d models of the game field into meshes

In order to generate the field meshes that we would operate on, we first needed to parse 3d models that we were given of the game field. This step is necessary to enable the use of the same algorithms and structure if the game field ever changed. This way, a new game field can be placed into the directory in the place of the old file, and will parametrically change the size of the pathfinding algorithm.

There are many libraries to do this, but we were working with .ply files, which are trivial to turn into meshes.

An outline of our code structure:

def parse_ply_into_mesh(ply_file: TextIO):
    
    # First parse the header part of the ply file by reading line by line.
    # This will get you the number of vertices, and the number of faces.

    for _ in range(num_vertices):

        # store each vertex as a tuple in a list

    for _ in range(num_faces):

        # store each face as a tuple in a list
    
    return (vertices, faces)

The one thing that is interesting about the ply and therefore our mesh format, is that each tuple in the list of faces contains the three indexes of the vertices (in the list of vertices) that represent that particular face.

Here is an example which represents a triangle:

vertices = [
    (0, 0, 0),
    (1, 0, 0),
    (0, 1, 0),
]

faces = [
    (0, 1, 2)
]

Slicing a mesh into a polyline

The next step in processing our 3d model for use with our pathfinding algorithm is slicing the 3d mesh representation into a 2d polyline. This slicing can be through of as taking a cross section of the 3d mesh at the desired z-height.

The algorithm for doing this goes as follows:

1. For each triangle in the mesh:
   1. Do any lines in the triangle exist on the slicing plane?
      • These lines are added to the polyline
   2. Do any lines intersect the slicing plane?
      • If 2 points intersect the slicing plane, the line between those two points is added to the polyline

Mapping a polyline onto a 2 dimensional grid

In order to represent the polyline we generated on the 2d grid we are using for pathfinding, we implemented Bresenham’s line drawing algorithm. This algorithm returns a list of the squares that contain any part of each line that is contained within the polyline we generated from the last step.

A great guide for implementing this algorithm can be found here on GeeksForGeeks.

Caching generated squares

The last aspect of the 3d model to usable 2d grid pipeline that we’re developing to optimize the algorithm is caching.

We would like to cache the generated squares representing the occupied squares on the 2d game field grid after each generation, so if the same input 3d model is used, the cached file could be used.

In order to implement such a system, we would need a similar file format to below:

---
x dimension: 26
y dimension: 10
grid square size: 0.1
---
5, 7
5, 6
5, 5
5, 4
4, 4

The x dimension and y dimensions represent the number of grid squares in the game field along that axis, and the grid square size represents the dimension of each square in both the x and y dimensions with the units in meters.

Pathfinding on the 2 dimensional grid

We reviewed a lot of options for pathfinding algorithms for our robot, and we settled on the A* (A Star), or D* (D Star) algorithms.

We compared the iteration speed for each iteration of the pathfinding algorithm for the previously mentioned A* and D* algorithms, as well as RRT, RRT*, and Informed RRT* algorithms. We found that while the RRT-based algorithms preformed well in some cases, with our use cases the grid-based A* and D* algorithms outperformed the RRT algorithms every single time.

We’re still working on our implementation of the D* algorithm, so it’s use is yet to be determined, but we have made some A* optimizations which allow it to approach the theoretical performance of D*. These optimizations include having both a statically and dynamically occupied characteristic for each square so that game field is only loaded once, and including the previously used dynamically located occupied squares in the input list of parameters for the algorithm.

Static Occupied Squares: the squares that occupied by the game field, generated by the input field 3d model Dynamic Occupied Squares: the squares that are seen as occupied by the robot’s vision system