IPC
For our project, the Football Table a synchronized high speed inter process communication(IPC) was necessary. For us to allow communication between our Motion Loop (in Simulink) and Gazebo. For simulation purposes we ideally want to run as fast possible, whilst retaining a causal link.
The library/communication makes use of the POSIX pthread library. Pthread was chosen for its compatibility with Matlab. For this work a very basic wrapper library was made for this project to deal clean up recurring parts of code. This library basically dumbs pthreads down a bit, making it more accesible. For advanced use i do not recommend using this. In our 'library' we used shared memory in combination with a mutex and a condition. For the sake of usability/adaptability this article will very shortly first discuss the basic principles used.
Shared memory is one of the fastest methods available for inter process communication. Basically a segment of memory gets allocated to which multiple threads/programs have access. This way the processes can exchange data and synchronize with each other. Using shared memory however, is slightly tricky because a simultaneous read/write will cause undefined behavior (most likely a crash). To manage this, mutexes are used.
Mutexes
A mutex, or mutual exclusion, does exactly what its name describes. It provides a thread/program with exclusive access to a shared resource. It attains this exclusive access by 'locking' the mutex. In our case, we use it to control access to shared memory. As long as one process has a lock, no other process can access the data (using a basic mutex), hence ensuring thread-save behavior. There are other types of mutexes which operate slightly different (such as a readers-writers lock). By default if a process cannot get a lock, it will wait until it does. If you do not carefully programs you can easily get deadlocks.
Conditions
Synchronizing programs/threads one program might not be able to continue without data provided by the other. Conditions are a wonderful tool to perform this type of synchronization. They are always used in combination with a mutex and some condition variable (which can be anything). It's use is best described with an short example:
Process 1 will lock the shared memory, check if a condition is met, find out is not (process 2 has not updated data yet). It will wait (on a condition_signal) and automatically unlock the mutex. Once process 2 is ready to update it: locks the mutex, updates the data, signals process 1 (waking it) and unlocks the mutex. This is summarized in the table below for two processes counting together.
| Process 1 | SHM Count | Process 2 |
|---|---|---|
| Lock SHM | 1 | Can't Lock |
| if (uneven) wait; else count++ | 1 | Lock SHM |
| waiting | 1 >> | if (even) wait; else count++ |
| waiting | 2 << | Unlock + Signal |
| Done waiting. Received lock | <- 2 | Can't Lock |
| Count++ | >> 3 | Can't Lock |
| Unlock | 3 | Lock SHM |
| Can't Lock | 4 << | if (even) wait; else count++ |
Usage in a feedback loop
Now, how do we use all this? Our goal is to place gazebo as the plant in our feedback loop in simulink and run as fast as possible.
As can be seen, gazebo now serves as the plant.
libshm_ipc
The library for this can be found on the Football Table SVN, called libshm_ipc. This is basically, thread-safe IPC for beginners, all functions are made so that the shared memory is not accessed unless a lock is achieved. The library functions should be quite self explanatory, however here i'll show a short example on how to use it. It features two classes: a client and a server. First we start a server
IPCServer shm_server(shm_key);
In
int shmid = shm_server.init()
mdlOutput we read-out the positions:
int timeout = 2; //time-out in full seconds
IPClient shm_client; //create an IPC client
shm_client.attach(shm_key);
shm_client.lock(); //First we attempt to lock the shared memory
int ret = shm_client.flagWait(array_index,false,timeout); //We wait if the flag associated with array_index is false (giving up lock)
if(ret==ETIMEDOUT){printf("Timed out \n"); ssSetStopRequested(S,1); //If we timed-out we tell simulink to abort the simulation
}else{ //If we are done waiting re-attaining lock, we start to read data (assign it to an output is this case)
for(int i=0; i<8; i++){
y[i] = shm_client.getData(i,0,array_index);//Reading and writing is done using functions usage: getData(row,column,array_index)
}
}
ret = shm_client.flagUpdate(array_index,false); //Here we reset the flag back to false, indicated that we've read it (no longer up-to-date)
shm_client.unlock();
In mdlTerminate we destroy the shared memory
IPCClient shm_client_term;
shm_client_term.attach(shm_key);
shm_client_term.lock();
shm_client_term.setProcessFlag(false); //Tell the other process(es) that we are about to destory SHM.
shm_client_term.unlock();
shm_client_term.dettach();
sleep(0.5);//Allow gazebo to finish. On a control+c this will allow gazebo to quit, however it will not allow matlab to detach.
IPCServer shm_server(shm_key);
shm_server.destroy(); //Destroy the server
The other program(e.g. gazebo) could do the opposite (wait if the flag is true -> write the position to the shm -> reset to true). This way both processes will take turns: one reading, the other writing. For a complete control loop we have matlab writing control commands and reading postions, the simulator does the opposite. In the real simulink file, we use (in mdlUpdate) to write the new torque to shm.
Apart from the flags belonging to the data, there is also a process flag. This process flag is there to warn the other process the shared memory is going to be destroyed. This allows a safe cleanup, as you can end your program based on this flag.
Gazebo plugin
The gazebo plugin does what was described in the previous paragraph:
OnUpdateStart()Update all positions (including the ball).OnUpdateEnd()Apply all torques (possible reset ball).
Apart from updating positions and applying torques, the plugin continuously checks the process flag shm_client.getProcessFlag(). Once the process flag is set false, it will terminate the simulation/plugin.
This plugin is universal, meaning that you can use it on any robot. There is only one part of code specific to the table, is that part where we reset the ball. This part can be commented out or re-purposed, it contains a hard-coded name in the code (of the ball).
In order to use the plugin in on your simulation, it is important that the order in which you define your joints in the *.sdf file is the order in which the torques are assigned. Each link has an associated column, in which the first row is either the torque or the position (place in seperate arrays), #define _N_COLS to make sure there are enough columns to allocate all control signals. the other rows in these arrays are currently used for e.g. resetting the ball.