Set Target Point
Measure USV’s location
Measure USV’s velocity
Set ,
for to   do
 Measure
if     then
  Get a New Waypoint
else
  for   to   do
   for   to   do
    Calculate
   end for
  end for
  for  each node   do
   Calculate
   if     then
    
   else
    
   end if
   Calculate
  end for
  Find
  Set
  Update
end if
end for
Algorithm 1: Planner pseudocode.