Blogs

Recently I've been reading about the Robot Operating System (ROS) from Willow Garage and thinking about ways that it could interface with a UAV system to take advantage of many of the robot navigation and task planning routines that have already been developed for other platforms.  After going through some of the ROS tutorials, I started coding a publisher node for APM, it reads telemtry data via serial in the GCS Standard format, and publishes the data as ROS messages that can be subscribed to by any other ROS nodes.  My chosen language is Python:

#!/usr/bin/env python
import roslib; roslib.load_manifest('apm_talker')
import rospy
from std_msgs.msg import Int16MultiArray, MultiArrayDimension
import serial
import struct

def parsemessage(mtype, mdata):
 ddict = dict()
 if mtype=='\x01':
 ddict['SystemStatus'] = list()
 ddict['SystemStatus'].append(int(mdata[2].encode('hex'), 16))
 ddict['SystemStatus'].append(struct.unpack_from("h", mdata[5:7])[0])
 if mtype=='\x02':
 ddict['Attitude'] = list()
 ddict['Attitude'].append(struct.unpack_from("h", mdata[2:4])[0])
 ddict['Attitude'].append(struct.unpack_from("h", mdata[4:6])[0])
 ddict['Attitude'].append(struct.unpack_from("h", mdata[6:8])[0])

 if mtype=='\x04':
 ddict['PressureAlt'] = list()
 ddict['PressureAlt'].append(struct.unpack_from("I", mdata[2:6]))
 #if mtype=='\x05':
 #    ddict['StatusText'] = list()
 #    ddict['StatusText'].append(str(mdata[2:len(mdata)-2]))
 return ddict

def talker():
 pub = rospy.Publisher('apm_data', Int16MultiArray)
 rospy.init_node('talker')
 ser = serial.Serial("/dev/ttyUSB0", 57600)
 while not rospy.is_shutdown():
 f = ser.read(size=1)
 if f=='\x34':
 f = ser.read(size=1)
 if f=='\x44':
 f = ser.read(size=1)
 data = ser.read(size=int(f.encode('hex'), 16)+4)
 datadict = parsemessage(data[0], data)
 for signal in datadict:
 msg = Int16MultiArray()
 msg.layout.dim = [MultiArrayDimension(signal, 1, 1)]
 msg.data = datadict[signal]
 pub.publish(msg)
 #rospy.loginfo(msg)

if __name__ == '__main__':
 try:
 talker()
 except rospy.ROSInterruptException: pass

This is fairly basic, and it doesn't cover the entire APM message set.  There is still work to do to decide how the APM messages should be translated into ROS topics and messages to allow for the most flexibility when using it with other packages - for example, ROS seems to use quaternions when representing robot attitude, so it will be necessary to convert from the APM Euler angles.

More details on my projects blog including code for a subscriber node, and some details on how the whole thing is run.