2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.241202
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:30.476 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:30.486 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:30.486 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:30.486 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:30.486 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:30.486 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:30.486 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:30.486 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:30.486 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:30.486 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:30.486 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:30.486 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.240525
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:30.487 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0964108
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:31.893 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:31.895 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:31.895 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:31.895 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:31.895 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:31.895 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:31.895 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:31.895 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:31.895 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:31.895 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:31.895 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:31.895 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0968094
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:31.896 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.39049e-17
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0779614
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:32.139 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0876457
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:32.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.098386
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:32.591 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:32.908 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.39858e-17
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.10665
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:32.909 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0752461
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:33.121 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0671238
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:33.353 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0813951
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:33.588 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0866265
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:33.824 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.089974
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.925 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:33.925 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:33.925 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:33.925 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:33.925 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:33.925 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:33.925 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0917458
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:33.968 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0925447
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:33.996 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.022 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0938297
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:34.023 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0943877
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:34.056 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:34.062 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:34.062 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0943243
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:34.063 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0976714
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:34.295 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.525 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0972059
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:34.526 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0979149
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:34.763 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.100079
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:34.999 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:35.072 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:35.072 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:35.072 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:35.072 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:35.072 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:35.072 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:35.072 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:35.072 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:35.072 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:35.072 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.10244
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:35.073 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.103093
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:35.099 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.103602
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:35.132 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:35.433 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:35.433 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:35.433 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:35.433 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:35.433 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:35.433 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:35.433 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:35.433 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:35.433 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:35.433 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:35.433 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0876376
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:35.434 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0778932
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:35.663 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0881256
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:35.889 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0900818
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:36.117 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:36.150 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:36.150 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.091067
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:36.151 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0920997
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:36.184 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:36.209 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:36.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:36.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:36.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0929913
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:36.210 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.093218
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:36.238 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0941026
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:36.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:36.261 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.261 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.261 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:36.261 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.261 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.261 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:36.261 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:36.261 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:36.261 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:36.261 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:36.261 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:36.261 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0940589
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:36.294 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0938839
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:36.318 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0958223
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.78097e-17
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0951277
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:36.357 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:36.357 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0953276
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:36.603 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0928313
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:36.832 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0957753
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.059 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:37.060 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:37.060 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:37.060 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:37.060 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:37.060 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:37.060 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0980723
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:37.292 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.78097e-17
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.100038
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:37.371 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.099651
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:37.400 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0985257
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:37.433 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0988562
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:37.462 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:37.526 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0986464
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0981641
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.526 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:37.527 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0974491
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:37.757 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.78097e-17
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0988747
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:37.986 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0971577
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:38.232 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0974769
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:38.464 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:38.539 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0992026
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:38.540 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.78097e-17
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0987225
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:38.566 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0982221
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:38.596 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0978297
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:38.619 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0996647
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0996359
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:38.696 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:38.696 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.100366
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:38.927 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:39.253 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0990904
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:39.254 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0897485
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:39.496 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0982294
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:39.709 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0991541
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:39.726 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0991913
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:39.738 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:39.739 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:39.739 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:39.739 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:39.739 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:39.739 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:39.739 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0927927
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:40.069 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.091657
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:40.296 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.77691e-17
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0952415
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:40.527 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.78097e-17
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0961805
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:40.752 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.097821
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:40.761 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.779 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.78097e-17
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0965159
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:40.780 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0976085
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:40.809 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0974942
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:40.839 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0984171
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:40.868 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0986485
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:40.999 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:41.000 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:41.000 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:41.000 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:41.000 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:41.000 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:41.000 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:41.000 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.100588
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:41.247 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0913124
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:41.584 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0892215
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:41.797 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0934131
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:41.894 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0969365
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:42.034 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.100276
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:42.261 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.77691e-17
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0988075
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:42.493 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:42.910 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:42.910 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:42.910 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:42.910 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0864926
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:42.911 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:42.928 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:42.928 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:42.928 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:42.928 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:42.928 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:42.928 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:42.928 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:42.928 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:42.928 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0852785
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:42.929 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0794082
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:42.938 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0738502
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:43.167 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0850786
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:43.390 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0902573
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:43.628 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0948821
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:43.853 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0939495
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:43.951 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0926154
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:44.087 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:44.311 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0976041
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:44.312 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:44.313 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:44.313 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0955228
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:44.534 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:44.535 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:44.535 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:44.535 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:44.535 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:44.535 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:44.535 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:44.535 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.096861
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:44.763 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0951065
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:44.964 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0934764
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:44.990 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.77691e-17
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0977344
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:45.218 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0985295
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:45.453 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0991758
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:45.674 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.101599
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:45.980 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0822002
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:46.124 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0719195
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:46.379 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0814049
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:46.608 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0873364
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:46.834 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.39049e-17
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.087627
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:46.994 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0880499
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.028 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.029 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.029 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.029 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.029 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.029 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.029 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.029 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.029 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.029 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0905142
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.052 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0910368
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.071 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0910251
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.080 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0906967
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.113 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0914226
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.142 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.78097e-17
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0942494
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.173 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.204 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.204 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.204 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.204 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.204 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.204 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.204 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.204 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.204 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.204 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.204 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.204 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.78097e-17
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.094128
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.205 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0944729
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.232 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0951313
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.265 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.095246
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.293 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.78097e-17
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0958179
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.319 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.320 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.320 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.320 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.320 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.320 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.320 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.320 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.320 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.320 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.320 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.320 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0967721
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.320 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0953848
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0962475
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.548 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.548 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:47.773 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:47.773 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:47.773 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:47.773 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.773 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:47.773 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:47.773 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:47.773 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:47.773 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:47.773 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.773 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.096559
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:47.774 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0996034
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:48.009 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0989356
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:48.232 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.77691e-17
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.100277
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:48.476 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.77691e-17
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0981442
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.560 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:48.561 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:48.561 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:48.561 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:48.561 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:48.561 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:48.561 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:48.592 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:48.592 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:48.592 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:48.592 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.592 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.592 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:48.592 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:48.592 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:48.592 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:48.592 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.592 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.592 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0981173
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:48.593 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:48.619 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:48.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0976692
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:48.620 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0979196
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:48.649 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0970357
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:48.704 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0978454
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:48.938 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:49.179 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0978263
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:49.180 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0992989
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:49.417 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0964646
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:49.650 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0965065
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:49.664 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0975689
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:49.691 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.77691e-17
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0973243
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:49.716 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0966517
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:49.761 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0977535
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:49.792 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0959351
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:49.888 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0974643
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:49.888 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:49.889 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0990079
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:50.108 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0971935
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:50.339 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0966065
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.575 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.576 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:50.576 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.576 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.576 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:50.576 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:50.576 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:50.576 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:50.576 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:50.576 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:50.576 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:50.810 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:50.810 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.099479
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:50.811 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.902 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0992657
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:50.903 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0982436
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:50.924 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0984489
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:50.953 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0990528
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:50.984 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:51.153 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0921859
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:51.154 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:51.374 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0905846
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:51.375 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.098296
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:51.600 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:51.601 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:51.601 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:51.601 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:51.601 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:51.601 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:51.601 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:51.601 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:51.601 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:51.601 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:51.601 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0975375
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:51.856 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.10034
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:52.003 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.10069
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:52.026 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0995441
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:52.052 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.100551
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:52.079 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0998547
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:52.090 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.100603
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:52.107 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.77691e-17
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0938448
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:52.434 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0936468
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:52.439 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0290676
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:53.665 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.030006
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:31:53.666 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.20641
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:39:54.260 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.206078
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:39:54.260 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:39:55.578 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:39:55.578 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0994474
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:39:55.579 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0996453
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:39:55.579 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.105036
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:39:55.812 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0908678
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:39:56.594 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.085957
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:39:56.657 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.16411e-17
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.903782
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:39:56.878 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:39:57.618 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:39:57.618 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.110032
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:39:57.619 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.78655e-17
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.936362
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:39:58.631 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.06938e-19
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0316466
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:08.836 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.34822e-20
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0311808
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:08.836 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.20112e-19
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0371398
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:09.077 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.93438e-19
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0447276
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:09.314 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.557 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.1815e-19
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0378576
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:09.558 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.997076
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:09.788 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.0061e-19
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0403166
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:09.849 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.157e-19
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0410303
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:09.874 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:6.64635e-20
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0414023
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.908 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:09.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:09.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:09.909 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:09.909 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:09.909 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:09.909 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.998927
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:09.941 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.044806
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0421794
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:13.632 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:13.633 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.040405
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:13.875 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:7.15245e-18
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0325416
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:14.129 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0331276
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:14.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0311472
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:14.596 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0308023
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:14.647 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0318496
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:14.827 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:14.828 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:14.828 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:4.21665e-19
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.952121
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:15.051 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:7.15245e-18
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0340743
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:15.671 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0218505
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:16.696 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.00739065
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:17.716 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.00688007
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:17.752 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.78272e-17
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.985303
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:17.780 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:3.50474e-19
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0286054
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:27.124 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.33585e-19
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0283091
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:27.128 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:27.129 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:27.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:27.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:27.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:27.129 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:27.129 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:27.129 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:27.129 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.05526e-19
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.051997
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:27.366 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.83486e-19
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0508147
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:27.587 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:3.2496e-20
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0469851
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:27.816 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.99757
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:28.061 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.51469e-19
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0420661
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:28.138 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:28.139 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:28.139 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:28.139 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:28.139 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:28.139 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:28.139 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:28.139 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:28.139 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:28.139 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:28.139 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:28.161 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:5.61467e-20
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0423515
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:28.162 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.27427e-19
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0407223
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:28.189 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.998594
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:28.212 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0665616
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:33.093 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0635644
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:33.099 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:33.325 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0707797
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:33.326 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.55158e-17
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.078177
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:33.550 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:33.551 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:33.551 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:33.783 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:33.783 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:33.783 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.43049e-17
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0846744
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:33.784 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0903367
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:34.029 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0902099
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:34.112 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:34.144 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0900107
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:34.145 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0889575
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:34.173 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:34.256 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0906591
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:34.257 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:3.43252e-18
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.910868
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:34.484 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.0891952
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:35.186 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:0
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.089322
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:35.209 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.69024e-17
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.913625
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:40:35.245 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:7.44652e-17
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.48081
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:02.864 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:4.3798e-17
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.48044
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:02.865 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.83511e-17
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.90934
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:09.026 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:4.84981e-17
2026-09-13 11:46:09.026 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.91018
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:09.027 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.66966e-18
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.36486
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:16.920 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:16.921 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.3917e-18
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.36408
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:16.922 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:25.121 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:7.66643e-18
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.27548
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:25.122 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.10268e-17
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.2774
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:25.123 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:5.20492e-17
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.84398
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:36.714 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:36.714 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:5.69558e-17
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.84407
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:36.715 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:5.91731e-17
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.83793
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:49.647 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.12688e-18
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.83949
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:49.651 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:9.51326e-19
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.34705
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:57.400 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:46:57.399 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:46:57.399 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:46:57.399 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:46:57.399 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:57.399 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:46:57.399 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.13107e-18
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.3415
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:46:57.400 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:8.12956e-19
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.27856
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:47:05.881 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:7.07588e-19
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.27965
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:47:05.887 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:47:43.882 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:5.60304e-19
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.09351
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:47:43.883 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.12096e-18
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.09345
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:47:43.883 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.12913e-17
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.37299
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.23928e-18
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.37273
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:47:51.700 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:47:51.700 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:3.62964e-17
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.8345
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:48:06.914 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.66869e-17
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.82975
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:48:06.914 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:5.73274e-17
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.86189
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:48:27.189 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:4.32566e-17
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.85839
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:48:27.191 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:4.99697e-18
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.35386
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:48:34.755 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.06394e-17
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.35412
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:48:34.755 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:9.28507e-18
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.28538
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:5.40281e-18
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：2.28373
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:48:42.548 [ffff997edee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:48:42.548 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:56:31.945 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.88471e-18
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：3.30116
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:56:31.946 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.74045e-17
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.915294
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:56:40.006 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:9.2358e-19
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.91953
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:56:46.938 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:7.25086e-18
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.84425
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:56:53.880 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.66705e-17
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.885942
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:57:03.934 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:7.01807e-18
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.903034
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:57:10.972 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:57:17.825 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.05673e-18
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.92364
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:57:17.826 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:5.95329e-18
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.85058
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:57:24.666 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:3.53294e-18
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.87777
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:57:35.022 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:7.46709e-18
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.895711
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:57:42.166 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:6.90551e-18
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.91716
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:57:48.801 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.09404e-18
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.84948
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:57:55.654 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.93285e-17
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.887824
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:58:05.608 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.06653e-17
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.91194
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:58:16.345 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.44203e-18
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.91851
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:58:23.078 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:4.02763e-18
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.83743
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:58:30.332 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:3.62155e-18
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.889539
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:58:40.085 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:7.45925e-18
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.899692
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:58:47.132 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:1.49392e-18
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.9237
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:58:53.865 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:4.38201e-18
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.84907
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:59:01.119 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:59:01.120 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:59:01.120 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:59:01.120 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:59:01.120 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:59:01.120 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:59:01.120 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:3.17158e-17
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.875216
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:59:11.363 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:2.55114e-17
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：0.900448
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1829] 在8888点上
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:59:18.609 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:3.01327e-18
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.91752
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:59:25.347 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1314] Starting poseNav function.
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2862] 开启随机导航.
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2910] 设置停障.
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:805] RemakeMatrix success!
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2922] 结束构造任意导航随机导航.
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1378] Performing path planning and slicing.
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2723] Starting poseNavPlanAndSlice function.
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2743] Starting path planning to target node: 9999
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2486] 提取PCD 0

2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:2457] 提取PCD 0

2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:295] 构造函数中当前点数量为:2
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:360] 构造成功！ 
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1797] 最短距离为:6.58997e-18
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1823] 距离8888点：0
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1824] 距离9999点：1.86095
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1839] 在8888点上
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1452] 狗在点上，起点为：8888
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1176] m_iCapacity in floyd 2
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1251] 输出路径为:
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1252] 8888
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:1270] ->9999
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1503] 轨迹规划成功
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:483] 查询边为:9999, total edge size:1
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphMap.cpp:489] 该边存在,id为:0
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2749] Path planning successful.
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:1889] 任务段0 类型:Normal 边数:1 节点数:2
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2765] sliceGraph successful.
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.cpp:2775] sliceGraph successful.
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1389] Serializing output JSON.
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1454] Final Output JSON: {"succeed":true,"errorCode":0,"info":"Path planning and slicing executed successfully.","data":{"track":[[8888,9999]],"edgelist":[[9999]]}}
2026-09-13 11:59:32.379 [ffff66ff0ee0] [GINFO ] [GraphManager.hpp:1456] topoNav function completed successfully.