Update Waypoint Marker

[Jared-Chan]

Update the waypoint_marker node so that it works better with our control system.

The waypoint marker sends the desired pose to our control system, and determines whether the waypoint has been reached.

There are two types of waypoints: stabilize waypoints and passthrough waypoints. More information about them are in our wiki page.

We want our waypoint marker to be able to

• Set a desired pose in the map frame based on an input pose in the body frame, and publish the desired pose for the control system
• Determine if a waypoint has been achieved based on success criteria specified along with the input pose

The updated waypoint marker may contain the following subscribers and publishers:

• A subscriber to the AUV's current pose (controls/ukf/odometry/filtered, (when issue 115 is merged with master))
  • This allows the waypoint marker to calculate the desired pose in the map frame
  • It also lets the waypoint marker determine whether a waypoint has been reached
• A subscriber for setting a new waypoint
  • The input message should contain
  • desired pose in the body frame
  • waypoint type: stabilize or passthrough
  • success criteria: threshold distance and threshold
• A publisher
  • The output message should contain
  • the desired pose in the map frame
  • whether the waypoint has been achieved

New custom messages may be necessary. They should be added to triton_interfaces.

After a waypoint is achieved and before a new waypoint is set, the AUV should stay still, i.e. stabilize at the current pose.