I have implemented two versions of the mission planner. Both use the same planners for Goto and Coverage paths. An upcoming post describes this in detail.

The first mission planner predetermines the sequence of targets and goal safepoint before planning Gotos. This is done heuristically. The target order is selected, then the goto paths are planned between them. If the path is infeasible, steps are taken to fix the path by removing the needed targets that least harms the reward.

It is possible that such a system leads to a suboptimal sequence. Thus, a fully connected TSP-style implementation has also been made. The Goto between all nodes is planned to build a reward and cost matrix to use for TSP.

This image shows two feasible paths generated. The red has greater reward with higher work cost than the blue.