Waypoint Markers

Introduction

Waypoint markers are how we will tell our control system where our desired location is and how we are supposed to achieve this. They are implemented in the triton_controls package, with associated interfaces being defined in triton_interfaces.

Terminology

• waypoint marker - A ROS2 component node which defines a 3D pose and success criteria for reaching this pose
• waypoint sequence manager - A ROS2 component node which is responsible for controlling the order of a set of waypoint markers
• pipeline type - A set of nodes which aim to complete a certain task
• controls - The act of receiving feedback (i.e from AUV sensors) to influence the behaviour of a system (i.e AUV reaches a certain pose)
• stabilize waypoint type - A type of waypoint where the AUV is supposed to stabilize to a certain pose (i.e velocity = 0)
• passthrough waypoint type - A type of waypoint the AUV is supposed to pass through a certain pose (i.e velocity isn't a concern)

Background

See this page to understand the role of the pipeline and how pipeline types work.

Detailed Description

We can use waypoint markers to determine where we want our AUV to be, and how we want this to happen. The waypoint marker component will be responsible for sending a control input and control strategy to our control system, and also determining whether or not the waypoint has been reached or not.

Waypoint Types

First, we define two types of waypoints, let's first consider the stabilize waypoint type. Consider a case where we want our AUV to localize to a certain point (for example, in front of a buoy). The diagram below shows how we would set a stabilize waypoint to achieve this

We will use these waypoints when we want our velocity to be 0 for all DOF when we reach the waypoint's pose. To do this, we will send the waypoint's pose as control input (i.e on some topic controls/input) and tell the control system to use a stabilize strategy (how this will be done is out of scope for this documentation). The waypoint marker component will then be responsible for listening to the AUV's state, and determining whether or not the waypoint has been reached (more on this later). Now lets define the passthrough waypoint type, consider a case where we want our AUV to pass through a point in space (for example, through the floating gate). The diagram shows how we would set a passthrough waypoint to achieve this

We will use these waypoints when the AUV's velocity when it passes through the waypoint's pose has no restrictions. To do this, just like the stabilize waypoints we will send the pose as control input, and tell the control system to use a passthrough strategy.

Waypoint Success Criteria

It may seem like waypoint markers are redundant given the control system (as both are in a way responsible for controlling where the AUV is), however waypoint markers implement higher level logic for AUV positioning. This is shown in the success criteria, which is different for the different waypoint types. For the stabilize waypoint type, the success criteria is when the AUV's state is maintained within a threshold bubble for a certain time (this might be anywhere from a second to a minute). For the passthrough waypoint type, the success criteria is very similar, except the AUV's state simply has to be within the threshold bubble for it to be a success (this might benefit from a smaller threshold bubble). The threshold bubble is simply a sphere of some constant radius which surrounds the waypoint, as shown below

Using Waypoints in the Pipeline

The waypoint marker and waypoint sequence manager component nodes can be added to any pipeline type. You can use the waypoint marker node by itself if you want to dynamically set the waypoints (the node will expose a service for setting waypoints), or you can use it in conjunction with the waypoint sequence manager if you have a predefined set of waypoints. As a concrete example of using both of the components, consider a pipeline type called triangle pipeline, whose task is to make the AUV travel in a triangular path and return to its starting pose. The diagram below illustrates how the pipeline type operates (fig. 1) and what the waypoints would look like (fig. 2)

Note that we have seperate stabilise waypoints to arrive at a vertex of the triangle and to rotate to face the next vertex. This is an optional way to move in a triangular motion, but we see it is really easy to implement using the waypoint sequence manager. This style of creating pipelines (we see we would be able to create ones for different shapes, circle, square etc...) would be primarily useful for testing, as the waypoints would have to be hardcoded (i.e if waypoint 1 is at the origin, then waypoint 2 might be at position [5,0,0]). In competition pipeline types, the waypoint marker would be fed values from some other component. For example in the gate task, a gate pose estimation component would send the estimated pose as a passthrough waypoint to the waypoint marker, and then would wait to be notified of waypoint success before notifying the pipeline of success.