Cannot reconnect to Realsense D435 once unplugged and plugged again

[mynickmynick]

---

Required Info
Camera Model	{ D400 }

| Operating System & Version | {Win10 |
|
| Platform | PC Dell |

| Language | {C++ } |

Issue Description

I have implemented a state machine to connect and stream and process frames from my realsense d435 camera. If i start the application with the camera connected everything goes well. When I unplug the camera I get errors as expected but when I reconnect I keep getting exceptions/errors and cannpot reset the connection and stream. I have tried several combinations of comands and sleeps to recover, none seems to work and always leads to the following cyclic error messages

I will paste here before my log and after a C++ code extract with the status machine, thank you very much for help

Status 0 -> 20
Succesfully set Emitter Laser
Succesfully set Emitter Laser max power = 360

------------- GRAB+MEDIAN TIME: 0.088 seconds.

------------- SEGMENTATION: 0.272 seconds.
Found a nr of Cluster: 6

--CLUSTERING LAST PART + optional HULLS TIME: 0.006 seconds.

------------- -------------------- INERTIA TIME: 1.665 msec.
Loaded cloud width*height = 266142 w,h= 266142 , 1
cloud is_dense= 0
cloud isOrganized= 0

------------- GRAB+MEDIAN TIME: 0.078 seconds.

------------- SEGMENTATION: 0.251 seconds.
Found a nr of Cluster: 5

--CLUSTERING LAST PART + optional HULLS TIME: 0.005 seconds.

------------- -------------------- INERTIA TIME: 1.772 msec.
Loaded cloud width*height = 266542 w,h= 266542 , 1
cloud is_dense= 0
cloud isOrganized= 0

.........

<<<<<<< HERE I UNPLUG THE DEVICE >>>>>>>>>>>>>>>

wait_for_frames Exception Device disconnected. Failed to recconect: No device connected8000
Status 0 -> 20
Exception-- MFCreateDeviceSource(_device_attrs, &_source) returned: HResult 0x80070003: "The system cannot find the path specified."
Realsense not found
Status 0 -> 20
Exception-- MFCreateDeviceSource(_device_attrs, &_source) returned: HResult 0x80070003: "The system cannot find the path specified."
Realsense not found
Status 0 -> 20

<<<<<<<<< HERE I PLUG AGAIN THE DEVICE >>>>>>>>>>>>>>>

Exception--
Failed to resolve the request:
Format: Z16, width: 1280, height: 720

Into:
Device is already streaming!
Realsense not found
Status 0 -> 20
Exception--
Failed to resolve the request:
Format: Z16, width: 1280, height: 720

Into:
Device is already streaming!
Realsense not found

------CODE EXTRACT ------

int RealSense::GrabAndShow()
{

cloud_pointer _cloud;//grabbed or load from file
cloud_pointer _cloudAverage = cloud_pointer(new pcl::PointCloud<pcl::PointXYZRGB>);//grabbed or load from file



//try 
{
	bool captureLoop = true;
	//======================
	// Stream configuration with parameters resolved internally. See enable_stream() overloads for extended usage
	//======================
	cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_BGR8, 30);//30
	cfg.enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_ANY, 30);//30

	rs2::config cfg_reset;
	cfg_reset.disable_all_streams();

	rs2::pipeline_profile selection;
	rs2::device selected_device;

	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addCoordinateSystem(0.1);
	viewer->initCameraParameters();

	int Status = 0;

	int i = 0;
	shared_ptr<rs2::pipeline> pipe=std::make_shared<rs2::pipeline>();
	while (captureLoop) // Application still alive?
	{
		// Declare RealSense pipeline, encapsulating the actual device and sensors

		float AverageLen = globalState.AverageLen_3D.get();
		if (!((int)AverageLen % 2)) AverageLen+=1;
		
		float*** Xvec ;
		float*** Yvec ;
		float*** Zvec ;
		bool skip = false;
		auto begin = std::chrono::high_resolution_clock::now();//GRAB + MEDIAN
		for (int ii = 1; ii <= AverageLen; ++ii)
		{
			skip = false;
			bool loadFromFile3D=globalState.loadFromFile3D.get();
			int StatusOld = Status;
			switch (Status) 
			{
				case 0://init
				{
					if (loadFromFile3D)
					{
						Status = 10;
						//cout << "Status 0 -> 10\n";
					}
					else
					{
						this_thread::sleep_for(1000ms);
						//pipe.reset(new rs2::pipeline);
						pipe=std::make_shared<rs2::pipeline>();
						this_thread::sleep_for(500ms);
						try { pipe->start(cfg_reset); }
						catch (...) {;}
						this_thread::sleep_for(500ms);
						try { pipe->stop(); }
						catch (...) {;}
						this_thread::sleep_for(500ms);
						Status = 20;
						cout << "Status 0 -> 20\n";
					}
					break;
				}
				case 10:// load from file
				{
					if (loadFromFile3D)
					{
#ifdef DETAILED_LOG
						auto begin = std::chrono::high_resolution_clock::now();
#endif
						// Load a single frame and obtain depth + RGB values from it    
						string openFileName;
						openFileName = "Captured.pcd";
						pcl::io::loadPCDFile(openFileName, *_cloud); // Load .pcd File

#ifdef DETAILED_LOG
						auto end = std::chrono::high_resolution_clock::now();
						auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
						printf("\n ------------- GRAB from file TIME: %.3f seconds.\n", elapsed.count() * 1e-9);
#endif
					}
					else
						Status = 0;
					
					break;
				}
				case 20:
				{
					if (!loadFromFile3D)
					{
						try {
							
							selection = pipe->start(cfg);
							if (!selection)
							{
								Status = 0;
								try {
									pipe->stop();
								}
								catch (...) { ; }
								this_thread::sleep_for(500ms);
								try {
									pipe.reset();
								}
								catch (...) { ; }
								break;
							}
							selected_device = selection.get_device();
							this_thread::sleep_for(500ms);
						}
						catch (exception e)
						{
							cout << "Exception-- " << e.what() << endl;
							cout << "Realsense not found " << endl;
							Status = 0;
							try {
									pipe->stop();
							}
							catch (...) { ; }
							this_thread::sleep_for(500ms);
							try {
									pipe.reset();
							}
							catch (...) { ; }
							//pipe.reset(new rs2::pipeline);
							break;
						}


						auto depth_sensor = selected_device.first<rs2::depth_sensor>();
						if (depth_sensor.supports(RS2_OPTION_EMITTER_ENABLED))
						{
							depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 1.f); // Enable emitter
							pipe->wait_for_frames();
							//depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 0.f); // Disable emitter
							if (depth_sensor.get_option(RS2_OPTION_EMITTER_ENABLED) == 1.0)
								cout << "Succesfully set Emitter Laser\n";
							else
								cout << "ERROR:: Laser emitter enable option = " << depth_sensor.get_option(RS2_OPTION_EMITTER_ENABLED) << endl;
						}

						if (depth_sensor.supports(RS2_OPTION_LASER_POWER))
						{
							// Query min and max values:
							auto range = depth_sensor.get_option_range(RS2_OPTION_LASER_POWER);
							depth_sensor.set_option(RS2_OPTION_LASER_POWER, range.max); // Set max power
							std::this_thread::sleep_for(1s);
							//depth_sensor.set_option(RS2_OPTION_LASER_POWER, 0.f); // Disable laser
							if (depth_sensor.get_option(RS2_OPTION_LASER_POWER) == range.max)
								cout << "Succesfully set Emitter Laser max power = " << range.max << endl;
							else
								cout << "ERROR:: Emitter Laser max power = " << depth_sensor.get_option(RS2_OPTION_LASER_POWER) << endl;
						}
						this_thread::sleep_for(500ms);
						Status = 30;
					}
					else
						Status = 0;

					break;
				}
				case 30:
				{
					if (loadFromFile3D)
					{
						try {
							pipe->stop();
						}
						catch (...) { ; }
						this_thread::sleep_for(500ms);
						try {
									pipe.reset();
						}
						catch (...) { ; }
						//pipe.reset(new rs2::pipeline);
						Status = 0;
						break;
					}
					else
					{
		#ifdef DETAILED_LOG
							auto begin = std::chrono::high_resolution_clock::now();
		#endif

							// Capture a single frame and obtain depth + RGB values from it    

							
							rs2::frameset frames;
							try {
								 frames = pipe->wait_for_frames(8000);
							}
							catch (exception e)
							{
								cout << "wait_for_frames Exception " << e.what() << endl;
								Status = 0;
								try {
									pipe->stop();
								}
								catch (...) { ; }
								this_thread::sleep_for(500ms);
								try {
									pipe.reset();
								}
								catch (...) { ; }
								//pipe.reset(new rs2::pipeline);
								break;
							}

		#ifdef DETAILED_LOG
							auto end = std::chrono::high_resolution_clock::now();
							auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
							printf("\n ------------- GRABBING TIME: %.3f seconds.\n", elapsed.count() * 1e-9);

							begin = std::chrono::high_resolution_clock::now();
		#endif
							auto depth = frames.get_depth_frame();
							auto RGB = frames.get_color_frame();

		#ifdef DETAILED_LOG
							end = std::chrono::high_resolution_clock::now();
							elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
							printf("\n ------------- FRAMES EXTRACTION TIME: %.3f seconds.\n", elapsed.count() * 1e-9);

							begin = std::chrono::high_resolution_clock::now();
		#endif
							// Map Color texture to each point
							pc.map_to(RGB);

							// Generate Point Cloud
							auto points = pc.calculate(depth);

							// Convert generated Point Cloud to PCL Formatting
							_cloud = PCL_Conversion(points, RGB);

		#ifdef DETAILED_LOG
							end = std::chrono::high_resolution_clock::now();
							elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
							printf("\n ------------- FRAMES TO PCL CONV TIME: %.3f seconds.\n", elapsed.count() * 1e-9);
		#endif

			
					}
					break;
				}


			}



			if (StatusOld == 0 || Status == 0 || StatusOld == 20)
			{
				skip = true;
				break;
			}


			//Median on XYZ and average on RGB
			if (_cloud->isOrganized() && AverageLen > 1)
			{



				if (ii == 1)
				{

					Xvec = new float** [_cloud->height];
					Yvec = new float** [_cloud->height];
					Zvec = new float** [_cloud->height];
					for (size_t s = 0; s < _cloud->height; ++s)
					{
						Xvec[s] = new float*[_cloud->width];
						Yvec[s] = new float*[_cloud->width];
						Zvec[s] = new float*[_cloud->width];

						for (size_t s2 = 0; s2 < _cloud->width; ++s2)
						{
							Xvec[s][s2] = new float [AverageLen];
							memset(Xvec[s][s2], 0, sizeof(float)* AverageLen);
							Yvec[s][s2] = new float [AverageLen];
							memset(Xvec[s][s2], 0, sizeof(float)* AverageLen);
							Zvec[s][s2] = new float [AverageLen];
							memset(Xvec[s][s2], 0, sizeof(float)* AverageLen);
						}

					}


					_cloudAverage->points.resize(_cloud->height * _cloud->width);
					_cloudAverage->width = _cloud->width;
					_cloudAverage->height = _cloud->height;
					_cloudAverage->is_dense = _cloud->is_dense;
					// traverse the depth map
					for (int n = 0; n < _cloud->height; n++)//to be checked//righe
					{
						for (int m = 0; m < _cloud->width; m++)
						{
							//_cloudAverage->at(m, n).x = _cloud->at(m, n).x;
							//_cloudAverage->at(m, n).y = _cloud->at(m, n).y;
							//_cloudAverage->at(m, n).z = _cloud->at(m, n).z;

							_cloudAverage->at(m, n).b = _cloud->at(m, n).b;
							_cloudAverage->at(m, n).r = _cloud->at(m, n).r;
							_cloudAverage->at(m, n).g = _cloud->at(m, n).g;

							Xvec[n][m][ii] = _cloud->at(m, n).x;
							Yvec[n][m][ii] = _cloud->at(m, n).y;
							Zvec[n][m][ii] = _cloud->at(m, n).z;

						}
					}
				}
				else
				{
					// traverse the depth map
					for (int n = 0; n < _cloud->height; n++)//to be checked//righe
					{
						for (int m = 0; m < _cloud->width; m++)
						{
							//_cloudAverage->at(m, n).x = (1.0 - AverageLenInverse) * _cloudAverage->at(m, n).x + AverageLenInverse * _cloud->at(m, n).x;
							//_cloudAverage->at(m, n).y = (1.0 - AverageLenInverse) * _cloudAverage->at(m, n).y + AverageLenInverse * _cloud->at(m, n).y;
							//_cloudAverage->at(m, n).z = (1.0 - AverageLenInverse) * _cloudAverage->at(m, n).z + AverageLenInverse * _cloud->at(m, n).z;
							//PROBLEM IF UNORGANIZED
							_cloudAverage->at(m, n).b = (1.0 - 1.0 / ((float)ii)) * ((float)_cloudAverage->at(m, n).b) + (1.0 / ((float)ii)) * ((float)_cloud->at(m, n).b);
							_cloudAverage->at(m, n).r = (1.0 - 1.0 / ((float)ii)) * ((float)_cloudAverage->at(m, n).r) + (1.0 / ((float)ii)) * ((float)_cloud->at(m, n).r);
							_cloudAverage->at(m, n).g = (1.0 - 1.0 / ((float)ii)) * ((float)_cloudAverage->at(m, n).g) + (1.0 / ((float)ii)) * ((float)_cloud->at(m, n).g);

							Xvec[n][m][ii] = _cloud->at(m, n).x;
							Yvec[n][m][ii] = _cloud->at(m, n).y;
							Zvec[n][m][ii] = _cloud->at(m, n).z;
						}
					}
				}
			}
			else
				_cloudAverage = _cloud;

		
		}

		if (skip)
			continue;

		if (_cloud->isOrganized() && AverageLen > 1)
			for (int n = 0; n < _cloud->height; n++)//righe
			{
				for (int m = 0; m < _cloud->width; m++)
				{
					_cloudAverage->at(m, n).x = findMedian(Xvec[n][m], AverageLen);
					_cloudAverage->at(m, n).y = findMedian(Yvec[n][m], AverageLen);
					_cloudAverage->at(m, n).z = findMedian(Zvec[n][m], AverageLen);
				}
			}

		auto end = std::chrono::high_resolution_clock::now();
		auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
		printf("\n ------------- GRAB+MEDIAN TIME: %.3f seconds.\n", elapsed.count() * 1e-9);


		cloud_pointer transformed_cloud(new PointCloud<PointXYZRGB>());





		Detection
				(_cloudAverage, transformed_cloud, viewer, i, globalState.showRgbPclView.get());
		++i;

//#ifdef DETAILED_LOG
cout << "Loaded cloud width*height = "
<< _cloud->width * _cloud->height
<< " w,h= " << _cloud->width << " , " << _cloud->height << std::endl;
cout << "cloud is_dense= " << _cloud->is_dense << std::endl;
cout << "cloud isOrganized= " << _cloud->isOrganized() << std::endl;
//#endif

		if (0==i%globalState.SaveOnFile3DGrabsPeriod.get() && globalState.saveOnFile3D.get())// && !globalState.loadFromFile3D.get())
		{
			cout << "saving in ascii format \n";
			pcl::io::savePCDFile<pcl::PointXYZRGB>( "pclRGB_captured_"+to_string(i%100)+".pcd", *_cloud, false);
			pcl::io::savePCDFile<pcl::PointXYZRGB>("pclRGB_captured_average" + to_string(i % 100) + ".pcd", *_cloudAverage, false);
		}

		this_thread::sleep_for(100ms);

	}

}
/*catch (exception e)
{
	cout<<"Exception "<<e.what()<<endl;//to be put in a log
	return(1);
}
*/

