@Jetson-Nano Unfortunately it is not yet fully working, as I said I managed to create plans with waypoints and to fly those plans we are currently launching the plans on a waypoint to waypoint basis so, no full trajectory without stopping (we don't have to click on the waypoints but the drone stops to plan the next strecht). We have done so because we don't trust the VIO to be able to mantain a correct pose for the long trajectory through the waypoints and we "stop" at the waypoints (where we placed april tags) to relocate and replan with a corrected position after a flight between waypoints.
If you trust VIO to follow your planned trajectory it is possible. We have modified the planTo function from mapper to localize the closest waypoint to the clicked target and we launch global_planner_->createPlan(current_start, final_target, waypoints_); iteratively from way point to waypoint without clearing the waypoints_ vector until it reaches the last waypoint and then plan to the actual target. This creates a long trajectory that should be followed when you press follow button. As I said we have never tried to follow the long trajectory, but as far as I understand the drone should follow it given that qvio maintains a good enough pose.
Right now we are experiencing stability issues when switching from position mode to trajectory mode. If we manage to solve that I can run a test with the long trajectory and get back to you.
