Non-successive way points generated by A-star algorithm in freespace planner

[NorahXiong]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I'm convinced that this is not my fault but a bug.

Description

Way points generated by A-star algorithm are not successive as the red circle shows. It could result in poor trajectory following performance.

Expected behavior

Way points generated by A-star algorithm are successive. Here successive means the vehicle can move from the former point to the next point through a movement in the transition_table_. Possible transitions are defined as below.
AstarSearch::TransitionTable createTransitionTable function

Actual behavior

Way points generated by A-star algorithm are not successive.

Steps to reproduce

1. Start autoware planning simulator using default
2. Set pose estimate and goal pose the parking lot

(These steps can not guarantee the problem will repeat. According to my experience, there is a higher probability if the number of arcs in ReesShepp solution is large.)

Versions

• OS: Ubuntu 20.04
• ROS2: Galactic
• Autoware: Autoware.universe:main

Possible causes

According to the transition table design, two consecutive points should be like the blue arrows. The yellow ones(shift_theta greater than dθ) and the green ones(can not reach the next point precisely through any of the transition types) should not have been appeared.

If the preceding node changed, the nodes behind should recheck the transition relation and recalculate the actual cost or just break the connection. But these steps are not implemented now. So if a node in the closed list is rewritten during the search, and its child node stays the same, the output may be as the green arrows show.

For example. In the following picture, S is the start point, G is the goal point and P(P*) is the non-successive point. At first, nodes P* is in the closed list and its child D is in the open list. Then point C is chosen from the open list and node P is pushed into the open list (again) as a child of C. Actually P and P* are the same node in search space cause they are in the same grid and have the same orientation. In other words, P and P* use the same part of memory and share the same index in search space. Even though P is not totally equal to P*, D keeps pointing to P/P*. If D is chosen from the open list, the problem may appear as S-C-P-D-G has a chance to be selected as the best way. In that case, we will get a non-successive way point array as the figure shows.

Additional context

No response

[mitsudome-r]

@TakaHoribe Could you assign someone from Planning team to take a look at this?

[NorahXiong]

The bug is caused by queuing a node more than once and modifying the node elements for different queues. Such operation results in incorrect data association and exploded when retrieving a queue to generate the final path. It can be fixed by creating an new node instance for data storage every time the node is queued. Another strategy is just to forbid a node to be queued second time just as the implementation in ROS-Nav2.

I tried to fix it by the first method but the node data storage implementation needs alteration as we don't know exactly how many times will a node be queued. I simply substituted the pre-allocation by instant-allocation and the memory consumption obviously decreased and meanwhile the time consumption obviously increased. In general, the performance is not very good. Maybe some other container such as map could improve the instant-allocation performance.

So instead I used the second method with reference to ROS-Nav2 implementation even though it will block some possible paths. I tested several cases after the fix and it works for them all. Maybe the tiny constraint of search space has little influence on the result because the visit to search space is originally very sparse in most cases. In ROS-Nav2 implementation, some tricks as AnalyticExpansion are composed into the main algorithm and I think it could benefit the algorithm to some extent.

If there are better solutions, please let me know.