return(0);

}

[mynickmynick]

sorry
it seems that i already managed to solve the issue by improving the reset changing the status 0
and also ( I guess that did not help) by localizing the definitions of rs2::config
inside the states

as follows,

but I would really appreciate a critical opinion or even fast review of this code , thanks a lot
I will proceed in pruning the code from probably unnecesary or excessive sleeps
is it really necessary such a heavy stop+ reset+ start with disable all streams +stop again +reset again + restart again???
would it be useful to provide more simple handling of connection restore or is it just me that didn't nail the right sequence??
thank you very much

int RealSense::GrabAndShow()
{

cloud_pointer _cloud;//grabbed or load from file
cloud_pointer _cloudAverage = cloud_pointer(new pcl::PointCloud<pcl::PointXYZRGB>);//grabbed or load from file

{
	bool captureLoop = true;

	rs2::pipeline_profile selection;
	rs2::device selected_device;

	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addCoordinateSystem(0.1);
	viewer->initCameraParameters();

	int Status = 0;
	int i = 0;
	shared_ptr<rs2::pipeline> pipe=std::make_shared<rs2::pipeline>();
	while (captureLoop) // Application still alive?
	{

		float AverageLen = globalState.AverageLen_3D.get();
		if (!((int)AverageLen % 2)) AverageLen+=1;

		float*** Xvec ;
		float*** Yvec ;
		float*** Zvec ;
		bool skip = false;
		auto begin = std::chrono::high_resolution_clock::now();//GRAB + MEDIAN
		for (int ii = 1; ii <= AverageLen; ++ii)
		{
			skip = false;
			bool loadFromFile3D=globalState.loadFromFile3D.get();
			int StatusOld = Status;
			switch (Status) 
			{
				case 0://init
				{
					if (loadFromFile3D)
					{
						Status = 10;
					}
					else
					{
						this_thread::sleep_for(1000ms);
						pipe=std::make_shared<rs2::pipeline>();
						this_thread::sleep_for(500ms);
						rs2::config cfg_reset;
						cfg_reset.disable_all_streams();

						try { pipe->start(cfg_reset); }
						catch (...) {;}
						this_thread::sleep_for(500ms);
						try { pipe->stop(); }
						catch (...) {;}
						this_thread::sleep_for(500ms);
						pipe.reset();
						pipe=std::make_shared<rs2::pipeline>();
						this_thread::sleep_for(500ms);
						Status = 20;
						cout << "Status 0 -> 20\n";
					}
					break;
				}
				case 10:// load from file
				{
					if (loadFromFile3D)
					{
#ifdef DETAILED_LOG
						auto begin = std::chrono::high_resolution_clock::now();
#endif
						// Load a single frame and obtain depth + RGB values from it    
						string openFileName;
						openFileName = "Captured.pcd";
						pcl::io::loadPCDFile(openFileName, *_cloud); // Load .pcd File

#ifdef DETAILED_LOG
						auto end = std::chrono::high_resolution_clock::now();
						auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
						printf("\n ------------- GRAB from file TIME: %.3f seconds.\n", elapsed.count() * 1e-9);
#endif
					}
					else
						Status = 0;
					
					break;
				}
				case 20:
				{
					if (!loadFromFile3D)
					{
						try {
							rs2::config cfg;
							cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_BGR8, 30);//30
							cfg.enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_ANY, 30);//30
							selection = pipe->start(cfg);
							if (!selection)
							{
								Status = 0;
								try {
									pipe->stop();
								}
								catch (...) { ; }
								this_thread::sleep_for(500ms);
								try {
									pipe.reset();
								}
								catch (...) { ; }
								break;
							}
							selected_device = selection.get_device();
							this_thread::sleep_for(500ms);
						}
						catch (exception e)
						{
							cout << "Exception-- " << e.what() << endl;
							cout << "Realsense not found " << endl;
							Status = 0;
							try {
									pipe->stop();
							}
							catch (...) { ; }
							this_thread::sleep_for(500ms);
							try {
									pipe.reset();
							}
							catch (...) { ; }
							break;
						}


						auto depth_sensor = selected_device.first<rs2::depth_sensor>();
						if (depth_sensor.supports(RS2_OPTION_EMITTER_ENABLED))
						{
							depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 1.f); // Enable emitter
							pipe->wait_for_frames();
							if (depth_sensor.get_option(RS2_OPTION_EMITTER_ENABLED) == 1.0)
								cout << "Succesfully set Emitter Laser\n";
							else
								cout << "ERROR:: Laser emitter enable option = " << depth_sensor.get_option(RS2_OPTION_EMITTER_ENABLED) << endl;
						}

						if (depth_sensor.supports(RS2_OPTION_LASER_POWER))
						{
							// Query min and max values:
							auto range = depth_sensor.get_option_range(RS2_OPTION_LASER_POWER);
							depth_sensor.set_option(RS2_OPTION_LASER_POWER, range.max); // Set max power
							std::this_thread::sleep_for(1s);
							if (depth_sensor.get_option(RS2_OPTION_LASER_POWER) == range.max)
								cout << "Succesfully set Emitter Laser max power = " << range.max << endl;
							else
								cout << "ERROR:: Emitter Laser max power = " << depth_sensor.get_option(RS2_OPTION_LASER_POWER) << endl;
						}
						this_thread::sleep_for(500ms);
						Status = 30;
					}
					else
						Status = 0;

					break;
				}
				case 30:
				{
					if (loadFromFile3D)
					{
						try {
							pipe->stop();
						}
						catch (...) { ; }
						this_thread::sleep_for(500ms);
						try {
									pipe.reset();
						}
						catch (...) { ; }
						//pipe.reset(new rs2::pipeline);
						Status = 0;
						break;
					}
					else
					{
		#ifdef DETAILED_LOG
							auto begin = std::chrono::high_resolution_clock::now();
		#endif

							// Capture a single frame and obtain depth + RGB values from it    
							rs2::frameset frames;
							try {
								 frames = pipe->wait_for_frames(8000);
							}
							catch (exception e)
							{
								cout << "wait_for_frames Exception " << e.what() << endl;
								Status = 0;
								try {
									pipe->stop();
								}
								catch (...) { ; }
								this_thread::sleep_for(500ms);
								try {
									pipe.reset();
								}
								catch (...) { ; }
								break;
							}

		#ifdef DETAILED_LOG
							auto end = std::chrono::high_resolution_clock::now();
							auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
							printf("\n ------------- GRABBING TIME: %.3f seconds.\n", elapsed.count() * 1e-9);

							begin = std::chrono::high_resolution_clock::now();
		#endif
							auto depth = frames.get_depth_frame();
							auto RGB = frames.get_color_frame();

		#ifdef DETAILED_LOG
							end = std::chrono::high_resolution_clock::now();
							elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
							printf("\n ------------- FRAMES EXTRACTION TIME: %.3f seconds.\n", elapsed.count() * 1e-9);

							begin = std::chrono::high_resolution_clock::now();
		#endif
							// Map Color texture to each point
							pc.map_to(RGB);
							// Generate Point Cloud
							auto points = pc.calculate(depth);
							// Convert generated Point Cloud to PCL Formatting
							_cloud = PCL_Conversion(points, RGB);

		#ifdef DETAILED_LOG
							end = std::chrono::high_resolution_clock::now();
							elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
							printf("\n ------------- FRAMES TO PCL CONV TIME: %.3f seconds.\n", elapsed.count() * 1e-9);
		#endif

					}
					break;
				}

			}

			if (StatusOld == 0 || Status == 0 || StatusOld == 20)
			{
				skip = true;
				break;
			}

			//Median on XYZ and average on RGB
			if (_cloud->isOrganized() && AverageLen > 1)
			{

				if (ii == 1)
				{

					Xvec = new float** [_cloud->height];
					Yvec = new float** [_cloud->height];
					Zvec = new float** [_cloud->height];
					for (size_t s = 0; s < _cloud->height; ++s)
					{
						Xvec[s] = new float*[_cloud->width];
						Yvec[s] = new float*[_cloud->width];
						Zvec[s] = new float*[_cloud->width];

						for (size_t s2 = 0; s2 < _cloud->width; ++s2)
						{
							Xvec[s][s2] = new float [AverageLen];
							memset(Xvec[s][s2], 0, sizeof(float)* AverageLen);
							Yvec[s][s2] = new float [AverageLen];
							memset(Xvec[s][s2], 0, sizeof(float)* AverageLen);
							Zvec[s][s2] = new float [AverageLen];
							memset(Xvec[s][s2], 0, sizeof(float)* AverageLen);
						}

					}


					_cloudAverage->points.resize(_cloud->height * _cloud->width);
					_cloudAverage->width = _cloud->width;
					_cloudAverage->height = _cloud->height;
					_cloudAverage->is_dense = _cloud->is_dense;
					// traverse the depth map
					for (int n = 0; n < _cloud->height; n++)//to be checked//righe
					{
						for (int m = 0; m < _cloud->width; m++)
						{
							//_cloudAverage->at(m, n).x = _cloud->at(m, n).x;
							//_cloudAverage->at(m, n).y = _cloud->at(m, n).y;
							//_cloudAverage->at(m, n).z = _cloud->at(m, n).z;

							_cloudAverage->at(m, n).b = _cloud->at(m, n).b;
							_cloudAverage->at(m, n).r = _cloud->at(m, n).r;
							_cloudAverage->at(m, n).g = _cloud->at(m, n).g;

							Xvec[n][m][ii] = _cloud->at(m, n).x;
							Yvec[n][m][ii] = _cloud->at(m, n).y;
							Zvec[n][m][ii] = _cloud->at(m, n).z;

						}
					}
				}
				else
				{
					// traverse the depth map
					for (int n = 0; n < _cloud->height; n++)//to be checked//righe
					{
						for (int m = 0; m < _cloud->width; m++)
						{
							//_cloudAverage->at(m, n).x = (1.0 - AverageLenInverse) * _cloudAverage->at(m, n).x + AverageLenInverse * _cloud->at(m, n).x;
							//_cloudAverage->at(m, n).y = (1.0 - AverageLenInverse) * _cloudAverage->at(m, n).y + AverageLenInverse * _cloud->at(m, n).y;
							//_cloudAverage->at(m, n).z = (1.0 - AverageLenInverse) * _cloudAverage->at(m, n).z + AverageLenInverse * _cloud->at(m, n).z;
							//PROBLEM IF UNORGANIZED
							_cloudAverage->at(m, n).b = (1.0 - 1.0 / ((float)ii)) * ((float)_cloudAverage->at(m, n).b) + (1.0 / ((float)ii)) * ((float)_cloud->at(m, n).b);
							_cloudAverage->at(m, n).r = (1.0 - 1.0 / ((float)ii)) * ((float)_cloudAverage->at(m, n).r) + (1.0 / ((float)ii)) * ((float)_cloud->at(m, n).r);
							_cloudAverage->at(m, n).g = (1.0 - 1.0 / ((float)ii)) * ((float)_cloudAverage->at(m, n).g) + (1.0 / ((float)ii)) * ((float)_cloud->at(m, n).g);

							Xvec[n][m][ii] = _cloud->at(m, n).x;
							Yvec[n][m][ii] = _cloud->at(m, n).y;
							Zvec[n][m][ii] = _cloud->at(m, n).z;
						}
					}
				}
			}
			else
				_cloudAverage = _cloud;
		
		}
		if (skip)
			continue;
		if (_cloud->isOrganized() && AverageLen > 1)
			for (int n = 0; n < _cloud->height; n++)//righe
			{
				for (int m = 0; m < _cloud->width; m++)
				{
					_cloudAverage->at(m, n).x = findMedian(Xvec[n][m], AverageLen);
					_cloudAverage->at(m, n).y = findMedian(Yvec[n][m], AverageLen);
					_cloudAverage->at(m, n).z = findMedian(Zvec[n][m], AverageLen);
				}
			}

		auto end = std::chrono::high_resolution_clock::now();
		auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
		printf("\n ------------- GRAB+MEDIAN TIME: %.3f seconds.\n", elapsed.count() * 1e-9);


		cloud_pointer transformed_cloud(new PointCloud<PointXYZRGB>());

		Detection
				(_cloudAverage, transformed_cloud, viewer, i, globalState.showRgbPclView.get());
		++i;

