Proposal: Integrating Behavior Trees with ArduPilot-ROS

[snktshrma]

Hi there,
This proposal outlines the plan to introduce Behavior Tree (BT) support to ArduPilot using ROS2 AP-DDS interface. @Ryanf55 suggested this framework to allow users to have a modular and flexible interface. This is inspired by how Nav2 in ROS 2 uses BTs to handle planning, failsafes, and advanced integrations.

Overview

The idea is to use BTs to manage tasks like planning, failsafes, and mission execution in a more intuitive way. This would also simplify integrating advanced features like SLAM modules, depth estimators, external planners and even external systems like Nav2. It will also make it easy to allow users integrate existing features like cartographer SLAM to their systems and have flexible control over the system. Users would benefit from a drag-and-drop approach to behavior design, using tools like Groot to build and debug BTs visually.

Here’s the rough plan:

• Start with some groundwork; explore BehaviorTree.CPP or similar libraries and experiment with a basic integration and controlling the vehicle.
• Create BT nodes for common ArduPilot tasks like waypoints, failsafe triggers, or high-level planning.
• Integrate with Groot so users can visually design and debug trees.
• Test with real-world scenarios (e.g., mission planning or obstacle avoidance, etc).

This is just the starting point, and I'm happy to refine or adjust the plan based on feedback.

Looking forward to your feedbacks!

[Ryanf55]

Looks like a good start. It would be great if you could find other open source examples of behavior trees in use beyond nav2 so we can compare to!

[snktshrma]

Hi! I’ve had some time to work further on this and have implemented some boilerplate scripts and base-level functionality in the ardupilot_bt repo.

I attempted to make a PR, but the repository needs restructuring to accommodate multiple packages, such as one for Cartographer, one for behavior trees, etc., under ardupilot_ros.

Additionally, we need better DDS support to incorporate topics and services for features like vehicle state (armed, mode, etc.), similar to what MAVROS offers.

For the next steps, I’ll work on adding more interfaces and topics/services to AP_DDS, specifically targeted towards behavior tree support.

I’d really appreciate your suggestions, @Ryanf55.

[Ryanf55]

Nice!

A PR to restructure would be fine using git mv so we can add new packages. ardupilot_ros can be a metapackage that depends on all the others. Start with that and then we can pull in your bt work?

All the things you listed are already supported.

• Arm State: https://github.com/ArduPilot/ardupilot/blob/master/Tools/ros2/ardupilot_msgs/msg/Status.msg#L18
• Current mode: https://github.com/ArduPilot/ardupilot/blob/master/Tools/ros2/ardupilot_msgs/msg/Status.msg#L19

You can also arm with the ArmMotors.srv and change modes with the ModeSwitch.srv