Concurrency issue with ROS C++

edit

Im calibrating three lidars; i subscribe to each one of them, synchronise the data with ros message filters and push them in suitable buffer queues. The issue is that when I call the Callback function in ros style, the ros::Spin() or ros::SpinOnce() is too fast and my computation is not able to complete before the callback refreshes. This causes my ICP function to bug and gives undesired results in the PCL viewer.

I am having a concurrency issue. All I want is once my buffers are not empty, I read from them and perform computation asynchronously in another thread. Running the whole thing in a single thread is causing my sensor data to corrupt thereby leading leading to a dysfunctional ICP iteration, i.e ICP bugs from time to time.

Bottom Line, all I want is a smooth running code with the sensor callback function running at fast rate and my heavy computation occurs in the background whilst the buffer is replenishing.

Here is my sync function

void LidarRegistration::start(){

    lidarfront_.subscribe(nh_, "/front_sick/scan", 10);
    lidarright_.subscribe(nh_, "/right_sick/scan", 10);
    lidarleft_.subscribe(nh_,  "/left_sick/scan", 10);

    sync_.reset(new Sync(MySyncPolicy(100), lidarfront_, lidarright_, lidarleft_));

    sync_->registerCallback(boost::bind(&LidarRegistration::scanCallback, this, _1, _2, _3));}

My callback function goes as follows:

void LidarRegistration::scanCallback (const sensor_msgs::LaserScan::ConstPtr &scan_msg_lf,
                                      const sensor_msgs::LaserScan::ConstPtr &scan_msg_lr,
                                      const sensor_msgs::LaserScan::ConstPtr &scan_msg_ll)
{
     // fill in the scans to pcl format
    this->laserScanToPCL(scan_msg_lf,cl_front_);
    if(frontqueue_.size() == 1000)
        frontqueue_.clear();
    else
       frontqueue_.push(cl_front_);

    cout<<"\033[1;34m frontqueue_ size:  "<<frontqueue_.size() <<"\033[0m"<<endl;

    this->laserScanToPCL(scan_msg_ll,cl_bkleft_);

    if(leftqueue_.size() == 1000)
        leftqueue_.clear();
    else
       leftqueue_.push(cl_bkleft_);

    cout<<"\033[1;34m leftqueue_ size:  "<<leftqueue_.size() <<"\033[0m"<<endl;


    this->laserScanToPCL(scan_msg_lr,cl_bkright_);

    if(rightqueue_.size() == 1000)
        rightqueue_.clear();
    else
       rightqueue_.push(cl_bkright_);
}

Perform registration function

void LidarRegistration::p2plRegistrationOnThread()
{
    //Perform ICP registration of the lidars
}

This is my main:

 int main(int argc, char** argv)
{

    ros::init(argc, argv, "calib_lidar");
    ros::NodeHandle nh;
    ros::NodeHandle nh_private("~");

    // instantiate calibration object
    lidutils::LidarRegistration calibObj (nh, nh_private);

    calibObj.start();

//    // First parameter is pointer to member function of the Class LidarRegistration
//    // Second parametre is pointer to object of the class LidarRegistration
//    // third parametre is variable passed to function (none overhere)
//    std::thread registrationthread(&lidutils::LidarRegistration::registrationThread, &calibObj);

//    ros::spin();

//    registrationthread.join();

    ros::Rate looprate(100);

    while (ros::ok())
    {
        // call registration function here
        calibObj.p2plRegistrationOnThread();
//        ros::spinOnce();
        ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.9));
        looprate.sleep();

    }

  return EXIT_SUCCESS;
}

I'll be grateful for any suggestions; Ive tried it all, buffer management, multithreading, nothing seems to work properly.