i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Windows: fixed CameraRealSense2 driver freezing on close.

Browse Source
This commit is contained in:
matlabbe
2020-07-01 20:58:04 -04:00
parent 736cf0efc7
commit cd10cafd0e
3 changed files with 15 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/camera/CameraRealSense2.h
View File

@@ -134,6 +134,7 @@ private:
bool dualMode_; bool dualMode_;
Transform dualExtrinsics_; Transform dualExtrinsics_;
std::string jsonConfig_; std::string jsonConfig_;
bool closing_;
static Transform realsense2PoseRotation_; static Transform realsense2PoseRotation_;
static Transform realsense2PoseRotationInv_; static Transform realsense2PoseRotationInv_;

3
corelib/src/CameraThread.cpp
View File

@@ -74,7 +74,6 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
CameraThread::~CameraThread() CameraThread::~CameraThread()
{ {
UDEBUG("");
join(true); join(true);
delete _camera; delete _camera;
delete _distortionModel; delete _distortionModel;
@@ -139,7 +138,6 @@ void CameraThread::mainLoopBegin()
void CameraThread::mainLoop() void CameraThread::mainLoop()
{ {
UTimer totalTime; UTimer totalTime;
UDEBUG("");
CameraInfo info; CameraInfo info;
SensorData data = _camera->takeImage(&info); SensorData data = _camera->takeImage(&info);
@@ -161,7 +159,6 @@ void CameraThread::mainLoop()
void CameraThread::mainLoopKill() void CameraThread::mainLoopKill()
{ {
UDEBUG("");
if(dynamic_cast<CameraFreenect2*>(_camera) != 0) if(dynamic_cast<CameraFreenect2*>(_camera) != 0)
{ {
int i=20; int i=20;

16
corelib/src/camera/CameraRealSense2.cpp
View File

@@ -77,7 +77,8 @@ CameraRealSense2::CameraRealSense2(
cameraHeight_(480), cameraHeight_(480),
cameraFps_(30), cameraFps_(30),
publishInterIMU_(false), publishInterIMU_(false),
dualMode_(false) dualMode_(false),
closing_(false)
#endif #endif
{ {
UDEBUG(""); UDEBUG("");
@@ -86,12 +87,15 @@ CameraRealSense2::CameraRealSense2(
CameraRealSense2::~CameraRealSense2() CameraRealSense2::~CameraRealSense2()
{ {
#ifdef RTABMAP_REALSENSE2 #ifdef RTABMAP_REALSENSE2
closing_ = true;
try try
{ {
UDEBUG("Closing device(s)...");
for(size_t i=0; i<dev_.size(); ++i) for(size_t i=0; i<dev_.size(); ++i)
{ {
if(dev_[i]) if(dev_[i])
{ {
UDEBUG("Closing %d sensor(s) from device %d...", (int)dev_[i]->query_sensors().size(), (int)i);
for(rs2::sensor _sensor : dev_[i]->query_sensors()) for(rs2::sensor _sensor : dev_[i]->query_sensors())
{ {
try try
@@ -104,6 +108,7 @@ CameraRealSense2::~CameraRealSense2()
UWARN("%s", error.what()); UWARN("%s", error.what());
} }
} }
dev_[i]->hardware_reset(); // To avoid freezing on some Windows computers in the following destructor
delete dev_[i]; delete dev_[i];
} }
} }
@@ -519,7 +524,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
{ {
if (info.was_removed(*dev_[i])) if (info.was_removed(*dev_[i]))
{ {
UERROR("The device has been disconnected!"); if (closing_)
{
UDEBUG("The device %d has been disconnected!", i);
}
else
{
UERROR("The device %d has been disconnected!", i);
}
} }
} }
} }