Wait main thread until condition

Question

I'm learning C++.

I need to wait to receive some data to start the main thread.

This is the callback function where I'm going to receive the data that I need:

/**
 * Robot's initial position (X component).
 */
double init_pos_x = 0.0;
/**
 * Robot's initial position (Y component).
 */
double init_pos_y = 0.0;

/**
 * Used to get only the robot's initial position.
 */
bool is_init_pos = true;

/**
 * Current robot's X position.
 */
double x = 0.0;
/**
 * Current robot's Y position.
 */
double y = 0.0;
/**
 * Current robot's orientation.
 */
tfScalar theta = 0.0;

/**
 * Command velocity topic's name.
 */
const std::string cmdTopic = "/pioneer2dx/mybot/cmd_vel";
/**
 * Odometry topic's name.
 */
const std::string odomTopic = "/pioneer2dx/mybot/odom_diffdrive";

/**
 * Callback function executes when new topic data comes.
 * Task of the callback function is to print data to screen.
 */
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
  // Update robot position (x, y).
  x = msg->pose.pose.position.x;
  y = msg->pose.pose.position.y;

  // If this is the first position given by the odometry topic, then it is the
  // initial position of the robot.
  if (is_init_pos)
  {
    is_init_pos = false;

    // Set robot's initial position.
    init_pos_x = x;
    init_pos_y = y;
  }

This is the main thread:

int main(int argc, char **argv)
{
  ros::init(argc, argv, "astar_controller");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * Topic where we are going to publish speed commands.
   */
  ros::Publisher cmd_vel_pub = n.advertise<geometry_msgs::Twist>(cmdTopic, 1000);

  /**
   * Topic where we are going to receive odometry messages.
   */
  ros::Subscriber odom_sub = n.subscribe(odomTopic, 1000, odomCallback);

  ros::Rate loop_rate(10);
  //*********************************************
  // I want to wait here.
  //*********************************************

  // More code...
}

I've thought to use a while loop to check if is_init_pos has changed and then continue, but I need to do something inside the while loop (something like a thread.sleep()).

How can I wait to receive the first "message" in odomCallback?

Answer 1

The other two answers are correct when you are running a multithreading application, but this is a Single thread application.

ROS use ros::spinOnce() to:

Process a single round of callbacks.

This method is useful if you have your own loop running and would like to process any callbacks that are available. This is equivalent to calling callAvailable() on the global CallbackQueue. It will not process any callbacks that have been assigned to custom queues.

Thanks to Jonathan Wakely to point me to the right direction. Sorry, I'm learning and I didn't know that.

I finally fixed using this code:

//*********************************************
// I want to wait here.
//*********************************************

while (ros::ok)
{
  ros::spinOnce();
  loop_rate.sleep();

  if (!is_init_pos)
    break;
}

Answer 2

Edit: I don't think you're supposed to use multiple threads. You wait for callbacks by running an "event loop" in the main thread.

  //*********************************************
  // I want to wait here.
  //*********************************************
  ros::spin();

See http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning

---

Original answer follows:

Firstly, the is_init_pos variable needs to be safe to access concurrently from multiple threads. Otherwise if the main thread reads it while the callback thread writes it your program will have undefined behaviour. So use a mutex for synchronizing access to the boolean, and use a condition variable to send a notification when its state changes:

bool is_init_pos = true;
std::mutex init_mutex;
std::condition_variable init_cv;

The callback function should lock that mutex when accessing is_init_pos and send a notification on the condition variable:

  std::lock_guard<std::mutex> lock(init_mutex);
  if (is_init_pos)
  {
    is_init_pos = false;

    // Set robot's initial position.
    init_pos_x = x;
    init_pos_y = y;

    init_cv.notify_one();
  }

Now the main thread can wait on the condition variable to receive a notification when the state changes:

ros::Rate loop_rate(10);

//*********************************************
// I want to wait here.
//*********************************************

// New nested scope that determines the duration of the mutex lock
{
  std::unique_lock<std::mutex> lock(init_mutex);
  auto condition = [] { return is_init_pos == false; };
  init_cv.wait(lock, condition);
}
// first callback happened, OK to proceed now ...

// More code...

The thread will block at the condition_variable::wait call and only return after it is woken by a notification and it sees a true value for the associated condition (defined as a lambda expression testing the boolean variable). While there's no notification or the condition is false it will continue to wait.

---

Edit: P.S. selbie's similar answer might be very slightly more efficient, because only the g_isReady variable is shared between the two threads, so there's no need to lock the mutex in the callback every time, only on the first call:

  if (is_init_pos)
  {
    is_init_pos = false;

    // Set robot's initial position.
    init_pos_x = x;
    init_pos_y = y;

    std::lock_guard<std::mutex> lock(init_mutex);
    g_isReady = true;
    init_cv.notify_one();
  }

and in the main thread use [] { return g_isReady; }; for the condition.

This uses one extra bool variable, but avoids locking the mutex on every callback. It probably won't make much difference in practice, so in my answer I just reused the existing is_init_pos variable that you already have.

Answer 3

Globally, declare a condition variable and a mutex that are shared between the callback thread and main. And a boolean that is under the mutex lock.

#include <mutex>
#include <condition_variable>
std::condition_variable g_cv;
std::mutex g_mutex;
bool g_isReady = false;

Inside your odomCallback, right before it returns, do the following to signal back to main that it can continue

g_mutex.lock();
    g_isReady = true;
g_mutex.unlock();
g_cv.notify_all();

Then, you wait as follows:

  //*********************************************
  // I want to wait here.
  //*********************************************

   // Wait for odomCallback to signal its done

    {
       std::unique_lock<std::mutex> lock(g_mutex);
       while (g_isReady == false)
       {
           g_cv.wait(lock);
       }
    }