ROS Action based standard BT executor

[MarqRazz]

I am proposing a ROS2 Action Server that can be used as a standard executor for Behaviors written with BehaviorTree.CPP. It offers the ability to load BehaviorTree.CPP plugins, including ROS based Behaviors, and execute them by calling the ROS action with the name of the Tree to execute.

As I wrote this tool some of my goals were to:

• Standardize a way to build, include, and assemble custom Behaviors and Trees/Subtrees
  • I plan to release several BehaviorTree.CPP plugin based libraries in the hope to help others accelerate their robotics projects.
• Offer a simple API to start, cancel, and manage execution
• Expose an API to retrieve a list of loaded Trees/Behaviors (using rosparam and the generate_parameter_library)
  • ros2 param get action_server_bt plugins
• Expose an API to add or remove Behaviors/Trees that have been loaded at runtime
  • ros2 param set action_server_bt plugins [....]

The user can create Behavior plugins by implementing the BT_REGISTER_NODES(factory) or CreateRosNodePlugin(MyClassName, "MyClassName") functions inside the .cpp file of the Behavior. When building the package they can create the shared object with target_compile_definitions(my_plugin_target PRIVATE BT_PLUGIN_EXPORT ) and export them to a location that is easy for other packages to find.

######################################################
# INSTALL plugins for other packages to load
install(TARGETS
  sleep_plugin
  LIBRARY DESTINATION share/${PROJECT_NAME}/bt_plugins
  ARCHIVE DESTINATION share/${PROJECT_NAME}/bt_plugins
  RUNTIME DESTINATION share/${PROJECT_NAME}/bt_plugins
  )

Once the plugins have been built the user can create a yaml file for ActionServer.BT to load. They can specify as many package_name/subfolder directories as they wish to load all of the Behaviors needed for their system (see here for documentation on the plugin yaml parameters). They can also load a list of directories that contain BehaviorTree.xml files to load.

** Not supported right now but I may extend the package_name/subfolder lists to also allow users to set package_name/subfolder/plugin_name so that they have complete control over the plugins they wish to load.

As ActionServer.BT starts it will print the name of the ROS action that the user has chosen and a list of plugins/BehaviorTree.xml files it has loaded into the factory.

[action_server_bt]: Starting Action Server: behavior_server
[action_server_bt]: Loaded Plugin: libdummy_nodes_dyn.so
[action_server_bt]: Loaded Plugin: libmovebase_node_dyn.so
[action_server_bt]: Loaded Plugin: libcrossdoor_nodes_dyn.so
[action_server_bt]: Loaded ROS Plugin: libsleep_plugin.so
[action_server_bt]: Loaded BehaviorTree: door_closed.xml
[action_server_bt]: Loaded BehaviorTree: cross_door.xml

Now that the system is running you can call the Action from other programs or from the command line.
To call the CrossDoor Behavior tree:
ros2 action send_goal /behavior_server action_server_bt_msgs/action/ActionTree "{target_tree: CrossDoor}"
or you can also try the ROS based SleepActionSample:
ros2 action send_goal /behavior_server action_server_bt_msgs/action/ActionTree "{target_tree: SleepActionSample}"

Please let me know if you have any questions or suggestions on this proposal.
Marq

[facontidavide]

Thanks a lot @MarqRazz for this. I would love to eventually merge this into BehaviorTree.ROS2 as a "stadard way to solve this problem.

Some initial random feedback:

• "Action server" might be a little too general. We may need a new name 😉 (just nitpicking)
• About the interface here: https://github.com/MarqRazz/ActionServer.BT/blob/main/action_server_bt_msgs/action/ActionTree.action , I believe that the result must contain the NodeStatus. Also, the NodeStatus in the feedback is a little weird, since the only one that will be returned is RUNNING. Am I missing something?
• I like the use of ROS params and the YAML configuration to deal with plugins, nice!

I will dig more into the code, to see if I have more specific remarks

[MarqRazz]

"Action server" might be a little too general. We may need a new name 😉 (just nitpicking)

I agree, what do you think about "Behavior Server"?

Also, the NodeStatus in the feedback is a little weird, since the only one that will be returned is RUNNING. Am I missing something?

Couldn't it also return FAILURE as it goes into recovery behaviors? I don't really care if we use NodeStatus as the feedback but I would like some message to let the caller to know the server is running / processing (like a heart beat). Do you have suggestions for a better feedback value? Simple sequence id? It would be nice to know if a robot is attempting a recovery method several times in a row so the caller can preempt if they want.

[facontidavide]

I agree that a feedback might be good. Tick counter maybe? Just to see that the while loop is not frozen?

Technically, if the top level returns FAILURE, we exit the loop here: https://github.com/MarqRazz/ActionServer.BT/blob/main/action_server_bt/src/action_server_bt.cpp#L105

[MarqRazz]

I agree that a feedback might be good. Tick counter maybe? Just to see that the while loop is not frozen?

