 |
AutoAPMS
Streamlining behaviors in ROS 2
|
|
AutoAPMS
Streamlining behaviors in ROS 2
|
Generic behavior tree node that publishes a PX4 VehicleCommand and waits for its acknowledgement.
More...
#include <auto_apms_px4/node/send_vehicle_command.hpp>
Public Member Functions | |
| virtual bool | setMessage (px4_msgs::msg::VehicleCommand &msg) |
Populate the VehicleCommand message to publish. | |
Protected Member Functions | |
| uint16_t | resolveSourceComponent () |
| Transitively resolve the in-charge mode executor's source component from the global blackboard. | |
 Protected Member Functions inherited from RosNodeBase | |
| void | applyPortAliasing (const BT::TreeNode *node) const |
| Support the node manifest 'port_alias' feature: copy the aliased port values onto the original ports the node implementation reads. | |
| template<typename InstanceT> | |
| std::shared_ptr< InstanceT > | getSharedEntity (const std::string &entity_name, const std::function< std::shared_ptr< InstanceT >()> &factory) |
Retrieve a process-wide shared ROS 2 entity, creating it via factory on first use. | |
Generic behavior tree node that publishes a PX4 VehicleCommand and waits for its acknowledgement.
This is the common building block for all command-based PX4 interactions. It exposes the raw MAVLink-style VehicleCommand fields (command id and param1..param7) as input ports so that any command - switching the flight mode, arming/disarming, starting a mission, triggering a servo, ... - can be represented in a behavior tree without a dedicated C++ node. Purpose-built nodes (e.g. SendCmdSetNavState) specialize this node by fixing the command id (and possibly some parameters) and exposing a friendlier port vocabulary.
It derives from the generic RosActionNodeBase and runs its own two-phase state machine in tick(): on the first tick (status IDLE) it publishes the command and starts waiting; on every subsequent tick (status RUNNING) it checks for PX4's reply on fmu/out/vehicle_command_ack. It returns SUCCESS once an acknowledgement for this command is received with result ACCEPTED (or IN_PROGRESS), and FAILURE if the command is rejected/denied or no acknowledgement arrives within the timeout (derived from the registration option request_timeout). This validation is built into the node so that a silently rejected command fails the behavior instead of leaving a downstream wait loop hanging forever.
To respect the ownership of an in-charge mode executor, the command's source_component is resolved from the global blackboard entry an active auto_apms_px4::BehaviorModeExecutor publishes. If it is undefined (behavior run standalone, no executor in charge), the command is sent with the default source component (0) and the blackboard is left untouched.
The command topic is chosen from the global blackboard: while a BehaviorModeExecutor is in charge (it sets a boolean flag), commands are published on the mode-executor command topic (fmu/in/vehicle_command_mode_executor); otherwise the default external command topic (fmu/in/vehicle_command) is used. A manifest-configured topic overrides this default. The message version suffix is appended at runtime in either case.
Definition at line 61 of file send_vehicle_command.hpp.
|
virtual |
Populate the VehicleCommand message to publish.
Specializations override this to fix the command id and expose friendlier ports. Return false to abort, in which case the node returns FAILURE without publishing.
Definition at line 146 of file send_vehicle_command.cpp.
|
protected |
Transitively resolve the in-charge mode executor's source component from the global blackboard.
VehicleCommand source component, or 0 if no executor published one (normal external command). Definition at line 176 of file send_vehicle_command.cpp.
1.13.2