Feature/waypoints nav

[MidnightRaider06]

Waypoint manager package:

waypoint_manager_node.py: Automatically stores GPS waypoints from navigation successes and allows the rover to navigate back to them. Subscribes to /navigate_to_pose/_action/status. Whenever Nav2 completes a goal, the current /gps/fix reading is stored as a waypoint with an incrementing integer ID.

Interfaces:

• NavigateToWaypoint action (~/navigate_to_waypoint) — takes a uint32 waypoint_id, routes through navigate_to_gps → Nav2
• ClearWaypoints service (~/clear_waypoints) — wipes all stored waypoints and resets the ID counter
• /waypoint_manager/state topic — publishes full waypoint list for the GUI

For the GUI what I pictured in my head was a box that would display all the waypoints currently being stored. There could be a button for clearing waypoints that would call the ClearWaypoints service in case it ever becomes too much to keep track of and we don't need previous ones, although I don't think that's likely. Lastly there could be a text entry for the waypoint ID we want to navigate back to and a button that calls the NavigateToWaypoint action.

[mdurrani808]

Does this let us add waypoints / enter them as well?

[Rishav-N]

If we restart the nav server will this keep the waypoints?

[MidnightRaider06]

Now keeps all the waypoints by default if you restart the nav server.
Added a service for adding waypoints manually instead of only previously reached goals.