Add target waypoint to Rviz dynamic waypoint publisher

[JMoore5353]

The visualization currently plots all waypoints and numbers them. Sometimes it is hard to know what the target waypoint is. Add something to the visualizer to know what the next waypoint is.

Also maybe plot the orbit?

[JMoore5353]

The visualization now has an arrow marker pointing to the target waypoint.

A TODO still left is to plot the orbit path when flying an orbit.

[JMoore5353]

@iandareid I have some thoughts about how to implement plotting the orbit.

1. We could have the path_manager call a service in the rviz_waypoint_publisher whenever the orbit_last parameter is changed.
   • Pros:
     • The orbit will be plotted or removed immediately following the parameter change. Low latency.
     • Only the exact number of parameter changes or service calls are made. No excess calls.
   • Cons:
     • Path manager will have to query to make sure GCS is running
     • Less separation in the code between GCS visualizer and path manager
2. rviz_waypoint_publisher queries path_manager's orbit_last param at a certain interval, or whenever waypoint indices are incremented.
   • Pros:
     • Separation of code. No dependence on the GCS visualizer in the path_manager
   • Cons:
     • Orbit marker won't update immediately following a orbit_last parameter change
     • Orbit marker won't appear at all sometimes (if the orbit_last parameter is changed at just the wrong time)
     • Many unnecessary parameter queries or service calls.

What are your thoughts?

It is also worth noting that the path_manager already has some "contamination", meaning that it has to publish a target waypoint for the visualizer. However, this doesn't rely on or look for a specific node in order to call a service.

[iandareid]

@JMoore5353 I think that having them as separate as possible is better. Another factor that may influence your decision:

• It would be useful to see the orbit that it is following through the fillets as well.
• Whenever the aircraft follows an orbit, /current_path publishes the center of the orbit and the radius.
  • We could just extract this data and only plot the orbit as it is following it. This would include orbit last.
    • This would have the downside of not visualizing the paths before or after they are actively being followed, but it would be simple and give us the behavior we would like.

So in short maybe there is a third option? I think either of the things you propose would be great, but I would suggest that the less decoupled version may be better.

[JMoore5353]

That's great, I like that option better. We would only see the orbit / fillet when the aircraft is following that part of the path, but I think that is acceptable.