NCARAI ~ Intelligent Systems
|NRL / Systems / ITD / NCARAI / ISS / Segway RMP / Wax Port||NRL Resources|
Porting Wax to the Segway RMP
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
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
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