Fix: Pathing Inputs

[AskewParity]

MUST MERGE MAV UPDATES BEFORE THIS

closes #341

• airdrop pathing now starts from current plane location
• waypoint pathing starts from current plane location
  • waypoint pathing is now dynamic, with a new path generated after each lap finishes.
• adds a helper function in static to get current plane location

Testing

• run in SITL (not stress tested, just generic routes)
• passes pathing integration tests (path_planning, coverage_pathing, airdrop_approach <-- terrible naming scheme)

TODO

• Issue Dynamic Coverage Pathing #350 : coverage pathing should also by dynamic
• ticks should probably be restructured for cleaner code
• pathing performance needs to be benchmarked and optimized (rewritten lol).
• If the jetson can not make calculations quickly enough, a bandaid fix is to delete the first few waypoints in pathing under the assumption that we've already crossed them. (risk is that on a turn we have a waypoint).

[miyatakazuya]

Big thanks for fixing this. I've noticed some extraneous comments + potential bug on the path generation timeout. I didn't SITL test it yet, but should be addressed.

Also looking at the TODOs on the PR, I think the need to add a "path generation" tick is urgent. As is, if the path generation does time out and return nullptr, the isMissionFinished() may still be true because a new mission isn't uploaded yet, which would cause the lap count to decrement without the path generation succeeding.

[miyatakazuya]

The change to MissionState is great. Much better way to modify the mission.

I do think that we still have the flaw where if the path generation times out, the tick() function will just rerun since I think the LOG_F(ERROR,...) doesnt actually exit the code which breaks the numlaps logic if the MissionFinished is not updated properly which it likely is not since a mission did not get uplodaded. Again this should be addressed via a different tick, but a bandaid solution would to stop execution if the path generation finished OR have state var in the tick like is_generating_path which if true does not decrement the laps.

Or we could just work on the new tick feature, assuming the path code will not time out for now lol. Fine with either, but @kimichenn should sitl test this b/c my ubuntu docker env is acting up and i need to fix it.

[kimichenn]

I have already sitl tested it during the meeting today. Most of the things work fine except for one of the waypoints, which had a weird loop around it for the generated path. Chris said it's related to computational efficiency, which can be resolved. (Waypoint 3 below)

[AskewParity]

The change to MissionState is great. Much better way to modify the mission.

I do think that we still have the flaw where if the path generation times out, the tick() function will just rerun since I think the LOG_F(ERROR,...) doesnt actually exit the code which breaks the numlaps logic if the MissionFinished is not updated properly which it likely is not since a mission did not get uplodaded. Again this should be addressed via a different tick, but a bandaid solution would to stop execution if the path generation finished OR have state var in the tick like is_generating_path which if true does not decrement the laps.

Or we could just work on the new tick feature, assuming the path code will not time out for now lol. Fine with either, but @kimichenn should sitl test this b/c my ubuntu docker env is acting up and i need to fix it.

oops, I meant to put decrement path after it succesfully generated a new path.