feat(map): prioritize path search by sdf distance buckets

[junlinp]

Summary

• update search_within_sdf_map to use multiple SDF-priority queues
• always expand lower-SDF buckets before higher-SDF buckets
• add warning log when search_within_sdf_map returns an empty path

Motivation

• keep navigation search in safer, closer-to-trajectory regions first
• only relax into larger-SDF regions when needed

Type

• feat(map)

[github-actions]

cloc	github.com/AlDanial/cloc v 2.04 T=0.13 s (330.1 files/s, 48498.2 lines/s)

Language	files	blank	comment	code
Python	13	610	259	3399
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	30	29	191
JSON	2	8	0	58
TOML	1	4	0	57
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	930	356	4884

[dvorak0]

I can merge this, although it's tooo complex than what I expected...

I will come back with a possible better solution.