Windows: fixed CameraRealSense2 driver freezing on close.

Author: matlabbe

corelib/include/rtabmap/core/camera/CameraRealSense2.h

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

corelib/src/CameraThread.cpp

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

corelib/src/camera/CameraRealSense2.cpp

@@ -77,7 +77,8 @@ CameraRealSense2::CameraRealSense2(
 	cameraHeight_(480),
 	cameraFps_(30),
 	publishInterIMU_(false),
-	dualMode_(false)
+	dualMode_(false),
+	closing_(false)
 #endif
 {
 	UDEBUG("");
@@ -86,12 +87,15 @@ CameraRealSense2::CameraRealSense2(
 CameraRealSense2::~CameraRealSense2()
 {
 #ifdef RTABMAP_REALSENSE2
+	closing_ = true;
 	try
 	{
+		UDEBUG("Closing device(s)...");
 		for(size_t i=0; i<dev_.size(); ++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())
 				{
 					try
@@ -104,6 +108,7 @@ CameraRealSense2::~CameraRealSense2()
 						UWARN("%s", error.what());
 					}
 				}
+				dev_[i]->hardware_reset(); // To avoid freezing on some Windows computers in the following destructor
 				delete dev_[i];
 			}
 		}
@@ -519,7 +524,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			{
 				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);
+					}
 				}
 			}
 		}