Usage
With standard ROS 2, to enable a robot to execute a specific mission requires the user to come up with a specific design for the real-time system and develop specialized ROS 2 nodes. By adopting the flexible system architecture of AutoAPMS, the user is already provided a robust framework that helps minimizing the development effort and reduce the time to deploy your custom application. The user must consider the following topics when designing an operation:
- Define the robot's capabilities/skills and corresponding interfaces for ROS 2.
- Implement behavior tree nodes acting as clients to this functionality.
- Assemble those nodes in behavior trees to represent specific behaviors which can be deployed using ROS 2.
- Optional: Create additional behavior trees and configure a mission to react to problems occurring at runtime.
Navigate through the sections of this chapter to learn more about the intended workflow when developing skills, behaviors and missions for your application. In the following, we provide a simple example that shows you how the above mentioned steps can be achieved using AutoAPMS. Visit the tutorials page for more usage examples.
Implement a simple Skill
We're going to create a simple skill that repeatedly prints a given text to the terminal. To achieve this using AutoAPMS and ROS 2, we need to implement an action server that performs the task of writing to the terminal and a separate client that is supposed to send a message specifying the goal of the action. You could approach this development task by sticking to the official ROS 2 tutorial for writing an action. However, we'd like to show you a more streamlined and modular approach enabled by AutoAPMS.
INFO
In the following, we only show the most relevant source code rather than giving you in depth information about every little line of code that is necessary to build this example. The full source code for the simple_skill example is available on GitHub.
Action Interface
To define your ROS 2 interfaces, it's common practice to create a separate package that contains .msg, .service and .action files for topics, services and actions respectively. We name this package auto_apms_interfaces and define the following interface definition file under action/ExampleSimpleSkill.action.
# Request
string msg
uint8 n_times 1
---
# Result
float64 time_required
---
# FeedbackServer
For implementing robot skills using a ROS 2 action server, we provide the helper class ActionWrapper. The SimpleSkillServer shown below provides the functionality required by the skill we want to implement.
#include "auto_apms_interfaces/action/example_simple_skill.hpp"
#include "auto_apms_util/action_wrapper.hpp"
namespace my_namespace
{
using SimpleSkillActionType = auto_apms_interfaces::action::ExampleSimpleSkill;
class SimpleSkillServer : public auto_apms_util::ActionWrapper<SimpleSkillActionType>
{
public:
SimpleSkillServer(const rclcpp::NodeOptions & options)
: ActionWrapper("simple_skill", options) {}
// Callback invoked when a goal arrives
bool onGoalRequest(std::shared_ptr<const Goal> goal_ptr) override final
{
index_ = 1;
start_ = node_ptr_->now();
return true;
}
// Callback invoked asynchronously by the internal execution routine
Status executeGoal(
std::shared_ptr<const Goal> goal_ptr,
std::shared_ptr<Feedback> feedback_ptr,
std::shared_ptr<Result> result_ptr) override final
{
RCLCPP_INFO(node_ptr_->get_logger(), "#%i - %s", index_++, goal_ptr->msg.c_str());
if (index_ <= goal_ptr->n_times) {
return Status::RUNNING;
}
result_ptr->time_required = (node_ptr_->now() - start_).to_chrono<std::chrono::duration<double>>().count();
return Status::SUCCESS;
}
private:
uint8_t index_;
rclcpp::Time start_;
};
} // namespace my_namespace
// Register the skill as a ROS 2 component
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(my_namespace::SimpleSkillServer)All we have to do in the CMakeLists.txt of our package is to invoke this macro provided by rclcpp_components (assuming you add the server's source file to a shared library called "simple_skill_server"):
# Create shared library
add_library(simple_skill_server SHARED
"src/simple_skill_server.cpp" # Replace with your custom path
)
ament_target_dependencies(simple_skill_server
rclcpp_components
auto_apms_interfaces
auto_apms_util
)
# Register server component
rclcpp_components_register_node(simple_skill_server
PLUGIN "my_namespace::SimpleSkillServer"
EXECUTABLE "simple_skill_server"
)
# Allows you to simply start the server by running
# ros2 run <package_name> simple_skill_server# Create shared library
add_library(simple_skill_server SHARED
"src/simple_skill_server.cpp" # Replace with your custom path
)
ament_target_dependencies(simple_skill_server
rclcpp_components
auto_apms_interfaces
auto_apms_util
)
# Register server component
rclcpp_components_register_nodes(simple_skill_server
"my_namespace::SimpleSkillServer"
)
# No executable file is generated. You must manually do that
# or write a launch script that loads this ROS 2 node componentClient
Until now, we've pretty much only applied the standard ROS 2 workflow. This is about to change when we create the client for SimpleSkillServer. Very differently to what you're used to with ROS 2, the SimpleSkillClient in the following snippet does NOT inherit the interface of a typical rclcpp::Node. When using AutoAPMS, we prefer to implement clients as behavior tree nodes. In this case, it is a RosActionNode.
#include "auto_apms_interfaces/action/example_simple_skill.hpp"
#include "auto_apms_behavior_tree/node.hpp"
namespace my_namespace
{
using SimpleSkillActionType = auto_apms_interfaces::action::ExampleSimpleSkill;
class SimpleSkillClient : public auto_apms_behavior_tree::core::RosActionNode<SimpleSkillActionType>
{
public:
using RosActionNode::RosActionNode;
// We must define data ports to accept arguments
static BT::PortsList providedPorts()
{
return {BT::InputPort<std::string>("msg"),
BT::InputPort<uint8_t>("n_times")};
}
// Callback invoked to specify the action goal
bool setGoal(Goal & goal) override final
{
RCLCPP_INFO(logger_, "--- Set goal ---");
goal.msg = getInput<std::string>("msg").value();
goal.n_times = getInput<uint8_t>("n_times").value();
return true;
}
// Callback invoked when the action is finished
BT::NodeStatus onResultReceived(const WrappedResult & result) override final
{
RCLCPP_INFO(logger_, "--- Result received ---");
RCLCPP_INFO(logger_, "Time elapsed: %f", result.result->time_required);
return RosActionNode::onResultReceived(result);
}
};
} // namespace my_namespace
// Make the node discoverable for the class loader
AUTO_APMS_BEHAVIOR_TREE_DECLARE_NODE(my_namespace::SimpleSkillClient)Just like with the server, we must also add something to the CMakeLists.txt of our package to actually make use of the SimpleSkillClient node. AutoAPMS provides a CMake macro that makes it easy for you to register custom nodes with the ament_index, a concept of ROS 2 that allows installing resources which can be queried at runtime. The following assumes that you add the client node's source file to a shared library called "simple_skill_nodes".
# Create shared library for the node
add_library(simple_skill_nodes SHARED
"src/simple_skill_client.cpp" # Replace with your custom path
)
ament_target_dependencies(simple_skill_nodes
auto_apms_interfaces
auto_apms_behavior_tree
)
# Declare client behavior tree node
auto_apms_behavior_tree_declare_nodes(simple_skill_nodes
"my_namespace::SimpleSkillClient"
)We use the term "declare a node" instead of "register a node" to avoid confusing this step with actually registering a node with a specific behavior tree. You will learn about the latter later when we execute our application. For now, you should note, that declaring a node works very similar to what we've done with SimpleSkillServer.
| Type | C++ | CMake |
|---|---|---|
| Server | RCLCPP_COMPONENTS_REGISTER_NODE | rclcpp_components_register_node rclcpp_components_register_nodes |
| Client | AUTO_APMS_BEHAVIOR_TREE_DECLARE_NODE | auto_apms_behavior_tree_declare_nodes |
Build a Behavior Tree
With both server and client implemented, we are done writing the low-level source code and ready to climb up the ladder of abstraction: We may now build our first behavior tree and actually employ the functionality we've just created!
Configure a Node Manifest
As you know, behavior trees are composed of nodes. Within AutoAPMS, all behavior tree nodes are plugins (except for the builtin/native nodes statically implemented by BehaviorTree.CPP). They are loaded at runtime when the tree is created. To specify which node classes to load and how to instantiate them, you must configure so called node manifests. To reproduce this example, you don't need to know the details about this concept. Nevertheless, feel encouraged to check out the designated chapter on how to configure node manifests.
The node manifest for the behavior tree we're going to build looks like this:
SimpleSkillActionNode:
class_name: auto_apms_examples::SimpleSkillClient
port: simple_skill
HasParameter:
class_name: auto_apms_behavior_tree::HasParameter
port: (input:node)/list_parametersWe want to include two custom nodes in our behavior tree:
SimpleSkillActionNode
This is the node that acts as a client to the
simple_skillaction we implemented above. As mentioned before, we need to include this node to send the action goal to the server.HasParameter
We additionally want to incorporate a node that allows us to determine if the tree executor defines a certain ROS 2 parameter, because want to support dynamically setting the message to be printed. This node is one of many standard nodes provided by the package
auto_apms_behavior_tree.
Before we're able to build our behavior tree, we must make sure that our node manifest will be available at runtime. This is achieved by registering one more ament_index resource using the NODE_MANIFEST argument accepted by the CMake macros auto_apms_behavior_tree_declare_nodes and auto_apms_behavior_tree_declare_trees. Visit the designated chapter towards node manifest resources to learn more about the different possibilities to create one. We've already used the former macro when declaring our client node. The latter is intended for - you've guessed it - registering yet another resource: The actual behavior tree source file. It's pretty obvious that the resource system of ROS 2 is invaluable for AutoAPMS.
You must modify the CMakeLists.txt of your package according to how you intend to create the behavior tree. We distinguish between two general approaches: You may either create a behavior tree graphically using a suitable visual editor or programmatically by incorporating the C++ API offered by AutoAPMS. The following shows the required configuration for being able to successfully build the example:
# Create shared library for the node
add_library(simple_skill_nodes SHARED
"src/simple_skill_client.cpp" # Replace with your custom path
)
ament_target_dependencies(simple_skill_nodes
auto_apms_interfaces
auto_apms_behavior_tree
)
# Declare client behavior tree node
auto_apms_behavior_tree_declare_nodes(simple_skill_nodes
"my_namespace::SimpleSkillClient"
)
# Declare simple skill tree
auto_apms_behavior_tree_declare_trees(
"config/simple_skill_tree.xml" # Replace with your custom path
NODE_MANIFEST
"config/simple_skill_node_manifest.yaml" # Replace with your custom path
)
# The plugin libraries MUST be installed manually
install(
TARGETS
simple_skill_nodes
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin
)# Create shared library for the node
add_library(simple_skill_nodes SHARED
"src/simple_skill_client.cpp" # Replace with your custom path
)
ament_target_dependencies(simple_skill_nodes
auto_apms_interfaces
auto_apms_behavior_tree
)
# Create another shared library for the build handler
add_library(simple_skill_build_handler SHARED
"src/simple_skill_build_handler.cpp" # Replace with your custom path
)
ament_target_dependencies(simple_skill_build_handler
auto_apms_behavior_tree
)
# Declare client behavior tree node
auto_apms_behavior_tree_declare_nodes(simple_skill_nodes
"my_namespace::SimpleSkillClient"
NODE_MANIFEST
"config/simple_skill_node_manifest.yaml" # Replace with your custom path
NODE_MODEL_HEADER_TARGET # Optional: Generate C++ code for custom nodes
simple_skill_build_handler
)
# We don't need to call auto_apms_behavior_tree_declare_trees in this example
# Instead, we implement a behavior tree build handler plugin
# Declare the behavior tree build handler that builds the simple skill tree
auto_apms_behavior_tree_declare_build_handlers(simple_skill_build_handler
"my_namespace::SimpleSkillBuildHandler"
)
# The plugin libraries MUST be installed manually
install(
TARGETS
simple_skill_nodes
simple_skill_build_handler
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin
)With the CMakeLists.txt for the programmatic approach we jumped ahead a little, because we haven't introduced the third CMake macro auto_apms_behavior_tree_declare_build_handlers. This fundamentally works like rclcpp_components_register_nodes, but it's designed specifically for registering/declaring behavior tree build handlers. You'll learn more about what a build handler is and how to implement it in the section about programmatically building a behavior tree.
Now we're finally able to configure a behavior tree that employs the functionality provided by our simple skill.
Graphical Approach
Behavior trees are defined using the XML format. So to define a behavior incorporating our simple skill, we must create a new .xml file from scratch. You can do that manually or run this convenient command that automatically writes an empty behavior tree to the given file.
ros2 run auto_apms_behavior_tree new_tree "config/simple_skill_tree.xml"TIP
Also consider using the corresponding VSCode Task which does the same thing.
➡️ Terminal -> Run Task... -> Write empty behavior tree
You must manually create an empty .xml file and open it before executing the task, since the current opened file will be used.
No matter how or where you create the behavior tree file, you'll probably need a visual editor for behavior trees. We recommend Groot2, because it's designed to be compatible with the behavior tree XML schema used by AutoAPMS and considered the de facto standard.
Once you launched Groot2, you need open/create your behavior tree file. Groot2 allows to build behavior trees by dragging and dropping node icons. However, you must explicitly tell the application how your custom nodes are called, which type they have and what data ports they implement. This is what behavior tree node model files are used for. These files hold information about custom nodes that you want to use in Groot2 for creating a specific behavior. When using AutoAPMS, they are automatically generated by CMake when registering node manifests and must manually be loaded using the "Import models from file" button. The model XML files are installed under auto_apms/auto_apms_behavior_tree_core/metadata/node_model_<metadata_id>.xml relative to the share directory of the respective package. For more information, refer to the detailed instructions on how to use Groot2.
The graphical approach allows the user to quickly and intuitively configure the XML for the behavior tree. We define the behavior according to these rules:
If a custom message was specified using a parameter, use this. Otherwise, use a default message.
If a custom number of prints to the terminal was specified using a parameter, use this. Otherwise, only print once.
After both variables have been determined, execute the
simple_skillaction with the corresponding goal.To indicate that the behavior is finished, print a final message once.
Example Behavior Tree
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4"
main_tree_to_execute="SimpleSkillDemo">
<BehaviorTree ID="SimpleSkillDemo">
<Sequence>
<ForceSuccess>
<HasParameter parameter="bb.msg"
node=""
_onSuccess="msg := @msg"
_onFailure="msg := 'No blackboard parameter'"/>
</ForceSuccess>
<ForceSuccess>
<HasParameter parameter="bb.n_times"
node=""
_onSuccess="n_times := @n_times"
_onFailure="n_times := 1"/>
</ForceSuccess>
<SimpleSkillActionNode n_times="{n_times}"
msg="{msg}"/>
<SimpleSkillActionNode n_times="1"
msg="Last message"/>
</Sequence>
</BehaviorTree>
</root>What are Global Blackboard Parameters?
This example behavior tree showcases a very useful concept introduced by AutoAPMS: Global Blackboard Parameters. They are accessed using the bb./@ prefix and allow us to adjust the behavior without rebuilding the entire tree, thus makes it reusable. This concept fuses ROS 2 Parameters with the Global Blackboard Idiom. This is one of the reasons why AutoAPMS's adaption of the behavior tree paradigm is very well integrated with ROS 2.
Refer to the designated chapter about global blackboard parameters for more information.
This behavior tree would theoretically be ready for deployment, but first we want to show you an alternative approach of building this tree.
Programmatic Approach
To allow building behavior trees programmatically, we offer a powerful C++ API. The required functionality is provided by the following two classes:
Implements a Document Object Model (DOM) for the behavior tree XML schema. It offers a modular, streamlined API to create and modify the XML representation of behavior trees within ROS 2 workspaces.
Introduces a plugin-based approach for implementing specialized algorithms for building behavior trees. When deploying behaviors, users may dynamically load plugins created by inheriting from this class for customizing how
TreeExecutorNodebuilds the behavior tree to be executed.
We're going to show you how to use this functionality by implementing a custom behavior tree build handler that builds the tree shown above. Refer to the designated chapter about building behavior trees using build handlers for a full guide on that topic.
We highly recommend incorporating node models when using TreeDocument. This makes the source code more verbose and enables detecting behavior tree syntax errors at compile time. This is why we added these lines
auto_apms_behavior_tree_declare_nodes(simple_skill_nodes
"my_namespace::SimpleSkillClient"
NODE_MANIFEST
"config/simple_skill_node_manifest.yaml" # Replace with your custom path
NODE_MODEL_HEADER_TARGET # Optional: Generate C++ code for custom nodes
simple_skill_build_handler
)to the CMake macro that parses our node manifest and registers a corresponding resource. Specifying the NODE_MODEL_HEADER_TARGET argument additionally allows us to include an automatically generated C++ header which contains specific classes that represent our custom nodes. They are intended to be used as template arguments when building the tree.
Example Build Handler
// This also includes all standard node models
// (under namespace auto_apms_behavior_tree::model)
#include "auto_apms_behavior_tree/build_handler.hpp"
// Include our custom node models
// (under namespace my_package::model)
#include "my_package/simple_skill_nodes.hpp"
namespace my_namespace
{
class SimpleSkillBuildHandler : public auto_apms_behavior_tree::TreeBuildHandler
{
public:
using TreeBuildHandler::TreeBuildHandler;
TreeDocument::TreeElement buildTree(
TreeDocument & doc,
TreeBlackboard & bb) override final
{
// Create an empty tree element
TreeDocument::TreeElement tree = doc.newTree("SimpleSkillDemo").makeRoot();
// Alias for the standard node model namespace
namespace standard_model = auto_apms_behavior_tree::model;
// Insert the root sequence as the first element to the tree
TreeDocument::NodeElement sequence = tree.insertNode<standard_model::Sequence>();
// Insert the HasParameter node for the variable msg
sequence.insertNode<standard_model::ForceSuccess>()
.insertNode<model::HasParameter>()
.set_parameter("bb.msg")
.setConditionalScript(BT::PostCond::ON_SUCCESS, "msg := @msg")
.setConditionalScript(BT::PostCond::ON_FAILURE, "msg := 'No blackboard parameter'");
// Insert the HasParameter node for the variable n_times
sequence.insertNode<standard_model::ForceSuccess>()
.insertNode<model::HasParameter>()
.set_parameter("bb.n_times")
.setConditionalScript(BT::PostCond::ON_SUCCESS, "n_times := @n_times")
.setConditionalScript(BT::PostCond::ON_FAILURE, "n_times := 1");
// Insert the SimpleSkillActionNode node that prints msg exactly n_times times
sequence.insertNode<model::SimpleSkillActionNode>()
.set_n_times("{n_times}")
.set_msg("{msg}");
// Insert the SimpleSkillActionNode node that prints the last message
sequence.insertNode<model::SimpleSkillActionNode>()
.set_n_times(1)
.set_msg("Last message");
// Return the tree to be executed
return tree;
}
};
} // namespace my_namespace
// Make the build handler discoverable for the class loader
AUTO_APMS_BEHAVIOR_TREE_DECLARE_BUILD_HANDLER(my_namespace::SimpleSkillBuildHandler)Congratulations! 🎉 You are now familiar with the general workflow of creating behaviors.
Execute the Behavior Tree
Finally, we're going to demonstrate how our simple skill and the behavior tree we've just created can be deployed. Make sure that you build and install AutoAPMS and your custom package which contains the source code for the example described above (we called it my_package before).
colcon build --packages-up-to my_package --symlink-installThe package auto_apms_behavior_tree offers an executable called run_tree which may be used to execute a specific behavior tree. It provides a runtime for AutoAPMS's flexible TreeExecutorNode. You may either use this executable by running it from a terminal
ros2 run auto_apms_behavior_tree run_tree -h # Prints help informationor by including it inside a ROS 2 launch file.
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription(
[
Node(
package="auto_apms_behavior_tree",
executable="run_tree",
arguments=["-h"] # Prints help information
)
]
)It accepts one argument: The build request for the underlying behavior tree build handler. By default, you can simply specify a behavior tree resource and the corresponding behavior tree will be executed immediately. If you want to know more about the usage of run_tree and alternative ways to execute a behavior tree, visit the chapter about deploying behaviors.
To run the simple skill example, execute the following steps:
If you decided to create a behavior tree XML file using Groot2 (theoretically you could also do so manually), your behavior tree should be declared using the CMake macro auto_apms_behavior_tree_declare_trees. A behavior tree declared like this can automatically be discovered at runtime. The macro installs a behavior tree resource and enables the TreeFromResourceBuildHandler which comes with AutoAPMS to read the corresponding XML file and build the tree. By default, TreeExecutorNode loads this build handler when it is started. This conveniently allows us to execute any declared behavior tree like this:
ros2 run auto_apms_behavior_tree run_tree "<package_name>::<tree_file_stem>::<tree_name>"TIP
Also consider using the corresponding VSCode Task which does the same thing.
➡️ Terminal -> Run Task... -> Run behavior tree
Run the Example (Graphical approach)
Let us demonstrate the intended usage of run_tree for the behavior tree we created applying the graphical approach. AutoAPMS provides SimpleSkillServer, SimpleSkillClient and the example tree called SimpleSkillDemo with the package auto_apms_examples. Other than executing the tree, you must of course make sure that the server providing our simple skill is started as well.
Using only the terminal
Start the simple skill server in its own terminal:
ros2 run auto_apms_examples simple_skill_serverAfterwards, create a new terminal and start executing the behavior tree:
ros2 run auto_apms_behavior_tree run_tree auto_apms_examples::simple_skill_tree::SimpleSkillDemoUsing a launch file
Or you do both in a single launch file similar to this:
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription(
[
# Spawn the simple skill server
Node(
package="auto_apms_examples",
executable="simple_skill_server"
),
# Spawn the behavior tree executor for the simple skill tree
Node(
package="auto_apms_behavior_tree",
executable="run_tree",
arguments=["auto_apms_examples::simple_skill_tree::SimpleSkillDemo"]
)
]
)ros2 launch auto_apms_examples simple_skill_launch.py approach:=graphicalModify the behavior using parameters
Remember that we configured the behavior tree so that we can adjust the behavior according to the parameters bb.msg and bb.n_times? They can be specified like any other ROS 2 parameter by either using the command line or a launch file. For example, run this:
ros2 run auto_apms_behavior_tree run_tree auto_apms_examples::simple_skill_tree::SimpleSkillDemo --ros-args -p bb.msg:="Custom message" -p bb.n_times:=10Or add the parameters inside the launch file:
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription(
[
# Spawn the simple skill server
Node(
package="auto_apms_examples",
executable="simple_skill_server"
),
# Spawn the behavior tree executor for the simple skill tree
Node(
package="auto_apms_behavior_tree",
executable="run_tree",
arguments=["auto_apms_examples::simple_skill_tree::SimpleSkillDemo"],
parameters=[{"bb.msg": "Custom message", "bb.n_times": 10}]
)
]
)Try out different parameters yourself!