avoid busy waiting and mode switches between realtime and non realtime threading

Question

I have the following problem: we do have a controller implemented with ros_control that runs on a Real Time, Xenomai linux-patched system. The control loop is executed by iteratively calling an update function. I need to communicate some of the internal state of the controller, and for this task I'm using LCM, developed in MIT. Regardless of the internal behaviour of LCM, the publication method is breaking the real time, therefore I've implemented in C++11 a publication loop running on a separated thread. But the loop it is gonna publish at infinite frequency if I don't synchronize the secondary thread with the controller. Therefore I'm using also condition variables.

Here's an example for the controller:

MyClass mc;

// This is called just once
void init(){
    mc.init();
}

// Control loop function (e.g., called every 5 ms in RT)
void update(const ros::Time& time, const ros::Duration& period) {
    double value = time.toSec();
    mc.setValue(value);
}

And for the class which is trying to publish:

double myvalue;
std::mutex mutex;
std::condition_variable cond;
bool go = true;

void MyClass::init(){
    std::thread thread(&MyClass::body, this);
}


void MyClass::setValue(double value){
    myvalue = value;
    {
    std::lock_guard<std::mutex> lk(mutex);
    go = true;
    }
    cond.notify_one();
} 

void MyClass::body() {
    while(true) {
        std::unique_lock<std::mutex>lk(mutex);
        cond.wait(lk, [this] {return go;});    
        publish(myvalue); // the dangerous call
        go = false;
        lk.unlock();
    }
 }

This code produces mode switches (i.e., is breaking real time). Probably because of the lock on the condition variable, which I use to synchronize the secondary thread with the main controller and is in contention with the thread. If I do something like this:

void MyClass::body() {
    while(true) {
        if(go){
        publish(myvalue);
        go = false;            
        }

    }
 }
 void MyClass::setValue(double value){
    myvalue = value;
    go = true;        
} 

I would not produce mode switches, but also it would be unsafe and most of all I would have busy waiting for the secondary thread.

Is there a way to have non-blocking synch between main thread and secondary thread (i.e., having body doing something only when setValue is called) which is also non-busy waiting?

Answer 1

Use a lock free data structure.

In your case here you don't even need a data structure, just use an atomic for go. No locks necessary. You might look into using a semaphore instead of a condition variable to avoid the now-unused lock too. And if you need a semaphore to avoid using a lock you're going to end up using your base OS semaphores, not C++11 since C++11 doesn't even have them.

Answer 2

This isn't perfect, but it should reduce your busy-wait frequency with only occasional loss of responsiveness.

The idea is to use a naked condition variable wake up while passing a message through an atomic.

template<class T>
struct non_blocking_poke {
  std::atomic<T> message;
  std::atomic<bool> active;
  std::mutex m;
  std::condition_variable v;

  void poke(T t) {
    message = t;
    active = true;
    v.notify_one();
  }
  template<class Rep, class Period>
  T wait_for_poke(const std::chrono::duration<Rep, Period>& busy_time) {
    std::unique_lock<std::mutex> l(m);
    while( !v.wait_for(l, busy_time, [&]{ return active; } ))
    {}
    active = false;
    return message;
  }
};

The waiting thread wakes up every busy_time to see if it missed a message. However, it will usually get a message faster than that (there is a race condition where it misses a message). In addition, multiple messages can be sent without the reliever getting them. However, if a message is sent, within about 1 second the receiver will get that message or a later message.

non_blocking_poke<double> poker;

// in realtime thread:
poker.poke(3.14);

// in non-realtime thread:
while(true) {
  using namespace std::literals::chrono_literals;
  double d = poker.wait_for_poke( 1s );
  std::cout << d << '\n';
}

In an industrial quality solution, you'll also want an abort flag or message to stop the loops.