Behavior Node Tutorial (Python)

1. Writing a Behavior Node (python)

The following instructions detail how to create a behavior for the tractor that interfaces with the tractor's state controller using rospy. First, let's create a basic behavior node (example code adapted from ROS python PublisherSubscriber tutorial):

1.1 The Code

Create the scripts/BehaviorEx.py file within the /scripts directory and paste the following inside it:

#!/usr/bin/env python
"""Example of a basic tractor behavior.

This module demonstrates an example behavior written in python to be
compatible with the state controller. The behavior publishes any
command received on /ex_topic directly to the corresponding topic
/state_controller/cmd_behavior.

authored by Connor Novak on 2019-10-29
"""

import rospy
from geometry_msgs.msg import Twist
from state_controller.msg import TwistLabeled

class BehaviorEx():

	def __init__(self):
		"""Initialize node, publisher and subscriber.""" 

		# In ROS, nodes are uniquely named. If two nodes with the same
		# name are launched, the previous one is kicked off. The
		# anonymous=True flag means that rospy will choose a unique
		# name for our 'listener' node so that multiple listeners can
		# run simultaneously.
		rospy.init_node('behavior_ex', anonymous=True)
		rospy.Subscriber('/ex_topic', Twist, self.callback)
		self.twist_publisher = rospy.Publisher(
			'/state_controller/cmd_behavior', TwistLabeled, queue_size=1)

		# spin() simply keeps python from exiting until this node is stopped
		rospy.spin()

	def callback(self, data):
		"""Forward recieved twist message to state controller with label."""
		new_msg = TwistLabeled()
		new_msg.twist = data 
		new_msg.label.data = 'example'
		self.twist_publisher.publish(new_msg)
	
if __name__ == '__main__':
	b = BehaviorEx()

1.2 The Code Explained

#!/usr/bin/env python

Every Python ROS Node will have this declaration at the top. The first line makes sure your script is executed as a Python script.

import rospy
from geometry_msgs.msg import Twist
from state_controller.msg import TwistLabeled

You need to import rospy if you are writing a ROS Node. The geometry_msgs.msg import is so that we can listen for the Twist message type (a basic vector message class, see docs.ros.org). We also import a custom message from the state_controller/msg directory called TwistLabeled (see the Code) to publish to the state controller.

class BehaviorEx():

Here we start to define a python class for the behavior. We use the class because making the publisher a class attribute allows it to be accessible within the callback function, an ability that we would not easily have if we kept two separate functions like the original ROS example code here.

def __init__(self):
	rospy.init_node('behavior_ex', anonymous=True)
	rospy.Subscriber('/ex_topic', Twist, self.callback)
	self.twist_publisher = rospy.Publisher(
		'/state_controller/cmd_behavior', TwistLabeled, queue_size=1)

	rospy.spin()

This section of code defines the behavior's interface to the rest of ROS. The first line, rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. In this case, your node will take on the name behavior_ex. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".

anonymous = True ensures that your node has a unique name by adding random numbers to the end of NAME. Refer to Initialization and Shutdown - Initializing your ROS Node in the rospy documentation for more information about node initialization options.

The next line, rospy.Subscriber('/ex_topic', Twist, self.callback), declares that yout node subscribes to the /ex_topic topic which is of type geometry_msgs.msgs.Twist. When new messages are received, BehaviorEx's callback method is invoked with the message as the first argument.

The next line, self.twist_publisher = rospy.Publisher('/state_controller/cmd_behavior', TwistLabeled, queue_size=1), declares that your node is publishing to the /cmd_behavior topic in the namespace /state_controller using the custom message type TwistLabeled. The queue_size argument limits the amount of queued messages if any subscriber is not receiving them fast enough.

The final addition, rospy.spin() simply keeps your node from exiting until the node has been shutdown. Unlike roscpp, rospy.spin() does not affect the subscriber callback functions, as those have their own threads.

def callback(self, data):
	new_msg = TwistLabeled()
	new_msg.twist = data 
	new_msg.label.data = 'example'
	self.twist_publisher.publish(new_msg)

This function is invoked whenever the subscriber initialized in the __init__ method recieves a message. The message is passed as the argument data. First, a new TwistLabeled message is created. The data, which is a Twist message, is stored as the twist component of the new message. the line new_msg.label.data = 'example' stores the string "example" in the label attribute of the TwistLabeled message. NOTE: the data attribute here is referenced because if we wanted to add a label to new_msg.label, it would have to be a ROS String class. Here, when we write new_msg = TwistLabeled(), the code populates the new message with a default ROS String, and we can just change that String's .data attribute. Lastly, we publish the new message.

if __name__ == '__main__':
    b = BehaviorEx()

Lastly, we run the new behavior, which automatically calls __init__(). Note that because we called rospy.spin() in the __init__ method, we need a while loop in the main section of the script.

2. Registering your Behavior

The last step in writing a new behavior for the tractor is to register your behavior with the main state controller such that the state controller doesn't discard messages received from a behavior it does not recognize. Add your behavior to gravl/config/tractor_behaviors.yaml, a file that is read by the state controller to know which behaviors to allow to control the tractor and what relative priorities the behaviors have:

behaviors/safety: '0' behaviors/teleop: '1' behaviors/example: '2'

The lower integer values override the higher integer values. The 0th priority belongs explicitly to safety behaviors. The 1st priority belongs to the teleoperative control. All other priorities are fair game for other written behaviors. These choices make it such that joystick commands override all behaviors besides safety, and safety commands override all behaviors period.