[✨FEAT✨]: Efficient coneMap.msg data structure

[dyu056]

Is there an existing enhancement request for this?

• I have searched the existing feature requests

Description

The simulation shows that the current cone map node and path planning node can not catchup the speed of the car after there are large number of cones recorded in the ConeMap message we are using for path planning and localization. Currently it is O(nlog(n)) complex to order the cones from the coneMap msg. It is desired that the trajectory planning node is just able to grab a sub set of the pre sorted cones in constant time.
Please produce a improved data collection to store cones in an array format and provide functions for building, appending and moving cones within that structure. Change the coneMap message appropriately if applicable.
The structure is to be used by the Trajectory Planning node the Cone Mapping node is responsible for creating and manipulating the data structure.

Additional Information

No response