Remote Laser Mapping
Abstract
This report is intended to describe the process of assembling the hardware and developing and implementing the software for a mapping and localization system for a mobile robot using a scanning laser. The mobile robot base to be used is a Pioneer-2, and the laser to be used is a Hokuyo scanning laser. A laptop mounted on the robot will allow for WIFI communication of the data. Using this system, it was possible to stream both robot and laser data wirelessly to a connected client in order to produce a remote, graphical, realtime display of both the pioneer's odometry data and the laser's distance measurements.
Previous Developments
There exist several toolkits and software packages designed for mobile robot control and laser mapping, the most comprehensive of which is the Carnegie Mellon Robot Navigation Toolkit, or CARMEN. Simply put, CARMEN is a modular collection of software packages designed to control and interface with mobile robots and lasers. The image below shows CARMEN's mapping and localization module.
It is able to load a previously mapped location and localize the robot's position within it, as well as plan paths for the robot to follow in order to navigate to a specific position within the map. Much of CARMEN's functionality is beyond the scope of this project, but it does serve as a reference point for what this project is meant to accomplish (for more on CARMEN, visit the link to its homepage at the bottom). Namely, to manually operate a mobile robot remotely, gather data from a mounted scanning laser, and generate a map of the environment being explored.
Hardware
|
Robot Base The robot base I used in this project was the Pioneer-2, built by ActivMedia. It features a flat, solid top platform and a 23kg payload capacity. This made the Pioneer very well suited for supporting both a scanning laser and the netbook that would act as the robot's wireless server. Communication to and from the Pioneer is done over a serial port on the robot's top panel. ActivMedia provides software libraries for communicating with the Pioneer, but for this project I wrote my own program for serial communication in order to have more low level control over the robot (see the software section below for details.) |
|
Scanning Laser The laser I used is a Hokuyo URG-04LX-UG01 scanning laser. It is capable of scanning 682 points along a 240 degree arc, for a resolution of about 2.8 distance readings per degree. The laser has an advertised maximum range of 5.6 meters, though accuracy is only guaranteed up to 4 meters. Its margin of error is 3%, and it can do a full scan in about 100ms, or ten times per second. These tolerances were more than sufficient for the purposes of this project, as it was not intended to map anything in intricate detail. As with the pioneer, there are libraries available for communicating with the laser but I decided to write my own from the ground up. |
|
Acer Netbook In order to communicate remotely with the Pioneer and the laser, I used an Acer netbook that was connected to both directly, and ran a server program that would process the laser and robot packets and transmit the data over WIFI to a connected client. Because of its small size, it did not significantly alter the profile of the Pioneer or interfere with the placement of the laser. Power became an issue, however, because the laser was powered by the netbook which caused a significant drain on the netbook's battery life. |
Hardware Setup and Configuration
Physically setting up the hardware consisted simply of connecting the laser and the Pioneer to the netbook. The laser is connected with a mini-USB cable, and the Pioneer is connected using a serial to USB converter cable. The serial to USB cable requires special drivers, as does the laser (in order to treat it as a serial connection.) Once these drivers were installed, it was simply a matter of refreshing the USB device list in the netbook's device manager, causing the two devices to appear as COM ports. Once the COM port numbers are noted, they are then used to tell the server program running on the netbook where to connect to the laser and the robot.
Software Development
I encountered numerous challenges while developing the software for this project. I wrote all my programs from scratch, only rarely borrowing functions or code snippets from other sources. This was a very rewarding experience, as it gave me the opportunity to solve complicated programming problems that I had never encountered before. The downside of this was that I was often slowed down significantly by bugs that I had not anticipated.
Serial Communication
When starting this project, the first thing I had to do was write functions to communicate to a serial device. I used an AVR lab board to test my code, since the boot loader on this board outputs a continuous ASCII message which made it very easy to see if my code was working. I needed four functions in total: one to initialize the port, one to write to the port, one to read from the port, and one to close the connection to the port.
My initialization function requires two parameters: the baud rate of the device, and the com port it is connected to. Once the connection is made, the read/write functions can be used to either read a byte from the device, or send a byte to the device. By placing the read call in an infinite loop, I was able to read the AVR board's output as a constant stream of ASCII characters. The write function works similarly, it sends a single byte to the device. By passively listening for keyboard input, I was able to view the board's output in real time, and send bytes to the board at the press of a button.
Once these functions were working, I could begin talking to the Pioneer and the laser.
Pioneer Packet Protocol
The biggest challenge of communication with the pioneer is that it doesn't take simple ASCII bytes like the lab board does, but has its own packet protocol. In order to send a packet to the pioneer, my program defines the packet one byte at a time and sends each byte to the Pioneer individually.
The command number field in the figure above refers to an integer that corresponds to a specific Pioneer command. For example, to tell the Pioneer to drive forward, this byte is the integer 7, which corresponds to the Pioneer's move command. These command codes are available in the Pioneer's data sheet.
The argument itself was somewhat complicated. The Pioneer reserves two bytes for an integer, so it is necessary to break integer arguments into a low order and a high order byte and send them separately, even if the integer to be sent is smaller than one byte. Signed integers are even more complicated, as the pioneer is expecting the argument to be in two's compliment form. Simply sending -0x07 to the Pioneer will not yield the desired results, it must be converted to two's compliment, broken into two bytes, and then sent.
The Pioneer returns data packets in a similar fashion. Every 100ms, it sends back a generic information packet, as seen below. The packet contains many more fields than shown below, but these are the only ones I needed for the sake of determining the Pioneer's position for drawing a map.
The variables DistConvFactor and AngleConvFactor are internal coefficients used by the Pioneer. The Pioneer uses its own units, and multiplying values returned by the Pioneer by these two conversion factors will yield the correct measurements in millimeters and degrees.
Laser Packet Protocol
Having already developed code to parse Pioneer packets, interpreting the laser packets was much easier. Unlike the Pioneer, the laser does not send back a constant stream of data on its own but requires commands to tell it what to do. During this project the only command I used was the MD command, which requests a certain number of scans encoded in a certain way.
This command is fairly straight forward, and it is worth noting that it must be sent entirely in ASCII. This actually makes it quite a bit simpler than the Pioneer, because ASCII characters are one byte each and there is no need to worry about breaking the command elements into low and high order bytes. So, in order to request a single scan the command is simply the ASCII string "MD0044072501001" followed by 0x0d (the hex code for line feed) sent one byte/character at a time.
The format in which scan data is returned depends on the manner in which it was requested, in this case using the MD command. The laser returns an echo of the command sent, as well as a few other header characters and error codes, but what we really care about is the scan data itself. When all the extraneous data is stripped away, all that is left is a stream of ASCII characters that represent the encoded laser distance readings. These readings must be decoded as seen below:
Basically, every three characters represents a single distance measurement. The laser scans from right to left and returns data in that order, so the first measurement returned is the first reading taken at the start step defined in the MD command. Once the data has been decoded and stored, it could be used to draw a graphical representation of what the laser was seeing.
WIFI Communication
With the Pioneer and the laser accepting commands and returning data properly, the next step was to operate them remotely. I separated the code into two applications, a client and a server. Essentially the client talks to the server, which sends commands to the Pioneer and the laser, parses the return data, and sends it back to the client. The client merely has to send single bytes to the server, for example sending 'w' to the server will tell it to send the "drive forward" command to the Pioneer.
Like the serial communication functions, my WIFI functions read and write one byte at a time. This made it necessary to break down the parsed laser and Pioneer data into individual bytes. For the laser data, each distance measurement is represented as an integer, and since no single measurement exceeded 6000mm, each point could be represented by a high and low order byte (2^16 = 65536). The data is sent in this manner and reassembled by the client. The process is the same for the Pioneer odometry data.
Since this data needs to be streaming in order to create a reliable map, it was important to keep the packets going from server to client as small as possible. Each laser scan contains 682 data points, represented by 2 bytes each, for a total of 1364 bytes. The Pioneer packets are much smaller, only 22 bytes each. If each packet is sent 10 times per second, the total bandwidth used by the system is only around 14kb/s, a small footprint for most wireless networks.
Displaying the Data
I used OpenCV to draw the laser data in the form of a top down view of the environment being scanned. Knowing that the laser scans 682 points over a 240 degree arc, I was able to calculate the angular separation of each point and draw a line to represent the distance measurement. Below are two images created using OpenCV and data from the laser. Both were taken in the NCART lab, ENG209.
The left image is looking up at the ceiling, the right image is looking down on a lab table covered with parts and equipment. In both images the green lines represent open space, and the red dots are solid objects. Alternatively, the green lines show the direction in which the laser made a measurement, and the red dots show the distance of that measurement. Black areas without green lines indicate that either there was an error with the measurement or there was no object within range, usually the latter.
In addition to the laser display, I began working on a way to display the robot's position. The way it works is to use the robot's position data to draw a series of waypoints showing the path the robot has taken. Due to time constraints this part of the project is still in its infancy, but so far the results are promising.
Operating the System
The system is designed to be simple to operate. The client operator can use the 'w', 'a', 's', and 'd' key to drive the Pioneer while the server streams fresh laser data automatically. Additionally, pressing the space bar will update the robot's waypoint map by requesting fresh odometry data, and pressing 'o' will reset the origin of the Pioneer to the current location. This means that the Pioneer's internal (x,y) coordinates are reset to (0,0) and any previous position data will be lost. Finally, pressing escape will close the client program and sent a message to the server telling it to shut down as well.
Future Development
There are many things I would like to continue developing on this project, as well as a few new things I would like to add. The original goal was to create a floor plan map as CARMEN does, but unfortunately other aspects of the project were more time consuming than expected and I was unable to finish developing this functionality.
The first priority for the future then, is to combine the Pioneer odometry and the laser scan data in order to draw an actual floor plan. I believe this can be done fairly easily by simply using the odometry data as an offset for the laser measurements, and having the red dots seen in the laser window persist rather than redrawing them for each scan. This way, as the Pioneer moves through an environment, the red laser dots will continue to accumulate and will form a real time (albeit rough) view of the environment. In addition to this, the Pioneer and laser data could be stored in a log file that would then be imported into CARMEN's map generator. This program is designed to reduce errors and inaccuracies in the measurements and output a final, correctly proportioned map.
Another improvement I would like to make is regarding the way data is transmitted from the server to the client. I think it might be more efficient for the server to pass the raw Pioneer/laser data to the client, and have the client decode it. Doing this would make WIFI transmission much simpler. Since the raw data is in byte form, there would be no need to pack and unpack the parsed data. I think this would take a substantial processing load off of the server and only add a small load to the client, particularly because the computer running the client is likely to be more powerful than the Netbook.
A new feature I would like to add to the hardware of the system is to use the Pioneer's battery to power the laser. Powering the laser with the Netbook causes the battery to drain in about 45 minutes, which may not be enough time to successfully map a large area. By regulating the Pioneer's battery down to 5 volts and using it to power the laser, the battery life of the whole system could be greatly increased.
Overall, this was a very enjoyable project. Progress was steady, and I learned a lot. I am very happy with the way this system has turned out so far and hope to continue improving it in the future.
Further Reading & Sample Data
CARMEN Homepage