Behavior Tree Tutorial 1.5 Writing a Tree that uses ROS

While we can of course call functions directly in the behavior Tree, we would then have all out functionality locked to it. A core idea of using it for mission control in our Robots is to seperate the "High level mission control" and the low level robot control. Is is usually done by using ros Topics, Actions and services to talk to the "low level" ROS Nodes.

The ROS_BT_PY was specifically designed to be able to handle ROS Communication. This is not trivial for a Behavior Tree as ROS uses asynchronous communication where a behavior tree requires immediate results following the ticking of a node. Thats why the ROS_BT_PY uses an extended state transitions for the nodes including an untick option for longer running tasks.

Still, while designing a BT you will need to be careful about blocking and timing issues.

A simple Topic Publisher

Lets start with the previous example of hello world with a selector:

Now, instead of using the logger, we can also publish the constructed message

• Search for topic in the package selection
• Select "TopicPublisher" and move it left of the Log node
• Change the topic_name Option to "HelloTopic"
• Change the topic type to a string message -- For this, simply type "string" as type and you will automatically get the suggestion of using "std_msgs.msg._String.String" -- Press "update Node"
• The input might still show int, you can klick on something else and then select the topicPublisher aggain to update this
• If you try to connect the Nodes "StringConcatenatenation" wit the "TopicPublisher" it should not work yet

So why can we not simply connect the string with the input as we did with the Log node?
This is due to the strong Type safety of ROS_BT_PY. The output of StrinConcatenation is a string the input of TopicPublisher however is a std_msgs.msg.String.String . You now have two options. Either create a modified publisher that accepts the basic data types or use an intermediate node to convert the data. In case of a larger message, the new node is the better option as you would otherwise create many nodes. In this case we can use a transform node.

• By searching for "msgs" we can find the node "FieldsToMessage" which is a generic transformation node

• Pull this in the editor, left of the TopicPublisher

• change its output type to std_msgs.msg.String.String

• update the node

• Connect the output of StringConcatenation node to the "FieldsToMessage" node and its output to the TopicPublisher

• Run the tree at least once (tickOnce) so the topic is published

• You can now get the ouptut on the console by running:

$ rostopic echo HelloTopic

• Every time you run the tree with tickonce should now produce an output, you can also run it periodically

data: "HelloUniverse"
---
data: "HelloMoon"
---
data: "HelloWorld"

• You can open the tree as 05_topic_publisher.yaml from the examples folder

Topic Subscriber

Now lets subscribe somethig, we will build uppon the last example and exchange one of the variables with the input

• First make sure that each of the 4 Constants is actually selectd when ticking periodically, in some examples the randomInt range is set to low so the last node will never be executed

• Delete the ConstantLAURON node

• We want to replace the constant node with a subscriber so search for available subscribers

• The TopicSubscriber seems like a good choice, but read its description carefully -- It will return RUNNING while waiting for a topic and only succede once a message was received

• Insert the TopicSubscriber node -- Change the TopicName to "HelloSubscriber" and the Type to std_msgs.msg._String.String -- You will realize that we can not connect the output to the StringConcatenation node as the types mismatch -- We will need a MessageToFiels node, transforming the string message to a string -- But placing it in the top level sequence leads to an error:

• The input of the messageToFields node is unset in this scenario it is therefore important to look at the actual controll flow.

• Remove (or move) the node form the ChildSelector node

• Place a sequence instead

• Place the topic subscriber and MessageToFields node underneath this subscriber

• The ChildSelector will now randomly select one of the nodes, if it is the sequence, it will try to read a message form the TopicSubscriber and only of that succedes run the MessageToFields node which has now a set input

• Try excecuting this tree (06_topic_subscriber.yaml) by selecting tick periodically and then publis messages to the Topic with different Publishing rates. See what happens

• While the tree works fine, the output is only visible if the publishing rate is large enough. This is due to the way the subscribeTopic node is implemented.

• As it only looks at the topic in the exact tick moment, it will not register any messages that have arrived when not ticked as the received message will be reset at after each tick

• Now you could also use the TopicMemorySubscriber which is behaving sometwhat simmilar, instead of returning a running it will always return a failed unless it has received a message within the last n seconds (n is an option). Similarly we can use it in a sequence and then only perform the MessageToField conversiopn after a success. However, the returned FAILED outcome might not be beneficial for the control flow of your application.

• Please feel free to extend the available options of topic subscribing nodes in the future

A Service client

Calling a Service works the same way as using topics. But, due to the synchronous execution of services, the node will spawn of a thread to handle the respone. This is done with the helper calass AsyncServiceProxy

To test it we will use the most simple server:

#!/usr/bin/env python

from __future__ import print_function

from std_srvs.srv import Empty,EmptyResponse
import rospy

def handle_hello(req):
    print("The service to tell you: 'Hello World' was triggered")
    return EmptyResponse()

def hello_server():
    rospy.init_node('hello_service_server')
    s = rospy.Service('helloService', Empty, handle_hello)
    print("Ready to say Hello.")
    rospy.spin()

if __name__ == "__main__":
    hello_server()

• Use the tree 05_topic_publisher as stating point
• replace the last node with a sequence
• Place a constant and the service node unterneath
• set the Type of the request, response and constant to that of the corressponing emptyservice type. -- Be aware that multiple empty definitions exist. Make sure to use the std_srvs one
• Run the tree by ticking it periodically, you should see the output of your node
• You can find the finished tree as 07_service_caller.yaml in the example trees

There are of course some considerations, especially due to flow control. The Service node uses the AsyncServiceProxy:

        self._service_proxy = AsyncServiceProxy(self.options['service_name'],
                                                self.options['service_type'])

the state of the service node will likely be running, for a tick, even if the return result is send instantly. It is therefore encuraged to use actions as they are natively asynchonous

What not to do

Now that you have an idea how to write your nodes and trees, it is a good idea to talk about what I would advice not to do when creating a tree