Develop move_to_position Action

[charlesmadjeri]

Description:

Implement the move_to_position action to manage the movement of the USV to specified GPS position.

Tasks:

• Define action interface for move_to_position
• Implement requests handling.
• Call path_planner service to get the path to follow (list of waypoints)
• Publish list of waypoints on /usv/guidance/waypoints for guidance_node.
• If guidance node publish deviation on /usv/guidance/deviation_detected, call again path_planner and publish new waypoints to guidance node.
• Call stabilize at position service with final waypoint position when arrived.
• Provide real-time feedback on navigation status (planning, progression, stabilization, stabilized) and when stabilized, action completion.