For a while now I have been following Willow Garage’s open source ROS (Robot Operating System). It just celebrated its third anniversary showing impressive growth with more and more universities and companies using and extending ROS: http://www.willowgarage.com/blog/2010/11/08/happy-3rd-anniversary-ros. Furthermore ROS is beginning to enter the hobby market.
As a first step to get familiar with ROS I decided to create a simple package that would allow me to receive lines of text from the Arduino board and publish them into ROS. Andrew Harris has already published a package for Arduino (http://www.ros.org/news/2010/01/arduino-and-cmucam3-support-for-ros.html) that uses an efficient binary protocol to communicate between Arduino and ROS. I might eventually use it but nothing beats writing a bit of software yourself when it comes to understanding a new software infrastructure.
The Source code of my simple Arduino playground package is available on Google code athttp://code.google.com/p/drh-robotics-ros/source/browse/?r=60#svn%2Ftrunk%2Fros%2Fplayground. If you want to play with it I suggest that you check out revision 60 of the source code into a folder under your home directory. In order for ROS to see the new package its path needs to be added to the ROS_PACKAGE_PATH environment variable. This is easily accomplished by adding a line at the end of the .bashrc file in your home folder. In my case the line reads:
export ROS_PACKAGE_PATH=~/dev/drh-robotics-ros/ros/playground:$ROS_PACKAGE_PATH
In your case the path to the playground folder might be different. To test it out we need an Arduino board with a sketch that periodically writes a string followed by a newline to the serial port like so:
Serial.println("Hello World");
The Python code of the arduino ROS node consists of two files. The entry point is the file /ros/playground/nodes/arduino.py:
#!/usr/bin/env python
'''
Created on November 20, 2010
@author: Dr. Rainer Hessmer
'''
import roslib; roslib.load_manifest('playground')
import rospy
from std_msgs.msg import String
from playground.msg import Encoder
from SerialDataGateway import SerialDataGateway
class Arduino(object):
'''
Helper class for communicating over serial port with an Arduino board
'''
def _HandleReceivedLine(self, line):
rospy.logdebug(line)
self._Publisher.publish(String(line))
def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
'''
Initializes the receiver class.
port: The serial port to listen to.
baudrate: Baud rate for the serial communication
'''
self._Publisher = rospy.Publisher('serial', String)
rospy.init_node('arduino')
self._SerialDataGateway = SerialDataGateway(port, baudrate, self._HandleReceivedLine)
def Start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
def Stop(self):
rospy.logdebug("Stopping")
self._SerialDataGateway.Stop()
if __name__ == '__main__':
arduino = Arduino("/dev/ttyUSB0", 115200)
try:
arduino.Start()
rospy.spin()
except rospy.ROSInterruptException:
arduino.Stop()
In the __main__ function at the bottom of the source file an instance of the Arduino class is instantiated with the appropriate port and baud rate. In the constructor the ROS topic ‘serial’ is published and the node is initialized. Finally an instance of the helper class SerialDataGateway is created (more info below). Once this is done the Arduino instance is started. The call to rospy.Spin() ensures that the program keeps running.
The helper class SerialDataGateway is responsible for receiving data via the serial port and assembling the data into individual lines. When it receives a ‘\n’ it calls into the line handler function that was passed into the constructor. The class uses a worker thread which in the current usage pattern is not really necessary. But it allows me to enhance the node later to support read and write operations in parallel.
#!/usr/bin/env python
'''
Created on November 20, 2010
@author: Dr. Rainer Hessmer
'''
import threading
import serial
from cStringIO import StringIO
import time
import rospy
def _OnLineReceived(line):
print(line)
class SerialDataGateway(object):
'''
Helper class for receiving lines from a serial port
'''
def __init__(self, port="/dev/ttyUSB0", baudrate=115200, lineHandler = _OnLineReceived):
'''
Initializes the receiver class.
port: The serial port to listen to.
receivedLineHandler: The function to call when a line was received.
'''
self._Port = port
self._Baudrate = baudrate
self.ReceivedLineHandler = lineHandler
self._KeepRunning = False
def Start(self):
self._Serial = serial.Serial(port = self._Port, baudrate = self._Baudrate, timeout = 1)
self._KeepRunning = True
self._ReceiverThread = threading.Thread(target=self._Listen)
self._ReceiverThread.setDaemon(True)
self._ReceiverThread.start()
def Stop(self):
rospy.loginfo("Stopping serial gateway")
self._KeepRunning = False
time.sleep(.1)
self._Serial.close()
def _Listen(self):
stringIO = StringIO()
while self._KeepRunning:
data = self._Serial.read()
if data == '\r':
pass
if data == '\n':
self.ReceivedLineHandler(stringIO.getvalue())
stringIO.close()
stringIO = StringIO()
else:
stringIO.write(data)
def Write(self, data):
info = "Writing to serial port: %s" %data
rospy.loginfo(info)
self._Serial.write(data)
if __name__ == '__main__':
dataReceiver = SerialDataGateway("/dev/ttyUSB0", 115200)
dataReceiver.Start()
raw_input("Hit <Enter> to end.")
dataReceiver.Stop()
The Arduino class passes its ‘_HandleReceivedLine’ function into the SerialDataGateway constructor. As a result whenever a new line is received the _HandleReceivedLine function in the Arduino class will be called:
def _HandleReceivedLine(self, line): rospy.logdebug(line) self._Publisher.publish(String(line))
The function simply logs the line and then publishes it as a string. In a real world scenario I would modify the function to parse the line and extract, say, pose data (x, y, and angle). Correspondingly a pose message rather than a simple string message would be published.
Finally we are ready to try out the new node. The required steps are as follows:
- Hook up an Ardunino board running a sketch that causes a line to be written to the serial port periodically.
- In a new terminal run:
roscore - In another new terminal run:
roscd playground
make
rosrun playground arduino.py
Provided the port and baud rate specified in Arduino.py match your setup you should have a running ROS Arduino node now. Next we want to tap into the published topic.
- In a new terminal subscribe to the ‘serial’ topic:
rostopic echo /serial
You should see the published lines fly by in this terminal. - To get a graphical representation of what is running start rxgraph in yet another terminal. The resulting graph should look similar to this:
On the left is the /arduino node with the exposed /serial topic. The node in the middle is the result of the rostopic app that we started. It subscribes to the /serial topic and displays the values in the terminal.
This concludes my first baby step into the world of ROS.
