i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixed deadlock when *.oni reading is finished

Browse Source
This commit is contained in:
matlabbe
2015-04-23 10:49:55 -04:00
parent a72ea25eac
commit c1e15c0f2f
3 changed files with 95 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/CameraRGBD.cpp
+58 -30
View File
@@ -250,23 +250,35 @@ std::string CameraOpenni::getSerial() const
void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{
rgb = cv::Mat();
depth = cv::Mat();
fx=0.0f;
fy=0.0f;
cx=0.0f;
cy=0.0f;
if(interface_ && interface_->isRunning())
{
dataReady_.acquire();
UScopeMutex s(dataMutex_);
if(depthConstant_)
if(!dataReady_.acquire(1, 2000))
{
depth = depth_;
rgb = rgb_;
fx = 1.0f/depthConstant_;
fy = 1.0f/depthConstant_;
cx = float(depth_.cols/2) - 0.5f;
cy = float(depth_.rows/2) - 0.5f;
UWARN("Not received new frames since 2 seconds, end of stream reached!");
}
else
{
UScopeMutex s(dataMutex_);
if(depthConstant_)
{
depth = depth_;
rgb = rgb_;
fx = 1.0f/depthConstant_;
fy = 1.0f/depthConstant_;
cx = float(depth_.cols/2) - 0.5f;
cy = float(depth_.rows/2) - 0.5f;
}
depth_ = cv::Mat();
rgb_ = cv::Mat();
depthConstant_ = 0.0f;
depth_ = cv::Mat();
rgb_ = cv::Mat();
depthConstant_ = 0.0f;
}
}
}
@@ -691,33 +703,49 @@ std::string CameraOpenNI2::getSerial() const
void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{
#ifdef WITH_OPENNI2
rgb = cv::Mat();
depth = cv::Mat();
fx = 0.0f;
fy = 0.0f;
cx = 0.0f;
cy = 0.0f;
int readyStream = -1;
if(_device->isValid() &&
_depth->isValid() &&
_color->isValid() &&
_device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
_device->getSensorInfo(openni::SENSOR_COLOR) != NULL)
{
openni::VideoFrameRef depthFrame, colorFrame;
_depth->readFrame(&depthFrame);
_color->readFrame(&colorFrame);
if(depthFrame.isValid() && colorFrame.isValid())
openni::VideoStream* depthStream[] = {_depth};
openni::VideoStream* colorStream[] = {_color};
if(openni::OpenNI::waitForAnyStream(depthStream, 1, &readyStream, 2000) != openni::STATUS_OK &&
openni::OpenNI::waitForAnyStream(colorStream, 1, &readyStream, 2000) != openni::STATUS_OK)
{
int h=depthFrame.getHeight();
int w=depthFrame.getWidth();
depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
UWARN("No frames received since the last 2 seconds, end of stream is reached!");
}
else
{
openni::VideoFrameRef depthFrame, colorFrame;
_depth->readFrame(&depthFrame);
_color->readFrame(&colorFrame);
if(depthFrame.isValid() && colorFrame.isValid())
{
int h=depthFrame.getHeight();
int w=depthFrame.getWidth();
depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
h=colorFrame.getHeight();
w=colorFrame.getWidth();
cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
cv::cvtColor(tmp, rgb, CV_RGB2BGR);
h=colorFrame.getHeight();
w=colorFrame.getWidth();
cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
cv::cvtColor(tmp, rgb, CV_RGB2BGR);
}
UASSERT(_depthFx != 0.0f && _depthFy != 0.0f);
fx = _depthFx;
fy = _depthFy;
cx = float(depth.cols/2) - 0.5f;
cy = float(depth.rows/2) - 0.5f;
}
UASSERT(_depthFx != 0.0f && _depthFy != 0.0f);
fx = _depthFx;
fy = _depthFy;
cx = float(depth.cols/2) - 0.5f;
cy = float(depth.rows/2) - 0.5f;
}
else
{
corelib/src/CameraThread.cpp
-4
View File
@@ -136,10 +136,6 @@ void CameraThread::mainLoop()
else if(!this->isKilled())
{
if(_cameraRGBD)
{
UERROR("Retrieved data is empty! Stopping the camera...");
}
else
{
UWARN("no more images...");
}
utilite/include/rtabmap/utilite/USemaphore.h
+37 -14
View File
@@ -32,6 +32,7 @@
#define SEM_VALUE_MAX ((int) ((~0u) >> 1))
#else
#include <pthread.h>
#include <sys/time.h>
#endif
/**
@@ -83,30 +84,48 @@ public:
* calling thread will wait until the count acquired is released.
* @see release()
* @param n number to acquire
* @param t time to wait (ms), a value <=0 means infinite
* @return true on success, false on error/timeout
*/
#ifdef _WIN32
void acquire(int n = 1) const
bool acquire(int n = 1, int ms = 0) const
{
while(n-- > 0)
int rt = 0;
while(n-- > 0 && rt==0)
{
WaitForSingleObject((HANDLE)S,INFINITE);
rt = WaitForSingleObject((HANDLE)S, ms<=0?INFINITE:ms);
}
return rt == 0;
}
#else
void acquire(int n = 1)
bool acquire(int n = 1, int ms = 0)
{
int rt = 0;
pthread_mutex_lock(&_waitMutex);
while (n > _available)
while (n > _available && rt == 0)
{
while(1)
if(ms > 0)
{
pthread_cond_wait(&_cond, &_waitMutex);
break;
struct timespec timeToWait;
struct timeval now;
gettimeofday(&now,NULL);
timeToWait.tv_sec = now.tv_sec + ms/1000;
timeToWait.tv_nsec = (now.tv_usec+1000UL*(ms%1000))*1000UL;
rt = pthread_cond_timedwait(&_cond, &_waitMutex, &timeToWait);
}
else
{
rt = pthread_cond_wait(&_cond, &_waitMutex);
}
}
_available -= n;
pthread_mutex_unlock(&_waitMutex);
#endif
return rt == 0;
}
#endif
/*
* Try to acquire the semaphore, not a blocking call.
@@ -116,6 +135,7 @@ public:
int acquireTry() const
{
return ((WaitForSingleObject((HANDLE)S,INFINITE)==WAIT_OBJECT_0)?0:EAGAIN);
}
#else
int acquireTry(int n)
{
@@ -128,8 +148,8 @@ public:
_available -= n;
pthread_mutex_unlock(&_waitMutex);
return true;
#endif
}
#endif
/**
* Release the semaphore, increasing its value by 1 and
@@ -139,6 +159,7 @@ public:
int release(int n = 1) const
{
return (ReleaseSemaphore((HANDLE)S,n,0)?0:ERANGE);
}
#else
void release(int n = 1)
{
@@ -146,8 +167,8 @@ public:
_available += n;
pthread_cond_broadcast(&_cond);
pthread_mutex_unlock(&_waitMutex);
#endif
}
#endif
/**
* Get the USempahore's value.
@@ -157,6 +178,7 @@ public:
int value() const
{
LONG V = -1; ReleaseSemaphore((HANDLE)S,0,&V); return V;
}
#else
int value()
{
@@ -165,8 +187,8 @@ public:
value = _available;
pthread_mutex_unlock(&_waitMutex);
return value;
#endif
}
#endif
#ifdef _WIN32
/*
@@ -182,15 +204,16 @@ public:
#endif
private:
void operator=(const USemaphore &S){}
#ifdef _WIN32
USemaphore(const USemaphore &S){}
HANDLE S;
#else
USemaphore(const USemaphore &S):_available(0){}
pthread_mutex_t _waitMutex;
pthread_cond_t _cond;
int _available;
#endif
void operator=(const USemaphore &S){}
USemaphore(const USemaphore &S){}
};
#endif // USEMAPHORE_H