Yes I was thinking it could freeze because a Behavior is not implemented correctly and blocking tick.

What is the purpose of Tree::getUID? Would it be a good candidate for the feedback tick counter?

[facontidavide]

First quick scan of the code:

Loop

https://github.com/MarqRazz/ActionServer.BT/blob/main/action_server_bt/src/action_server_bt.cpp#L122-L126

Wheen looping, I suggest using BT::Tree::sleep instead of rclcpp::WallRate::sleep , because the former has some nice properties, being one the fact that it can be easily interrupted.

Status

https://github.com/MarqRazz/ActionServer.BT/blob/main/action_server_bt/src/action_server_bt.cpp#L137-L148

Carefull, the state SKIPPED could also be returned!

Creating tress

https://github.com/MarqRazz/ActionServer.BT/blob/main/action_server_bt/src/action_server_bt.cpp#L98

Some users reported that creating trees with a lot of Action Clients is slow because of... yes, you quessed correctly, DDS (discovery).
Therefore, we should think about how a Tree instance can be kept "alive", in case the same tree is requested again.

Logging

https://github.com/MarqRazz/ActionServer.BT/blob/main/action_server_bt/src/action_server_bt.cpp#L101-L103

We should think about other mechanism to add logging, since some users have custom loggers.
A potential solutions is to add some callbacks that the user can inject with a std::function, such as:

• void onTreeCreated(Tree&)
• void onTreeExecutionCompleted(NodeStatus)

[MarqRazz]

👍 on replacing rclcpp::WallRate::sleep with BT::Tree::sleep and I will get SKIPPED added.

Some users reported that creating trees with a lot of Action Clients is slow because of... yes, you quessed correctly, DDS (discovery).
Therefore, we should think about how a Tree instance can be kept "alive", in case the same tree is requested again.

If we find a better way than staticly creating objects this could allow the clients to stay connected and the discovery would only happen when loading/reloading the plugins. I was planning on using this approach for publisher/subscribers. That way I don't get multiple copies of them when there is more that one Node in the Tree that's executing.

We should think about other mechanism to add logging, since some users have custom loggers.

Should we also consider templating the action server class like Nav2? That way people could create their own API to call the Server. Nav2 did a similar approach where they allow a developer to pass in a on_loop callback.

[MarqRazz]

When looping, I suggest using BT::Tree::sleep instead of rclcpp::WallRate::sleep , because the former has some nice properties, being one the fact that it can be easily interrupted.

On second though this would remove my ability to tick at a given rate, unless I manually keep track of the remaining time needed to hold rate after ticking the tree. I still offer the ability to catch a cancel request but it wont happen till the next loop. Do you think responding to that a few milliseconds sooner will help?

[MarqRazz]

Carefull, the state SKIPPED could also be returned!

What does it mean when the Tree has finished executing and returns SKIPPED? Does that mean something failed or aborted and couldn't finish?

[facontidavide]

What does it mean when the Tree has finished executing and returns SKIPPED? Does that mean something failed or aborted and couldn't finish?

This is an extremely rare corner case where some top-level node uses PreConditions to skip execution.
So, unlikely, but we must support that case.

[facontidavide]

Also, but this is something I can do myself, we should be able to add a default Blackboard for the root node, here:

https://github.com/MarqRazz/ActionServer.BT/blob/main/action_server_bt/src/action_server_bt.cpp#L98

There is a new design pattern that I am working on that makes that mandatory. But as I said, I can foxus on this myself once we discussed the other topics.

[MarqRazz]

I had a default blackboard added there but didn't see the need for it yet so I removed it. What is the advantage of requiring it?

[facontidavide]

That deserves its own long thread 😄

I am working on the concept of "Knowledge Base / World State" and that would require its own global blackboard.
I don't want to go into the details here to reduce the noise, but... trust me on this one

[facontidavide]

When looping, I suggest using BT::Tree::sleep instead of rclcpp::WallRate::sleep , because the former has some nice properties, being one the fact that it can be easily interrupted.

On second though this would remove my ability to tick at a given rate, unless I manually keep track of the remaining time needed to hold rate after ticking the tree. I still offer the ability to catch a cancel request but it wont happen till the next loop. Do you think responding to that a few milliseconds sooner will help?

No, you should still have the same rate, but under SOME conditions, you may react faster that means ticking more frequently (but never less frequently).

[facontidavide]

We should think about other mechanism to add logging, since some users have custom loggers.

Should we also consider templating the action server class like Nav2?

I prefer personally dependency injections with std::function, rather than templating / polymorphism (at least in this particular case)

[MarqRazz]

Here is a PR to address some of your feedback. Let me know what you think.

[MarqRazz]

Other rename suggestions for the library/repo:

• BehaviorTree.ROSAction
• TaskRunner.BT
• ScriptRunner.BT
• Willow.BT
• Stormbreaker.BT