test(nav): add shortcut pruning for map path

[xiaolefang-dm]

Summary

• Add conservative 3D shortcut pruning for generated map navigation paths
• Keep shortcuts local and bounded by segment length / skipped nodes to avoid overly sparse global plans
• Reject shortcuts through occupied, unknown/invalid SDF, or cells too far from the odom/SDF corridor
• Return early with a warning if start/goal cannot connect to the SDF corridor

Notes

This runs after the existing 3D SDF path search and only removes small zig-zags when a straight segment is verified safe in the 3D voxel map.

before:

after:

[github-actions]

cloc	github.com/AlDanial/cloc v 2.04 T=0.17 s (252.6 files/s, 38166.1 lines/s)

Language	files	blank	comment	code
Python	13	626	271	3529
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	31	32	203
JSON	2	8	0	58
TOML	1	4	0	58
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	947	371	5027

[junlinp]

How about using line-of-sight at the search?

[github-actions]

cloc	github.com/AlDanial/cloc v 2.04 T=0.18 s (238.5 files/s, 36076.1 lines/s)

Language	files	blank	comment	code
Python	13	628	273	3533
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	31	32	203
JSON	2	8	0	58
TOML	1	4	0	58
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	949	373	5031