An implementation of a Simultaneous Location and Mapping algorithm for the Sparki robot to be used in middle or high school robotics classes
As of my last post, I had finished a working ROS node for Sparki so I have now completed the Sparki <--> Raspberry Pi communication portion of the project. Now I am working on one of the harder parts, getting the ROS Navigation Stack to run on the Pi. I compiled the core ROS pieces from source on the Pi successfully. I am now working on installing the navigation stack itself. It has a lot more dependencies that are not available through Raspbian or Debian packages. We'll see if I can pull that off.
In the mean time, I am also working on getting the navigation stack working with Sparki on my laptop. That involves implementing a TF publisher in the Sparki node and, for the time being, converting the servo angle/ultrasonic distance data into a different format that the nav stack accepts to trick it to work crudely. Once I do that I'll look into a more native, less hack-y solution.
Here's my thoughts on the project so far:
In Part 1, I discussed how I had to drop rosserial in favor of a homespun solution to create a ROS node for Sparki. I developed a communication protocol to talk between a ROS node on a PC written in python and Sparki's microcontroller. I created a slimmed-down version for the micro for SparkiSLAM.
Now, I'll discuss the development of the python side of things. The way this node should work is that it is constantly sending out sensor data to various ROS topics as the data is received from Sparki and it should be listening to various command topics, such as robot linear and angular velocity commands, which it should transmit to Sparki.
As is recommended, I created a python class for communicating with Sparki that does not depend ROS. This class needs to provide methods for sending commands such as motor speeds or to actuate the gripper. It also needs to read in messages from Sparki and process that data. The first part, sending commands is trivial: just format the message and send it over the serial connection. The second part is harder. I first considered using pyserial to handle the serial comms. It, however, does not do asynchronous reading of the data on the port. One must continuous poll the port to check if data is available. This isn't ideal, as nearly all data transfer in ROS is asynchronous. To simulate that with polling could be processor intensive.
Some quick googling led me to the twisted library. It took a bit of learning but I figured out that I could create a custom protocol for twisted's serial port that would allow me to trigger a callback function whenever a line ending in '\n' was received on the port. Every message in my protocol ends with that delimiter so this suited my needs perfectly. I added functionality such that a user can add their own callback functions for whenever a specific type of message is received. I.e. when accelerometer data is received, print it and when distance data is received, send it over a ROS topic.
This almost got me where I needed to be, with one problem. Twisted requires a reactor to be running to process its event queue and ROS also requires its spin() function it run to process its event queue. Time for some threading. After a bit of trial and error, I managed to get the twisted reactor to run on its own thread without interfering with the ROS functionality.
I setup the appropriate ROS publishers and subscribers in the ROS node file and now we're on our way.
Currently, the node only has enough functionality for the SLAM project but it's designed to very easily expend to support all of Sparki's functionality. Once I do that, I'll release it into the ROS ecosystem.
You can check out a demo of the node running in my first update video: https://www.youtube.com/watch?v=FrYAD91eReY
I've decide to use ROS to get SLAM up and running for Sparki. I may not finish the project using ROS but it's a good way to quickly get SLAM up and running.
The first step is to create a ROS node for Sparki. My first thought was to use the rosserial library. This allows the code for node initialization and message publishing and subscribing to be run directly on Sparki's microcontroller. I started down this path and quickly realized that to expose all of Sparki's functionality to ROS, I'd need a lot of publisher and subscriber objects. This would quickly consume the flash memory (and probably the RAM too) on the micro.
Instead, I decided to create my own communication protocol that allows the Sparki to talk over serial with a ROS node written in python running externally. The protocol messages look like this: "U V 0.02 0.1 \n" (without the quotes). The first character defines the type of message. Uppercase for messages to the Sparki and lowercase from Sparki. The second letter is for an option and then the rest of the message is optional data. In this case, U means this is a command for the motors. The V option indicted that a linear velocity and angular velocity will be commanded. The data contains first the linear velocity in meter/second followed by angular in radians/second. The full protocol will be published on the project's github wiki and perhaps here as well.
I created a library called SparkiSerial that implements this protocol. It has functions to send information over serial such as accelerometer or phototransistor data as well as a function that reads in all data on the serial buffer, processes the messages, and executes the commands. I ran into another problem where, again, I exceeded the flash memory on the micro. I will be refining the library and perhaps altering the communication protocol to reduce the compile size of the library.
In the mean time, I created a slimmed-down version of the library with just enough functionality to do SLAM. I used this library to write an Arduino sketch for Sparki that can accept motor commands over serial and scan with the ultrasonic distance sensor. The sketch moves the distance sensor 5 degrees, polls the distance, and continues on, sweeping back and forth. This information is sent over serial to the ROS node.
In Part 2, I'll discuss the python side of the ROS node for Sparki.
For robotics system design, it is often useful to think about dividing system functionality by time scales. For SparkiSLAM, I'm splitting tasks into three groups: "real-time," fast, and slow.
"Real-time" tasks are processes that need to react very quickly. Examples instead closed loop control of motor speeds, edge detection and reaction for line following, and obstacle or cliff avoidance. These control loops are computationally simple and need to happen very quickly
For SparkiSLAM, these so-called real-time tasks run on-board the Sparki's Arduino-compatible processor. The Sparki will poll for commands over serial, move it's servo-mounted distance sensor, read from the distance sensor, and transmit the information over serial. I may also add cliff detection and avoidance and send accelerometer and magnetometer data over the serial connection to use sensor-fusion for positioning.
Fast tasks are processes that require more computation and need to be off-loaded to a separate processor. This ensures that tasks that need to occur quickly like very fast feedback loops are not bogged down by higher level functionality. In this SparkiSLAM project these tasks will run on a Raspberry Pi that is mounted on-board the Sparki, connected through a serial connection, and powered by the Sparki's batteries. Tasks it will perform include trajectory following, state estimation, and sensor fusion.
Slow tasks take more much computational power. These processes include the SLAM algorithm itself and path planning. These tasks do not endanger the robot or its operators so it's OK if these task run at a few hertz or less. For now, I will try to run these processes on the Raspberry Pi. I hope to off-load them to a server to make this a cloud powered robot.
One of the broad goals of this project is to create a simple way to deploy cloud processors to perform complex robotics algorithms to reduce the cost and complexity of robot hardware and make advanced robotics more accessible to students.