Fix: Prevent infinite loop that leads to "ran out of pool" error in Astar obstacle checking

[long-tree]

-This fix replaces an opaque system crash with a clear, actionable error message, which is the correct intended behavior.

• In the while loop, the code checked checkOccupancy(Index2Coord(start_idx)) instead of checkOccupancy(Index2Coord(end_idx)) when adjusting the end point.
• This meant that even if the end point was moved outside the map (where occ = -1), the condition while (checkOccupancy(Index2Coord(end_idx)) would continue because -1 is truthy in C++ , causing endless adjustments.
• Eventually, this led to an index out of bounds during coordinate conversion Coord2Index, causing "Ran out of pool" error.

-I have tested in tracking_ws. Before the fix, the code would fail with an obscure ran out of pool, which was difficult to trace back to its root cause. After the fix, the function logging a clear message: [Astar] End point outside the map region. This fix replaces an opaque system crash with a clear, actionable error message, which is the correct intended behavior.