//#ifdef DETAILED_LOG
cout << "Loaded cloud width*height = "
<< _cloud->width * _cloud->height
<< " w,h= " << _cloud->width << " , " << _cloud->height << std::endl;
cout << "cloud is_dense= " << _cloud->is_dense << std::endl;
cout << "cloud isOrganized= " << _cloud->isOrganized() << std::endl;
//#endif

		if (0==i%globalState.SaveOnFile3DGrabsPeriod.get() && globalState.saveOnFile3D.get())// && !globalState.loadFromFile3D.get())
		{
			cout << "saving in ascii format \n";
			pcl::io::savePCDFile<pcl::PointXYZRGB>( "pclRGB_captured_"+to_string(i%100)+".pcd", *_cloud, false);
			pcl::io::savePCDFile<pcl::PointXYZRGB>("pclRGB_captured_average" + to_string(i % 100) + ".pcd", *_cloudAverage, false);
		}
		this_thread::sleep_for(100ms);
	}
}
/*catch (exception e)
{
	cout<<"Exception "<<e.what()<<endl;//to be put in a log
	return(1);
}
*/

return(0);

}

[MartyG-RealSense]

Hi @mynickmynick It's great to hear that you achieved a solution. I will provide the feedback that I had already written.

The impression that I got from the sequence of events is that when the camera was disconnected, the pipeline may not have been closing down like it is supposed to and so when the camera is reconnected, it could not be accessed because the pipeline was still active from the first session. If a camera is already busy when a pipe start request is made then an error such as Failed to set power state can occur.

The Failed to resolve the request error would also occur. This error happens when the camera cannot provide a particular stream configuration. In this case it may be because the sensor had already been 'claimed' by the previous session and so would not become available again until the pipeline is closed.

You can use an instruction called set_devices_changed_callback to detect USB disconnection and reconnection events and define actions to occur when those conditions are detected, such as a camera reset. An example of using this instruction in C++ can be found at #9287

[mynickmynick]

ok thanks I'll have a look