Reading serial ports in a real-time manner

One of our functions for the robot control program is to communicate with the Arduino microcontroller over a serial port.  How do we do that and maintain our timing loop we have worked so hard for?  Let's put down another very important rule about controlling robots that will be illustrated in the next bit of code. Let's make this a tip:

Let's have a quick review. A blocking call suspends execution of our program to wait for some event to happen. In this case, we would be waiting for the serial port to receive data ending in a new line character.  If the system on the other end of the serial port never sends data, we can be blocked forever, and our program will freeze.  So how do we talk to our serial port?  We poll the port (examine the port to see if data is available), rather than wait for data to arrive, which would the be standard manner to talk to a serial port.  That means we use read instead of readline commands, since readline blocks (suspends our execution) until a newline character is received.   That means we can't count on the data in the receive buffer to consist only of complete lines of data.  We need to pull the data until we hit a newline character (\n in Python), then put that data into our dataline output buffer (see the following code), and process it.  Any leftover partial lines we will save for later, when more data is available.   It's a bit more work, but the result is that we can keep our timing.

For advanced students, it is possible to put the read serial function into a separate thread and pass the data back and still use a blocking call, but I think this is just as much work as what we are doing here, and the polling technique is less overhead for the processor and more control for us, because we are never blocked:  

def readBuffer(buff):
# the data we get is a series of lines separated by EOL symbols
# we read to a EOL character (0x10) that is a line
# process complete lines and save partial lines for later
# 
EOL = '\n'
if len(buff)==0:
    return
dataLine = ""
lines=[]
for inChar in buff:
    if inChar != EOL:
    dataLine +=inChar
else:
    lines.append(dataLine)
    dataLine=""
for telemetryData in lines:
    processData(telemetryData)
return dataLine

This part of the code processes the complete lines of data we get from the Arduino. We have three types of message we can get from the microcontroller. Each message starts with a three letter identifier followed by data. The types are HBB for heartbeat, TLn for telemetry, and ERR for error messages. Right now we just have one telemetry message, TL1 (telemetry list 1). Later we will add more telemetry lists as we add sensors to the robot. The HBB message is just the Arduino repeating back the heartbeat message we send it twice a second. We'll use ERR to send messages back to the control program from the microcontroller, and these will be things like illegal motor command:

def processData(dataLine):
 #
 # take the information from the arduino and process telemetry into
 # status information
 # we recieve either heartbeat (HBB), TL1 (telemtry List 1), or ERR (Error messages)
 # we are saving room for other telemetry lists later
 dataType = dataLine[:3]
 payload = dataLine[3:] # rest of the line is data
 if dataType == 'HBB':
 # process heartbeat
 # we'll fill this in later
 pass
 if dataType == "TL1": # telemetry list 1
 # we'll add this later
 pass
 if dataType == "ERR": # error message
 print "ARUDUINO ERROR MESSAGE ",payload
 # log the error
 rospy.loginfo(payload)
 return
 

This section starts our main program.  We start by instantiating our objects for the ROS interface and the robot. I like to put the ROS interface stuff in an object because it makes it easier to keep track of and make changes.  Then, we open the serial port to the Arduino on /dev/ttyUSB0.  Note that we set the timeout to zero.  I don't think this is relevant since we are not using blocking calls on the serial port, but it can't hurt to double check that no blocking takes place.  We do some error checking with the try...except block to catch any errors.  Since the lack of a connection to the robot motors means we can't operate at all, I've had the program raise the error and stop the program run:

# main program starts here
# *****************************************************
rosif = RosIF() # create a ROS interface instance
robot = Robot() # define our robot instance
serialPort = "/dev/ttyUSB0"
# open serial port to arduino
ser = serial.Serial(serialPort,38400,timeout=0) #
# serial port with setting 38,400 baud, 8 bits, No parity, 1 stop bit
try:
 ser.open()
except:
 print "SERIAL PORT FOR ARDIONO DID NOT OPEN ", serialPort
 raise

Now, we start the ROS loop. not rospy.is_shutdown() controls the program and allows us to use the ROS shutdown command to terminate the program.  We also initialize our frame counter we use to schedule tasks.  I count the frames in each second (frameKnt) and then can set tasks to run at some divisor of the frame rate, as we discussed earlier in the chapter: 

 frameKnt = 0 # counter used to time processes
 while not rospy.is_shutdown():
 # main loop
 topOfFrame=time.time()
 # do our work
 # read data from the seral port if there is any
 serData = ser.read(1024)
 # process the data from the arduino
 # we don't want any blocking, so we use read and parse the lines ourselves
 holdBuffer = readBuffer(serData)
 #drive command
 com = ',' # comma symbol we use as a separator
 EOL = '\n'
 if robot.newCommand:
 ardCmd = "DRV"+str(robot.leftMotorCmd)+com+str(robot.rightMotorCmd)+EOL
 serial.write(ardCmd)
 serial.flush() # output Now

Here is an example of scheduling a task.  We want the heartbeat message to go to the Arduino twice a second, so we compute how many frames that is  (15 in this case) and then use the modulo operator to determine when that occurs.  We use the formula in case we want to change the frame rate later, and we probably will.:

 if frameKnt % (FRAMERATE/2)==0: # twice per second
 hbMessage = "HBB"+com+str(time.time())+EOL
 serial.write(hbMessage)
 serial.flush() # output Now

We manage the frame counter, resetting it each second back to zero. We could let it just run, but let's keep it tidy.  We'll reset the frame counter each second so we don't have to worry about overflow later:

 frameKnt +=1
 if frameKnt > FRAMERATE: frameKnt = 0 # just count number of frames in a second
 
 # done with work, now we make timing to get our frame rate to come out right
 #
 endOfWork = time.time()
 workTime = endOfWork - topOfFrame
 sleepTime = (FRAME-workTime)+ timeError
 time.sleep(sleepTime)
 endOfFrame = time.time()
 actualFrameTime = endOfFrame-topOfFrame
 timeError = FRAME-actualFrameTime
 # clamp the time error to the size of the frame
 timeError = min(timeError,FRAME)
 timeError = max(timeError,-FRAME)

Finally, when we fall out of the loop and end the program, we need to close the serial port to prevent the port from being locked the next time we start up:

 # end of the main loop
 #
 ser.close()