How to correctly using mutex in OpenCV-ROS Threads? - c++

Question

I have just started using C++ for some image processing tasks. I want to integrate my RGB (OpenCV Mat) and Depth (PCL) data which I get from ros::Subscribe into colored-pointcloud data.
I use the cv::Mat acquiredImage to hold the transmitted image from ros::Subscribe and then the Mat acquiredImage is used for another processes in another threads, but I am facing segmentation fault. Or the error is shown like this:
[xcb] Unknown sequence number while processing queue
[xcb] Most likely this is a multi-threaded client and XInitThreads has not been called
[xcb] Aborting, sorry about that. Viewtest: ../../src/xcb_io.c:260:
poll_for_event: Assertion `!xcb_xlib_threads_sequence_lost' failed.
Aborted (core dumped)
I have tried using std::mutex but it still doesn't work. Could anyone tell me how to properly manage the cv::Mat in two different threads?
typedef pcl::PointXYZRGB XYZRGB;
typedef pcl::PointCloud<XYZRGB> pclXYZRGB;
typedef pcl::PointCloud<XYZRGB>::Ptr pclXYZRGBptr;
typedef pcl::PointCloud<XYZRGB>::ConstPtr pclXYZRGBcptr;
pclXYZRGBptr acquiredCloud (new pclXYZRGB());
pclXYZRGBptr acquiredCloud2 (new pclXYZRGB());
cv::Mat acquiredImage, acquiredImageRotated;
std::thread thread_RunThread;
std::mutex mutexMutex;
bool stopThread, has_data1, has_data2;
inline float PackRGB(uint8_t r, uint8_t g, uint8_t b) {
uint32_t color_uint = ((uint32_t)r << 16 | (uint32_t) g << 8 | (uint32_t)b);
return *reinterpret_cast<float *> (&color_uint);
}
void RunThread(){
while(ros::ok()){
ros::spinOnce();
}
}
void imageReceive(const sensor_msgs::ImageConstPtr& msg){
mutexMutex.lock();
acquiredImage = cv::Mat(cv_bridge::toCvShare(msg, "bgr8")->image);
mutexMutex.unlock();
has_data1 = true;
}
void cloudReceive(const sensor_msgs::PointCloud2ConstPtr& cloudInMsg){
//mutexMutex.lock();
pcl::fromROSMsg(*cloudInMsg, *acquiredCloud);
has_data2 = true;
//mutexMutex.unlock();
}
void StartThread(){
stopThread = false;
has_data1 = has_data2 = false;
thread_RunThread = std::thread(RunThread);
while(!has_data1 && !has_data2){
std::this_thread::sleep_for(std::chrono::milliseconds(1));
std::cout << has_data1 << "-" << has_data2 << std::endl;
}
}
void CloseThread(){
stopThread = true;
thread_RunThread.join();
}
int main(int argc, char **argv){
ros::init(argc, argv, "Viewtest");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
cv::startWindowThread();
image_transport::Subscriber sub = it.subscribe("/rsCamera/image", 1, imageReceive);
ros::Subscriber pclsubAcquirer = nh.subscribe("/rsCamera/cloud", 1, cloudReceive);
StartThread();
while (ros::ok()){
if(!has_data1 && !has_data2){
std::this_thread::sleep_for(std::chrono::milliseconds(1));
std::cout << has_data1 << "-" << has_data2 << std::endl;
}
else {
mutexMutex.lock();
cv::rotate(acquiredImage, acquiredImageRotated, cv::ROTATE_180);
mutexMutex.unlock();
copyPointCloud(*acquiredCloud, *acquiredCloud2);
int i = 640, j = 480, k;
for (auto& it : acquiredCloud2->points){
it.x = it.x; it.y = it.y; it.z = it.z;
it.rgb = PackRGB(
acquiredImageRotated.at<cv::Vec3b>(j,i)[2], // r
acquiredImageRotated.at<cv::Vec3b>(j,i)[1], // g
acquiredImageRotated.at<cv::Vec3b>(j,i)[0] // b
);
i--;
if(i <= 0) { i = 640; j--; }
if(j < 0) { break; }
}
}
}
CloseThread();
return 0;
}