Improve freespace planning performance

[NorahXiong]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I've agreed with the maintainers that I can plan this task.

Description

I've met several problems caused by hardware restrictions when testing parking function. For example, if I set costmap resolution to a small value the machine may corrupt as the memory almost fully occupied. In some complicated cases, it takes a long time to get a result and to which collision detection contributes a lot.

Purpose

To improve the performance of freespace planning module.

Possible approaches

1. Change the data structure of node cache
2. Optimize collision detection method

Definition of done

• Use unordered_map to store node data instead of vector
• Eliminate unnecessary calculation steps in collision detection function

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.