The core of the code utilizes the STA* algorithm, an extension of the A* algorithm, adapted for space-time grids. Within this algorithm, an open list and a closed list are maintained, with each node in these lists representing a tuple of (x, y, t) coordinates within the space-time grid.
If the STA* fails to find a path within a predetermined timeout, a backup path is calculated from the robot's initial position to the goal position using a basic A* algorithm. Should the robot arrive earlier than the target, it will attempt to backtrack the target’s trajectory to expedite capturing.
std::priority_queue is employed to implement the open list in the A* algorithm. Each node in this queue is sorted based on its 'f' value, with the design of the priority queue intended to expedite the retrieval of the least-f value.
std::vector is utilized for implementing the bool closed list (true for every node put in CLOSED), storing the bool of trajectory path (true for every node the target visits in space-time), and storing g-values (for smaller maps, to hasten indexing value retrieval).
std::unordered_map is used to store the g values when the map size is substantial, optimizing for memory usage albeit at the cost of value retrieval speed.
Implemented within the heuristic() function, this heuristic estimates the distance from the robot’s CURRENT position to the target's FUTURE position based on the target's trajectory. It first estimates the distance the robot has already traveled to arrive at the current querying node, then uses this distance estimate to index a “Future” location of the target on its trajectory to compute and output a Euclidean distance.
This heuristic computes the Euclidean distance between two points.
This is implemented in the heristicBackward() function, which calls for the heristicAstar() function to compute and return backward g values for all nodes on a 2D map from the goal position of the target to the initial position of the robot. This method is designed to make the search more informed by providing a precomputed estimate of the cost. The heristicBackward() function also outputs a greyscale image of the heuristic estimates for debugging and visualization purposes. However, this method wasn't adopted in the final version due to its inferior performance compared to the Looking Forward Distance Heuristic implemented in the heuristic() function, likely because this table doesn't account for the time dimension, which is crucial in STA*.
Depending on the map size, the code dynamically decides between using a vector or a hash table for storing 'g' values, thereby trading off time for memory or vice versa.
A custom hash function, employing large prime numbers, is used to hash a tuple of three integers, reducing the likelihood of hash collisions and thus optimizing the performance of the hash table.
A backup A* path is pre-computed with a predetermined time-out to the final goal position as a fallback. This ensures that the robot has a greater assurance of capturing the target, even if the STA* were not able to compute an optimal path swiftly enough. If the path generated by the standard A* is not able to arrive at the goal location faster than the target, a second backup A* is used that ignores all costs below the collision threshold, with the aim of finding a faster path. This strategy aims to provide a second degree of fallback to capture the target.