 |
AutoAPMS
Streamlining behaviors in ROS 2
|
|
AutoAPMS
Streamlining behaviors in ROS 2
|
PX4 mode executor that runs an AutoAPMS behavior in-process when put in charge by the FMU. More...
#include <auto_apms_px4/behavior_mode_executor.hpp>
Public Types | |
| enum class | CompletionReaction { HOLD , RTL , LAND , DISARM , COMPLETE , NONE } |
| Reaction performed when a behavior terminates or fails, using the in-charge executor API. More... | |
Public Member Functions | |
| BehaviorModeExecutor (BehaviorOwnedMode &owned_mode, const px4_ros2::ModeExecutorBase::Settings &settings, std::function< Config()> config_provider, BehaviorSpec initial_spec, auto_apms_behavior_tree::GenericTreeExecutorNode &engine) | |
| Constructor. | |
| void | prepareTree () |
| Build the current behavior tree and keep it detached, ready to be executed on the next activation. | |
| bool | setBehavior (const BehaviorSpec &candidate, std::string &message) |
| Verify a candidate behavior and, if valid, make it the current behavior. | |
| bool | requestAndVerifyActivation (std::chrono::milliseconds timeout=DEFAULT_ACTIVATION_TIMEOUT) |
| Request activation of the owned mode from ROS 2 and wait until it is confirmed in charge. | |
| void | onExecutionResult (ExecutionResult result) |
| Handle the termination of the behavior running on the in-process executor. | |
Static Public Member Functions | |
| static CompletionReaction | reactionFromString (const std::string &str) |
| Parse a string into a CompletionReaction. | |
Static Public Attributes | |
| static constexpr std::chrono::milliseconds | DEFAULT_ACTIVATION_TIMEOUT {5000} |
| Default time to wait for the owned mode's nav_state to become active in requestAndVerifyActivation(). | |
PX4 mode executor that runs an AutoAPMS behavior in-process when put in charge by the FMU.
The behavior tree is built ahead of time and kept detached (see prepareTree): the potentially expensive tree construction (which instantiates the ROS 2 waitables of the behavior tree nodes) happens once when the node is constructed and again after every run, off the activation path. When the owned mode is selected (RC switch, GCS or ActivateImmediately), PX4 puts this executor in charge and onActivate() hands the pre-built tree to the in-process executor (engine) via startExecution, so activation only recreates the lightweight execution timer and starts ticking. A pilot override or failsafe triggers onDeactivate(), which terminates the behavior. When the behavior finishes, a configurable in-charge reaction (hold/RTL/land/disarm/complete/none) is performed and a fresh tree is prepared for the next activation.
The behavior itself is built from standard AutoAPMS nodes and drives the vehicle through the regular PX4 skills. Before starting, the executor publishes its VehicleCommand source component on the global blackboard (under the key configured via the AUTO_APMS_PX4_SOURCE_COMPONENT_GLOBAL_KEY compile definition) so that ownership-aware nodes (e.g. SendCmdSetNavState) can attribute their commands to the executor.
Definition at line 102 of file behavior_mode_executor.hpp.
|
strong |
Reaction performed when a behavior terminates or fails, using the in-charge executor API.
Definition at line 108 of file behavior_mode_executor.hpp.
| BehaviorModeExecutor | ( | BehaviorOwnedMode & | owned_mode, |
| const px4_ros2::ModeExecutorBase::Settings & | settings, | ||
| std::function< Config()> | config_provider, | ||
| BehaviorSpec | initial_spec, | ||
| auto_apms_behavior_tree::GenericTreeExecutorNode & | engine ) |
Constructor.
| owned_mode | Placeholder mode owned by this executor. |
| settings | PX4 mode executor settings (activation policy). |
| config_provider | Callable returning the current Config (completion reactions and failsafe deferral). It is invoked at the start of every activation, so runtime parameter changes take effect on the next run. It must stay valid for the lifetime of this executor and is expected to keep any underlying parameter listener alive. The behavior to run is not part of this config; it is held internally and changed via setBehavior(). |
| initial_spec | Behavior to run initially (seeded from the node's read-only behavior parameters). |
| engine | In-process behavior tree executor used to run the behavior. |
Definition at line 50 of file behavior_mode_executor.cpp.
| void prepareTree | ( | ) |
Build the current behavior tree and keep it detached, ready to be executed on the next activation.
Builds the tree for the currently set behavior (see setBehavior) via the in-process executor's makeTreeConstructor. This is the expensive step (it instantiates the ROS 2 waitables of the behavior tree nodes) and is intentionally kept off the activation path: it is called once when the node is constructed (before FMU registration, so a build failure prevents the mode from being registered) and again after every run.
| std::exception | if the tree cannot be built (e.g. an invalid build request). |
Definition at line 185 of file behavior_mode_executor.cpp.
| bool setBehavior | ( | const BehaviorSpec & | candidate, |
| std::string & | message ) |
Verify a candidate behavior and, if valid, make it the current behavior.
Builds the candidate behavior's tree exactly as prepareTree() does. Building validates the tree structure and instantiates its nodes, so a malformed or unresolvable behavior fails here. On success the freshly built tree and the candidate spec are latched atomically (they become what the next activation runs); on failure the previously prepared tree and spec are left untouched.
| candidate | Behavior to verify and set. | |
| [out] | message | Diagnostic detail on failure (the build error). |
true if the candidate was verified and set, false otherwise. Definition at line 192 of file behavior_mode_executor.cpp.
| bool requestAndVerifyActivation | ( | std::chrono::milliseconds | timeout = DEFAULT_ACTIVATION_TIMEOUT | ) |
Request activation of the owned mode from ROS 2 and wait until it is confirmed in charge.
Commands the FMU to switch to the owned mode and blocks until the vehicle reports the owned mode's nav_state as active (i.e. this executor has been put in charge and onActivate() has run the prepared tree). Whether activation is granted depends on the configured activation policy and the vehicle state (e.g. it must be armed under the armed policy).
| timeout | Maximum time to wait for the owned mode to become active. |
true if the owned mode became active within timeout, false otherwise. Definition at line 213 of file behavior_mode_executor.cpp.
|
static |
Parse a string into a CompletionReaction.
| std::invalid_argument | if the value is not a valid reaction. |
Definition at line 89 of file behavior_mode_executor.cpp.
| void onExecutionResult | ( | ExecutionResult | result | ) |
Handle the termination of the behavior running on the in-process executor.
Translates the tree execution result into a px4_ros2::Result and performs the configured completion reaction. Intended to be called from the owning node's TreeExecutorBase::onTermination override.
| result | Final result of the tree execution. |
Definition at line 65 of file behavior_mode_executor.cpp.
|
staticconstexpr |
Default time to wait for the owned mode's nav_state to become active in requestAndVerifyActivation().
Definition at line 126 of file behavior_mode_executor.hpp.
1.13.2