Px4-autopilot: Incompatilibilty with MAVSDK missions

[julianoes]

Describe the bug
When running a mission with MAVSDK we now get the failure:

Distance between waypoints too close: 0 meters

To Reproduce
Run SITL.

Run MAVSDK integration test:

cmake -DCMAKE_BUILD_TYPE=Debug -Bbuild/debug -S.
cmake --build build/debug -j8
build/debug/src/integration_tests/integration_tests_runner --gtest_filter="SitlTest.MissionAddWaypointsAndFly"

[05:25:25|Info ] Arming... (mission.cpp:211)
[05:25:26|Info ] Armed. (mission.cpp:214)
[05:25:26|Info ] Starting mission. (mission.cpp:226)
[05:25:26|Debug] MAVLink: critical: Distance between waypoints too close: 0 meters (system_impl.cpp:257)
[05:25:26|Warn ] command temporarily rejected (176). (mavlink_commands.cpp:172)
/home/julianoes/src/MAVSDK/src/integration_tests/mission.cpp:230: Failure
Expected equality of these values:
  result
    Which is: Error
  Mission::Result::Success
    Which is: Success
/home/julianoes/src/MAVSDK/src/integration_tests/mission.cpp:236: Failure
Expected equality of these values:
  status
    Which is: 4-byte object <01-00 00-00>
  std::future_status::ready
    Which is: 4-byte object <00-00 00-00>
[  FAILED  ] SitlTest.MissionAddWaypointsAndFly (7255 ms)
[----------] 1 test from SitlTest (7255 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (7255 ms total)
[  PASSED  ] 0 tests.
[  FAILED  ] 1 test, listed below:
[  FAILED  ] SitlTest.MissionAddWaypointsAndFly

Expected behavior
It should fly the mission as previously.

[julianoes]

Introduced with https://github.com/PX4/Firmware/pull/11878, presumably on purpose as mentioned in: https://github.com/PX4/Firmware/pull/11878/commits/e5690b375dd435d27a573491710ff75e6e3c2005

Missions that have waypoints that are in the same physical location do not make sense and need to be rejected (the GCS / SDK generating them needs to be fixed). By enforcing this we can work with a reasonable and simpler state machine while executing the mission.

[julianoes]

The reason MAVSDK sends the same lat/lon twice in a row is because it adds a LOITER_TIME right after a NAV_WAYPOINT. The right approach would be to use NAV_DELAY but that's not implemented for PX4.

@LorenzMeier do you think we could make an exception in the MissionFeasibilityChecker to ignore loiter mission items for the distance check?

Also see:
https://github.com/mavlink/MAVSDK/blob/62fa8925d1d7e98b0dd4f167b9a319e84d16c152/src/plugins/mission/mission_impl.cpp#L355-L390

[LorenzMeier]

The core issue is that the mission geometry logic gets a lot more complex if waypoints are in the same physical location (you need to traverse the list back a lot more than a single step to find the previous location). I would have a strong preference for just adding NAV_DELAY.

[LorenzMeier]

@julianoes NAV_DELAY is supported in PX4 since quite a while (probably years). I would suggest to fix the generation of the mission in MAVSDK accordingly.

[LorenzMeier]

I'm closing this issue here and I've opened a new issue in MAVSDK and assigned @julianoes . I've provided further context of why this is not something the autopilot should support. https://github.com/mavlink/MAVSDK/issues/1101

[julianoes]

@LorenzMeier ok two points to this:

1. That's good news that NAV_DELAY is implemented. I could not tell from the code. Unfortunately, it does not work as expected: https://github.com/PX4/Firmware/issues/14909.

2. The PX4 change still breaks existing MAVSDK implementations which is something that I'm usually trying to avoid. I'm aware that this is not always possible and this might be one of these cases, however, I still would like to find a solution if somehow possible, especially since NAV_DELAY is not an option just yet until 1. is fixed.
   Edit: here is a suggestion https://github.com/PX4/Firmware/pull/14910

[dayjaby]

So it is recommended to use NAV_WAYPOINT + NAV_DELAY instead of NAV_LOITER_TIME? Sometimes I have issues with NAV_LOITER_TIME that it is not correctly setting the waypoint as finished.

[julianoes]

For now fixed by https://github.com/PX4/Firmware/pull/14921.
For the longer term we need to decide whether to add the requirement to the MAVLink spec and then enforce it in the feasibility checker.