Porting Wax to the Segway RMP

Basic Design
The Wax system already segregates robot functions into a few robot-specific libraries, each have several versions, supporting different robot types. The most prominent of these libraries is robotcontrol.o, which encapsulates state data retreival and motor commands. Applications (e.g. collision avoidance) in general have a single body of code, and are simply linked to the appropriate version of the library for the robot at hand.

For applications that make heavy use of sensor data for mapping, rather than abstract widely differing sensor data formats at the cost of efficiency, that mapping portion of code is implemented natively for each robot, and as before only the version appropriate for the robot at hand is linked. Fortunately, this sensor data gathering code is largely the same across different applications, reducing the effort to write new applications.

Although there are many library varieties (Nomad 200, Nomad 150, Nomad Scout, Nomad Superscout, Nomad XR4000, iRobot ATRVJr, iRobot ATRVMini, iRobot ATRVB21r), these basically fall into variants of three types: Nomad 200, Nomad XR4000, and iRobot rflex. Because of the simple nature of the RMP's native interface, the need to have a process consistently read and write to the CANbus even when idle, and the need to ultimately go through a single process (which is using the CANbus hardware connection), we chose to base the RMP version of Wax on the Nomad 200 model.

The RMP version of the robotcontrol.o library therefore makes calls to a new "rmpclient.o" library. This rmpclient.o library is analogous to the Nomadic Technologies "Nclient.o" library and contains RMP-specific functions. These functions communicate with a separate process, the "rmpd" daemon, running on a computer physically connected to the RMP's CANbus. The rmpd is analogous to the Nomadic Technologies "robotd" daemon.

Hardware and Software Architecture for the RMP

RMPD Features
The rmpd is reponsible for the actual CANbus communication with the robot hardware. The provided mirandalink/canpoll/canio interface was written for a Vector brand CANbus card under the Windows operating system. We obtained a second version from Segway written for the Kvaser card under WinCE. Because our system is Linux/Unix based, we had to use the Kvaser card (which offered Linux drivers and library). We combined the greater functionality of the Vector version of the Segway code with the Kvaser library calls demonstrated in the Kvaser version, ported the result to Linux, and matched it up to the Kvaser Linux "canlib" library.

For ease, the rmpd was given an interface to retrieve SICK range data from the existing Sickserver via TCP/IP. Applications use their native code portions to access this data through the functions and data structures in rmpclient.o.

Additionally, USB joystick support is included in the rmpd. Commands from the joystick are interleaved with incoming commands from applications. As a safety, one of the joystick buttons must be held down in order for the command to be considered. This prevents unwanted motion (likely high speed) if the joystick is inadvertantly bumped. At each iteration, an attempt to read all pending joystick events is made. Any new joystick event with a "0,0" (no motion) value is is accepted as a command and issued. This is also a safety issue, as we want to be sure not to miss a "stop" command. Otherwise, only the last joystick event read during the iteration is issued. This is in essence a filter to limit the high frequency joystick twiddles that are coming in faster than could be acted upon anyway and would otherwise bog down the rmpd and introduce considerable lag in the reponse time

.

RMPD Design Details
The rmpd is composed of several threads. One thread handles input from the hardware joystick. A second thread sets up a listen socket on a known port and listens for new TCP/IP connections. A new thread is spawned off for each connection (each client) to handle requests for data and act on motor or system commands. Having a separate thread for each client helps ensure that any problems or locking up of one client up will not affect the other clients or the rmpd itself.

Each incoming motor command goes into a single (semaphore protected) buffer, such that the last one wins. Commands to change acceleration, reset wheel encoders, etc are buffered independently.

A watchdog thread looks at the commands in the buffers, combines motor with status commands, and issues them. If there is no new motor command, the watchdog will continue re-issuing the last motor command in order to satisfy the RMP's 0.25 second watchdog, keeping the RMP moving. This re-issue continues until the rmpd's own watchdog timer is exceeded, and the watchdog thread issues a motor "stop". Note that the status commands are *not* re-issued, as that would violate their intent (such as when reseting counters).

Finally, a frame-handling thread continually checks the CANbus for incoming data frames. The robot's state data is updated and made available to the client handlers via semaphore-protected global variables. The frame handler also maintains odometry for the RMP, integrating the reported yaw and travel displacements. Incremental or absolute changes to the integrated pose sent from the clients are acted upon by the client handler threads.

Threads and Data Flow Within rmpd