[Feature] Support custom action waypoints #18506
Closed
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
…cess a custom action The custom action is to be processed by an entity external to the FMU.
A custom action waypoint is an action defined in a waypoint that is scripted and processed outside the flight controller, usually in a mission computer process. The ability to process that action in the mission computer should be coded by the user, or instead using the MAVSDK custom_action plugin implementation (a full-scale example can be seen in the integration tests directory). The custom action command associated, for now, is the MAV_CMD_WAYPOINT_USER_1 but will eventually be replaced with an appropriate MAV_CMD to the purpose. The modifications in the navigator module allow to block the navigator for an amount of time defined on param3 of the MAV_CMD_WAYPONT_USER_1 mission item. The navigator expects to receive a progress status of the action being process, which in the case of not received, will result on the mission continuing to the next waypoint, and a COMMAND_CANCEL to be broadcasted to cancel that same command. The mission will continue to the next waypoint as soon as an ACCEPTED ACK gets sent from the component processing the custom action.
This allows to send command cancelation messages through the offboard link.
dagar
reviewed
Oct 25, 2021
| custom_action_cmd.source_system, | ||
| custom_action_cmd.source_component); | ||
|
|
||
| _cmd_pub.publish(custom_action_cmd); |
There was a problem hiding this comment.
Suggested change
| _cmd_pub.publish(custom_action_cmd); | |
| custom_action_cmd.timestamp = hrt_absolute_time(); | |
| _cmd_pub.publish(custom_action_cmd); |
Put a fresh timestamp on before publication in case the sequencing of events is ever in question from the perspective of a log review or replay.
2 tasks
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Describe problem solved by this pull request
A user might want to run an action in a waypoint that is not controlled by the navigator, but rather by the mission computer in a specified waypoint.
A custom action on a waypoint is an action that is determined by a system processing a navigation behavior which metadata is stored and processed in that system in specific. An example of it can be a planner in the companion computer that processes a simple action as triggering a servo, or a set of commands, split to be executed in a specific time frame, which then result in a more complex action.
The action is not standardized or handled by MAVLink and its microservices. The action is triggered by the autopilot with a command that gets sent to the onboard/mission computer, and the mission computer then handles the execution of that same action. The process of this action is user implementation specific, but we already provide a way of handling it with a lot of flexibility through the custom_action MAVSDK plugin: mavlink/MAVSDK#1581.
Describe your solution
MAV_CMD_WAYPOINT_USER_1is processed as a mission item command that triggers a custom action in the system that is capable of processing it. Although not ideal, this is an intermediate step to proof a functional implementation, already in production in the Auterion Skynode, which can allow us to enable mavlink/mavlink#1516 again, and then standardize its usage.While the action is being processed, the
navigatoris blocked while receiving command IN_PROGRESS ACKs with the process of the action at a steady rate. If by any means those acks do not get received, the navigator broadcasts a COMMAND_CANCEL to theMAV_CMD_WAYPOINT_USER_1command and continues the mission. The action is complete when an ACCEPTED ACK is received by the FMU to the command, meaning that the action is complete, and so thenavigatorcan move to the next waypoint.The following sequence diagram exposes the functionality:
Describe possible alternatives
There can improvements to the approach so I am looking for feedback on improvements.
Test data / coverage
This can be tested locally with PX4 SITL together with the MAVSDK integration tests available in this PR:
This implementation is also already being used in production on the Auterion SW stack.
Additional context
Here's a video showing the capability with with Auterion Software stack and PX4 SITL: https://www.youtube.com/watch?v=dbiO7FTCk10