Rs2Device.cpp
Go to the documentation of this file.
1 #include "Rs2Driver.h"
2 #include "Rs2Commands.h"
3 #include <librealsense2/rsutil.h>
4 
5 #define WORKER_THREAD_IDLE_MS 500
6 #define WORKER_THREAD_STOP_TIMEOUT_MS 5000
7 #define WAIT_FRAMESET_TIMEOUT_MS 2000
8 #define WAIT_ALIGNED_DEPTH_TIMEOUT_MS 100
9 
10 namespace oni { namespace driver {
11 
13 :
14  m_driver(driver),
15  m_device(device),
16  m_registrationMode(ONI_IMAGE_REGISTRATION_OFF),
17  m_config(nullptr),
18  m_pipeline(nullptr),
19  m_pipelineProfile(nullptr),
20  m_alignQueue(nullptr),
21  m_alignProcessor(nullptr),
22  m_runFlag(false),
23  m_configId(0),
24  m_framesetId(0)
25 {
26  rsLogDebug("+Rs2Device");
27 }
28 
30 {
31  rsLogDebug("~Rs2Device");
32 
33  shutdown();
34 }
35 
37 {
38  rsTraceFunc("");
39 
40  for (;;)
41  {
43 
44  if (m_thread.get())
45  {
46  rsTraceError("Already initialized");
47  break;
48  }
49 
50  if (queryDeviceInfo(m_device, &m_info) != ONI_STATUS_OK)
51  {
52  rsTraceError("queryDeviceInfo failed");
53  break;
54  }
55 
56  {
57  Rs2ScopedMutex streamLock(m_streamsMx);
58 
59  if (initializeStreams() != ONI_STATUS_OK)
60  {
61  rsTraceError("initializeStreams failed");
62  break;
63  }
64  }
65 
66  m_configId = 0;
67  m_runFlag = true;
68 
69  try {
70  m_thread.reset(new std::thread(&Rs2Device::mainLoop, this));
71  }
72  catch (std::exception& ex) {
73  rsTraceError("std::thread failed: %s", ex.what());
74  break;
75  }
76 
77  return ONI_STATUS_OK;
78  }
79 
80  shutdown();
81  return ONI_STATUS_ERROR;
82 }
83 
85 {
86  if (m_device) { rsTraceFunc(""); }
87 
89 
90  m_runFlag = false;
91  if (m_thread)
92  {
93  m_thread->join();
94  m_thread = nullptr;
95  }
96 
97  {
98  Rs2ScopedMutex streamLock(m_streamsMx);
99 
100  for (auto iter = m_createdStreams.begin(); iter != m_createdStreams.end(); ++iter) { delete(*iter); }
101  m_createdStreams.clear();
102 
103  for (auto iter = m_streams.begin(); iter != m_streams.end(); ++iter) { delete(*iter); }
104  m_streams.clear();
105  }
106 
107  for (auto iter = m_sensorInfo.begin(); iter != m_sensorInfo.end(); ++iter) { delete[](iter->pSupportedVideoModes); }
108  m_sensorInfo.clear();
109  m_profiles.clear();
110 
111  if (m_device)
112  {
114  m_device = nullptr;
115  }
116 }
117 
118 OniStatus Rs2Device::queryDeviceInfo(rs2_device* device, OniDeviceInfo* deviceInfo)
119 {
120  rsTraceFunc("");
121 
122  Rs2Error e;
123  const char* serial = rs2_get_device_info(device, RS2_CAMERA_INFO_SERIAL_NUMBER, &e);
124  if (!e.success()) return ONI_STATUS_ERROR;
125 
126  const char* name = rs2_get_device_info(device, RS2_CAMERA_INFO_NAME, &e);
127  if (!e.success()) return ONI_STATUS_ERROR;
128 
129  strncpy(deviceInfo->uri, serial, sizeof(deviceInfo->uri) - 1);
130 
131  #ifdef RS2_EMULATE_PRIMESENSE_HARDWARE
132  strncpy(deviceInfo->name, "PS1080", sizeof(deviceInfo->name) - 1);
133  strncpy(deviceInfo->vendor, "PrimeSense", sizeof(deviceInfo->vendor) - 1);
134  deviceInfo->usbVendorId = 7463;
135  deviceInfo->usbProductId = 1537;
136  #else
137  strncpy(deviceInfo->name, name, sizeof(deviceInfo->name) - 1);
138  strncpy(deviceInfo->vendor, "", sizeof(deviceInfo->vendor) - 1);
139  deviceInfo->usbVendorId = 0;
140  deviceInfo->usbProductId = 0;
141  #endif
142 
143  rsLogDebug("DEVICE serial=%s name=%s", serial, name);
144 
145  return ONI_STATUS_OK;
146 }
147 
148 //=============================================================================
149 // DeviceBase overrides
150 //=============================================================================
151 
152 OniStatus Rs2Device::getSensorInfoList(OniSensorInfo** sensors, int* numSensors)
153 {
154  rsTraceFunc("");
155 
157 
158  *numSensors = (int)m_sensorInfo.size();
159  *sensors = ((*numSensors > 0) ? &m_sensorInfo[0] : nullptr);
160 
161  return ONI_STATUS_OK;
162 }
163 
164 StreamBase* Rs2Device::createStream(OniSensorType sensorType)
165 {
166  rsTraceFunc("sensorType=%d", (int)sensorType);
167 
169 
170  for (auto iter = m_streams.begin(); iter != m_streams.end(); ++iter)
171  {
172  Rs2Stream* streamObj = *iter;
173  if (streamObj->getOniType() == sensorType)
174  {
175  m_createdStreams.push_back(streamObj);
176  m_streams.remove(streamObj);
177  return streamObj;
178  }
179  }
180 
181  return nullptr;
182 }
183 
184 void Rs2Device::destroyStream(StreamBase* streamBase)
185 {
186  rsTraceFunc("ptr=%p", streamBase);
187 
188  if (streamBase)
189  {
191 
192  Rs2Stream* streamObj = (Rs2Stream*)streamBase;
193  streamObj->stop();
194 
195  m_streams.push_back(streamObj);
196  m_createdStreams.remove(streamObj);
197  }
198 }
199 
200 OniStatus Rs2Device::invoke(int commandId, void* data, int dataSize)
201 {
202  if (commandId == RS2_PROJECT_POINT_TO_PIXEL && data && dataSize == sizeof(Rs2PointPixel))
203  {
204  for (auto iter = m_createdStreams.begin(); iter != m_createdStreams.end(); ++iter)
205  {
206  Rs2Stream* stream = *iter;
207  if (stream->getOniType() == ONI_SENSOR_DEPTH)
208  {
209  auto pp = (Rs2PointPixel*)data;
210  rs2_project_point_to_pixel(pp->pixel, &stream->m_intrinsics, pp->point);
211  return ONI_STATUS_OK;
212  }
213  }
214  return ONI_STATUS_NO_DEVICE;
215  }
216 
217  #if defined(RS2_TRACE_NOT_SUPPORTED_CMDS)
218  rsTraceError("Not supported: commandId=%d", commandId);
219  #endif
220  return ONI_STATUS_NOT_SUPPORTED;
221 }
222 
223 OniBool Rs2Device::isCommandSupported(int commandId)
224 {
225  switch (commandId)
226  {
227  case RS2_PROJECT_POINT_TO_PIXEL: return true;
228  }
229 
230  return false;
231 }
232 
234 {
235  return ONI_STATUS_OK;
236 }
237 
238 OniBool Rs2Device::isImageRegistrationModeSupported(OniImageRegistrationMode mode)
239 {
240  return true;
241 }
242 
243 //=============================================================================
244 // Start/Stop
245 //=============================================================================
246 
248 {
249  rsTraceFunc("");
250 
251  for (;;)
252  {
253  if (m_pipeline)
254  {
255  rsTraceError("Already started");
256  break;
257  }
258 
259  Rs2Error e;
260 
261  rsLogDebug("rs2_create_config");
263  if (!e.success())
264  {
265  rsTraceError("rs2_create_config failed: %s", e.get_message());
266  break;
267  }
268 
269  rsLogDebug("rs2_config_enable_device %s", m_info.uri);
271  if (!e.success())
272  {
273  rsTraceError("rs2_config_enable_device failed: %s", e.get_message());
274  break;
275  }
276 
278  bool enableStreamError = false;
279 
280  {
282 
283  for (auto iter = m_createdStreams.begin(); iter != m_createdStreams.end(); ++iter)
284  {
285  Rs2Stream* stream = *iter;
286  if (stream->isEnabled())
287  {
288  const OniVideoMode& mode = stream->getVideoMode();
289 
290  rsLogDebug("ENABLE STREAM type=%d sensorId=%d streamId=%d %dx%d @%d",
291  (int)stream->getRsType(), stream->getSensorId(), stream->getStreamId(), mode.resolutionX, mode.resolutionY, mode.fps);
292 
294  m_config, stream->getRsType(), stream->getStreamId(),
295  mode.resolutionX, mode.resolutionY, convertPixelFormat(mode.pixelFormat), mode.fps, &e
296  );
297  if (!e.success())
298  {
299  rsTraceError("rs2_config_enable_stream failed: %s", e.get_message());
300  enableStreamError = true;
301  break;
302  }
303 
304  stream->onStreamStarted();
305  }
306  }
307  }
308 
309  if (enableStreamError)
310  {
311  rsTraceError("enable_stream error");
312  break;
313  }
314 
315  // pipeline
316 
317  rs2_context* context = getDriver()->getRsContext();
318  rsLogDebug("rs2_create_pipeline");
319  m_pipeline = rs2_create_pipeline(context, &e);
320  if (!e.success())
321  {
322  rsTraceError("rs2_create_pipeline failed: %s", e.get_message());
323  break;
324  }
325 
326  rsLogDebug("rs2_pipeline_start_with_config");
328  if (!e.success())
329  {
330  rsTraceError("rs2_pipeline_start_with_config failed: %s", e.get_message());
331  break;
332  }
333 
334  {
336 
337  for (auto iter = m_createdStreams.begin(); iter != m_createdStreams.end(); ++iter)
338  {
339  Rs2Stream* stream = *iter;
340  if (stream->isEnabled())
341  {
342  stream->onPipelineStarted();
343  }
344  }
345  }
346 
347  // depth to color aligner
348 
350  if (!e.success())
351  {
352  rsTraceError("rs2_create_frame_queue failed: %s", e.get_message());
353  break;
354  }
355 
357  if (!e.success())
358  {
359  rsTraceError("rs2_create_align failed: %s", e.get_message());
360  break;
361  }
362 
364  if (!e.success())
365  {
366  rsTraceError("rs2_start_processing_queue failed: %s", e.get_message());
367  break;
368  }
369 
370  rsLogDebug("STARTED");
371  return ONI_STATUS_OK;
372  }
373 
374  stopPipeline();
375  return ONI_STATUS_ERROR;
376 }
377 
379 {
380  if (m_pipeline) { rsTraceFunc(""); }
381 
382  if (m_pipeline)
383  {
384  Rs2Error e;
385  rsLogDebug("rs2_pipeline_stop");
387  }
388  if (m_alignProcessor)
389  {
391  m_alignProcessor = nullptr;
392  }
393  if (m_alignQueue)
394  {
396  m_alignQueue = nullptr;
397  }
398  if (m_pipelineProfile)
399  {
400  rsLogDebug("rs2_delete_pipeline_profile");
402  m_pipelineProfile = nullptr;
403  }
404  if (m_config)
405  {
406  rsLogDebug("rs2_delete_config");
408  m_config = nullptr;
409  }
410  if (m_pipeline)
411  {
412  rsLogDebug("rs2_delete_pipeline");
414  m_pipeline = nullptr;
415  rsLogDebug("STOPPED");
416  }
417 }
418 
420 {
421  rsTraceFunc("");
422 
423  stopPipeline();
424 
425  bool hasStreams;
426  {
428  hasStreams = hasEnabledStreams();
429  }
430 
431  if (hasStreams && m_runFlag)
432  {
433  startPipeline();
434  }
435 }
436 
437 //=============================================================================
438 // MainLoop
439 //=============================================================================
440 
442 {
443  rsTraceFunc("");
444 
445  try
446  {
447  int configId = 0;
448  while (m_runFlag)
449  {
450  const int curConfigId = m_configId;
451  if (configId != curConfigId) // configuration changed since last tick
452  {
453  configId = curConfigId;
454  restartPipeline();
455  }
456 
457  if (m_pipelineProfile)
458  {
459  waitForFrames();
460  }
461  else
462  {
463  std::this_thread::sleep_for(std::chrono::milliseconds(WORKER_THREAD_IDLE_MS));
464  }
465  }
466  }
467  catch (...)
468  {
469  rsTraceError("Unhandled exception");
470  }
471 
472  stopPipeline();
473 }
474 
476 {
477  rsTraceFunc("");
478 
479  m_configId++;
480 }
481 
483 {
485 
486  Rs2Error e;
487  rs2_frame* frameset;
488  {
489  NAMED_PROFILER("rs2_pipeline_wait_for_frames");
491  }
492  if (!e.success())
493  {
494  rsTraceError("rs2_pipeline_wait_for_frames failed: %s", e.get_message());
495  return;
496  }
497 
498  const int nframes = rs2_embedded_frames_count(frameset, &e);
499  //rsLogDebug("frameset %llu (%d)", m_framesetId, nframes);
500 
501  if (m_registrationMode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR)
502  {
503  rs2_frame_add_ref(frameset, &e);
504  {
505  NAMED_PROFILER("rs2_process_frame");
506  rs2_process_frame(m_alignProcessor, frameset, &e);
507  if (!e.success())
508  {
509  rsTraceError("rs2_process_frame failed: %s", e.get_message());
510  }
511  }
512  }
513 
514  for (int i = 0; i < nframes; ++i)
515  {
516  rs2_frame* frame = rs2_extract_frame(frameset, i, &e);
517  if (frame)
518  {
519  processFrame(frame);
520  releaseFrame(frame);
521  }
522  }
523 
524  releaseFrame(frameset);
525  ++m_framesetId;
526 
527  if (m_registrationMode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR)
528  {
530  }
531 }
532 
534 {
536 
538  Rs2Stream* stream = getFrameStream(frame, &spi);
539  if (stream)
540  {
541  if (m_registrationMode == ONI_IMAGE_REGISTRATION_OFF || spi.streamType != RS2_STREAM_DEPTH)
542  {
543  OniFrame* oniFrame = createOniFrame(frame, stream, &spi);
544  if (oniFrame)
545  {
546  submitOniFrame(oniFrame, stream);
547  }
548  }
549  }
550 }
551 
553 {
555 
556  Rs2Error e;
557  rs2_frame* frameset;
558  {
559  NAMED_PROFILER("rs2_wait_for_frame");
561  }
562  if (!e.success())
563  {
564  rsTraceError("rs2_wait_for_frame failed: %s", e.get_message());
565  return;
566  }
567 
568  const int nframes = rs2_embedded_frames_count(frameset, &e);
569  for (int i = 0; i < nframes; ++i)
570  {
571  rs2_frame* frame = rs2_extract_frame(frameset, i, &e);
572  if (frame)
573  {
575  Rs2Stream* stream = getFrameStream(frame, &spi);
576  if (stream && spi.streamType == RS2_STREAM_DEPTH)
577  {
578  OniFrame* oniFrame = createOniFrame(frame, stream, &spi);
579  if (oniFrame)
580  {
581  submitOniFrame(oniFrame, stream);
582  }
583  }
584  releaseFrame(frame);
585  }
586  }
587 
588  releaseFrame(frameset);
589 }
590 
592 {
594 
595  if (!frame || !stream || !stream->isEnabled() || !spi)
596  {
597  return nullptr;
598  }
599 
600  Rs2Error e;
601  OniFrame* oniFrame;
602  {
603  NAMED_PROFILER("StreamServices::acquireFrame");
604  oniFrame = stream->getServices().acquireFrame();
605  if (!oniFrame)
606  {
607  rsTraceError("acquireFrame failed");
608  return nullptr;
609  }
610  }
611 
612  oniFrame->sensorType = stream->getOniType();
613  oniFrame->timestamp = (uint64_t)(rs2_get_frame_timestamp(frame, &e) * 1000.0); // millisecond to microsecond
614  oniFrame->frameIndex = (int)rs2_get_frame_number(frame, &e);
615 
616  oniFrame->width = rs2_get_frame_width(frame, &e);
617  oniFrame->height = rs2_get_frame_height(frame, &e);
618  oniFrame->stride = rs2_get_frame_stride_in_bytes(frame, &e);
619  const size_t frameSize = oniFrame->stride * oniFrame->height;
620 
621  OniVideoMode mode;
622  mode.pixelFormat = convertPixelFormat(spi->format);
623  mode.resolutionX = oniFrame->width;
624  mode.resolutionY = oniFrame->height;
625  mode.fps = spi->framerate;
626 
627  oniFrame->videoMode = mode;
628  oniFrame->croppingEnabled = false;
629  oniFrame->cropOriginX = 0;
630  oniFrame->cropOriginY = 0;
631 
632  if (frameSize != oniFrame->dataSize)
633  {
634  rsTraceError("invalid frame: rsSize=%u oniSize=%u", (unsigned int)frameSize, (unsigned int)oniFrame->dataSize);
635  stream->getServices().releaseFrame(oniFrame);
636  return nullptr;
637  }
638 
639  const void* frameData = rs2_get_frame_data(frame, &e);
640  if (!e.success())
641  {
642  rsTraceError("rs2_get_frame_data failed: %s", e.get_message());
643  stream->getServices().releaseFrame(oniFrame);
644  return nullptr;
645  }
646 
647  {
648  NAMED_PROFILER("_copyFrameData");
649  memcpy(oniFrame->data, frameData, frameSize);
650  }
651 
652  return oniFrame;
653 }
654 
655 void Rs2Device::submitOniFrame(OniFrame* oniFrame, Rs2Stream* stream)
656 {
658 
659  if (stream->getOniType() == ONI_SENSOR_DEPTH) // HACK: clamp depth to OpenNI hardcoded max value
660  {
661  NAMED_PROFILER("_clampDepth");
662  uint16_t* depth = (uint16_t*)oniFrame->data;
663  for (int i = 0; i < oniFrame->width * oniFrame->height; ++i)
664  {
665  if (*depth >= ONI_MAX_DEPTH) { *depth = ONI_MAX_DEPTH - 1; }
666  ++depth;
667  }
668  }
669  {
670  NAMED_PROFILER("StreamServices::raiseNewFrame");
671  stream->raiseNewFrame(oniFrame);
672  }
673  {
674  NAMED_PROFILER("StreamServices::releaseFrame");
675  stream->getServices().releaseFrame(oniFrame);
676  }
677 }
678 
680 {
682 
683  Rs2Error e;
685  if (e.success())
686  {
687  memset(spi, 0, sizeof(Rs2StreamProfileInfo));
688  rs2_get_stream_profile_data(profile, &spi->streamType, &spi->format, &spi->streamId, &spi->profileId, &spi->framerate, &e);
689  if (e.success())
690  {
691  OniSensorType sensorType = convertStreamType(spi->streamType);
692  {
694  return findStream(sensorType, spi->streamId);
695  }
696  }
697  }
698  return nullptr;
699 }
700 
702 {
703  NAMED_PROFILER("rs2_release_frame");
704  rs2_release_frame(frame);
705 }
706 
707 //=============================================================================
708 // Stream initialization
709 //=============================================================================
710 
712 {
713  rsTraceFunc("");
714 
715  std::map<int, rs2_stream> sensorStreams;
716 
717  Rs2Error e;
718  rs2_sensor_list* sensorList = rs2_query_sensors(m_device, &e);
719  if (sensorList)
720  {
721  const int nsensors = rs2_get_sensors_count(sensorList, &e);
722  for (int sensorId = 0; sensorId < nsensors; sensorId++)
723  {
724  rsLogDebug("SENSOR %d", sensorId);
725 
726  rs2_sensor* sensor = rs2_create_sensor(sensorList, sensorId, &e);
727  if (sensor)
728  {
729  sensorStreams.clear();
730 
731  rs2_stream_profile_list* profileList = rs2_get_stream_profiles(sensor, &e);
732  if (profileList)
733  {
734  const int nprofiles = rs2_get_stream_profiles_count(profileList, &e);
735  for (int profileId = 0; profileId < nprofiles; profileId++)
736  {
737  const rs2_stream_profile* profile = rs2_get_stream_profile(profileList, profileId, &e);
738  if (profile)
739  {
741  spi.profile = profile;
742  spi.sensorId = sensorId;
743  rs2_get_stream_profile_data(profile, &spi.streamType, &spi.format, &spi.streamId, &spi.profileId, &spi.framerate, &e);
744 
746  {
747  rs2_get_video_stream_resolution(profile, &spi.width, &spi.height, &e);
748  if (e.success())
749  {
750  #if 1
751  rsLogDebug("\ttype=%d sensorId=%d streamId=%d profileId=%d format=%d width=%d height=%d framerate=%d",
752  (int)spi.streamType, (int)spi.sensorId, (int)spi.streamId, (int)spi.profileId, (int)spi.format, (int)spi.width, (int)spi.height, (int)spi.framerate);
753  #endif
754 
755  m_profiles.push_back(spi);
756  sensorStreams[spi.streamId] = spi.streamType;
757  }
758  }
759  }
760  }
761  rs2_delete_stream_profiles_list(profileList);
762  }
763 
764  for (auto iter = sensorStreams.begin(); iter != sensorStreams.end(); ++iter)
765  {
766  rsLogDebug("UNIQ streamId (%d) -> type (%d)", iter->first, (int)iter->second);
767  }
768 
769  for (auto iter = sensorStreams.begin(); iter != sensorStreams.end(); ++iter)
770  {
771  const OniSensorType oniType = convertStreamType(iter->second);
772 
773  std::vector<Rs2StreamProfileInfo> profiles;
774  findStreamProfiles(&profiles, oniType, iter->first);
775 
776  if (addStream(sensor, oniType, sensorId, iter->first, &profiles) == ONI_STATUS_OK)
777  {
778  sensor = nullptr;
779  }
780  }
781 
782  if (sensor) { rs2_delete_sensor(sensor); }
783  }
784  }
785  rs2_delete_sensor_list(sensorList);
786  }
787 
788  rsLogDebug("FILL OniSensorInfo");
789  for (auto iter = m_streams.begin(); iter != m_streams.end(); ++iter)
790  {
791  Rs2Stream* stream = *iter;
792  #if 1
793  rsLogDebug("STREAM type=%d sensorId=%d streamId=%d", (int)stream->getRsType(), stream->getSensorId(), stream->getStreamId());
794  #endif
795 
796  std::vector<Rs2StreamProfileInfo> profiles;
797  findStreamProfiles(&profiles, stream->getOniType(), stream->getStreamId());
798 
799  OniSensorInfo info;
800  info.sensorType = stream->getOniType();
801  info.numSupportedVideoModes = (int)profiles.size();
802  info.pSupportedVideoModes = nullptr;
803 
804  if (info.numSupportedVideoModes > 0)
805  {
806  info.pSupportedVideoModes = new OniVideoMode[info.numSupportedVideoModes];
807  int modeId = 0;
808 
809  for (auto p = profiles.begin(); p != profiles.end(); ++p)
810  {
811  OniVideoMode& mode = info.pSupportedVideoModes[modeId];
812  mode.pixelFormat = convertPixelFormat(p->format);
813  mode.resolutionX = p->width;
814  mode.resolutionY = p->height;
815  mode.fps = p->framerate;
816  modeId++;
817 
818  #if 1
819  rsLogDebug("\ttype=%d sensorId=%d streamId=%d profileId=%d format=%d width=%d height=%d framerate=%d",
820  (int)p->streamType, (int)p->sensorId, (int)p->streamId, (int)p->profileId, (int)p->format, (int)p->width, (int)p->height, (int)p->framerate);
821  #endif
822  }
823 
824  m_sensorInfo.push_back(info);
825  }
826  }
827 
828  return ONI_STATUS_OK;
829 }
830 
831 OniStatus Rs2Device::addStream(rs2_sensor* sensor, OniSensorType sensorType, int sensorId, int streamId, std::vector<Rs2StreamProfileInfo>* profiles)
832 {
833  rsTraceFunc("type=%d sensorId=%d streamId=%d", (int)convertStreamType(sensorType), sensorId, streamId);
834 
835  Rs2Stream* streamObj = nullptr;
836  switch (sensorType)
837  {
838  case ONI_SENSOR_IR: streamObj = new Rs2InfraredStream(); break;
839  case ONI_SENSOR_COLOR: streamObj = new Rs2ColorStream(); break;
840  case ONI_SENSOR_DEPTH: streamObj = new Rs2DepthStream(); break;
841 
842  default:
843  {
844  rsTraceError("Invalid type=%d", (int)sensorType);
845  return ONI_STATUS_ERROR;
846  }
847  }
848 
849  if (streamObj->initialize(this, sensor, sensorId, streamId, profiles) != ONI_STATUS_OK)
850  {
851  rsTraceError("Rs2Stream::initialize failed");
852  delete(streamObj);
853  return ONI_STATUS_ERROR;
854  }
855 
856  m_streams.push_back(streamObj);
857  return ONI_STATUS_OK;
858 }
859 
860 void Rs2Device::findStreamProfiles(std::vector<Rs2StreamProfileInfo>* dst, OniSensorType sensorType, int streamId)
861 {
862  const rs2_stream rsType = convertStreamType(sensorType);
863 
864  for (auto iter = m_profiles.begin(); iter != m_profiles.end(); ++iter)
865  {
866  Rs2StreamProfileInfo& p = *iter;
867  if (p.streamType == rsType && p.streamId == streamId)
868  {
869  dst->push_back(p);
870  }
871  }
872 }
873 
874 Rs2Stream* Rs2Device::findStream(OniSensorType sensorType, int streamId)
875 {
876  for (auto iter = m_createdStreams.begin(); iter != m_createdStreams.end(); ++iter)
877  {
878  Rs2Stream* stream = *iter;
879  if (stream->getOniType() == sensorType && stream->getStreamId() == streamId)
880  {
881  return stream;
882  }
883  }
884 
885  return nullptr;
886 }
887 
889 {
890  for (auto iter = m_createdStreams.begin(); iter != m_createdStreams.end(); ++iter)
891  {
892  Rs2Stream* stream = *iter;
893  if (stream->isEnabled())
894  {
895  return true;
896  }
897  }
898 
899  return false;
900 }
901 
902 }} // namespace
static const textual_icon lock
Definition: model-views.h:218
int rs2_get_sensors_count(const rs2_sensor_list *info_list, rs2_error **error)
Definition: rs.cpp:267
void rs2_config_enable_stream(rs2_config *config, rs2_stream stream, int index, int width, int height, rs2_format format, int framerate, rs2_error **error)
Definition: rs.cpp:1928
#define RS2_PROJECT_POINT_TO_PIXEL
Definition: Rs2Commands.h:3
const rs2_stream_profile * rs2_get_frame_stream_profile(const rs2_frame *frame, rs2_error **error)
Definition: rs.cpp:971
GLuint const GLchar * name
rs2_processing_block * m_alignProcessor
Definition: Rs2Device.h:82
rs2_context * getRsContext()
Definition: Rs2Driver.h:32
rs2_frame_queue * m_alignQueue
Definition: Rs2Device.h:81
void findStreamProfiles(std::vector< Rs2StreamProfileInfo > *dst, OniSensorType sensorType, int streamId)
Definition: Rs2Device.cpp:860
virtual OniBool isCommandSupported(int commandId)
Definition: Rs2Device.cpp:223
virtual OniStatus invoke(int commandId, void *data, int dataSize)
Definition: Rs2Device.cpp:200
static OniStatus queryDeviceInfo(rs2_device *device, OniDeviceInfo *deviceInfo)
Definition: Rs2Device.cpp:118
volatile int m_runFlag
Definition: Rs2Device.h:85
Rs2Device(class Rs2Driver *driver, rs2_device *device)
Definition: Rs2Device.cpp:12
GLfloat GLfloat p
Definition: glext.h:12687
void rs2_get_video_stream_resolution(const rs2_stream_profile *mode, int *width, int *height, rs2_error **error)
Definition: rs.cpp:466
rs2_sensor * rs2_create_sensor(const rs2_sensor_list *list, int index, rs2_error **error)
Definition: rs.cpp:308
OniStatus startPipeline()
Definition: Rs2Device.cpp:247
OniSensorType convertStreamType(rs2_stream type)
Definition: Rs2Base.cpp:19
OniStatus initialize()
Definition: Rs2Device.cpp:36
std::list< class Rs2Stream * > m_createdStreams
Definition: Rs2Device.h:93
GLenum GLsizei dataSize
Definition: glext.h:5691
void rs2_delete_device(rs2_device *device)
Definition: rs.cpp:301
rs2_config * rs2_create_config(rs2_error **error)
Definition: rs.cpp:1914
rs2_pipeline * rs2_create_pipeline(rs2_context *ctx, rs2_error **error)
Definition: rs.cpp:1756
GLint GLint GLsizei GLsizei GLsizei depth
GLenum GLenum dst
Definition: glext.h:1751
void rs2_delete_frame_queue(rs2_frame_queue *queue)
Definition: rs.cpp:1036
bool isSupportedPixelFormat(rs2_format type)
Definition: Rs2Base.cpp:47
rs2_time_t rs2_get_frame_timestamp(const rs2_frame *frame, rs2_error **error)
Definition: rs.cpp:908
unsigned short uint16_t
Definition: stdint.h:79
OniFrame * createOniFrame(rs2_frame *frame, Rs2Stream *stream, Rs2StreamProfileInfo *spi)
Definition: Rs2Device.cpp:591
rs2_frame_queue * rs2_create_frame_queue(int capacity, rs2_error **error)
Definition: rs.cpp:1030
rs2_frame * rs2_wait_for_frame(rs2_frame_queue *queue, unsigned int timeout_ms, rs2_error **error)
Definition: rs.cpp:1043
void rs2_process_frame(rs2_processing_block *block, rs2_frame *frame, rs2_error **error)
Definition: rs.cpp:2097
GLuint GLuint stream
Definition: glext.h:1790
int getStreamId() const
Definition: Rs2Stream.h:46
e
Definition: rmse.py:177
bool isSupportedStreamType(rs2_stream type)
Definition: Rs2Base.cpp:7
#define rsTraceError(format,...)
Definition: Rs2Base.h:41
rs2_stream_profile_list * rs2_get_stream_profiles(rs2_sensor *sensor, rs2_error **error)
Definition: rs.cpp:327
rs2_pipeline * m_pipeline
Definition: Rs2Device.h:78
int rs2_get_frame_height(const rs2_frame *frame, rs2_error **error)
Definition: rs.cpp:955
Rs2Stream * getFrameStream(rs2_frame *frame, Rs2StreamProfileInfo *spi)
Definition: Rs2Device.cpp:679
OniDeviceInfo m_info
Definition: Rs2Device.h:89
Definition: Rs2Base.cpp:3
def info(name, value, persistent=False)
Definition: test.py:301
virtual void destroyStream(StreamBase *streamBase)
Definition: Rs2Device.cpp:184
int rs2_get_frame_stride_in_bytes(const rs2_frame *frame, rs2_error **error)
Definition: rs.cpp:963
rs2_processing_block * rs2_create_align(rs2_stream align_to, rs2_error **error)
Definition: rs.cpp:2211
GLenum mode
#define WAIT_FRAMESET_TIMEOUT_MS
Definition: Rs2Device.cpp:7
rs2_frame * rs2_pipeline_wait_for_frames(rs2_pipeline *pipe, unsigned int timeout_ms, rs2_error **error)
Definition: rs.cpp:1774
void rs2_get_stream_profile_data(const rs2_stream_profile *mode, rs2_stream *stream, rs2_format *format, int *index, int *unique_id, int *framerate, rs2_error **error)
Definition: rs.cpp:484
#define ONI_MAX_DEPTH
Definition: Rs2Base.h:17
std::vector< Rs2StreamProfileInfo > m_profiles
Definition: Rs2Device.h:90
void rs2_config_disable_all_streams(rs2_config *config, rs2_error **error)
Definition: rs.cpp:2000
int rs2_embedded_frames_count(rs2_frame *composite, rs2_error **error)
Definition: rs.cpp:2144
void submitOniFrame(OniFrame *oniFrame, Rs2Stream *stream)
Definition: Rs2Device.cpp:655
int rs2_get_frame_width(const rs2_frame *frame, rs2_error **error)
Definition: rs.cpp:947
void processFrame(rs2_frame *frame)
Definition: Rs2Device.cpp:533
virtual OniStatus getSensorInfoList(OniSensorInfo **sensors, int *numSensors)
Definition: Rs2Device.cpp:152
void rs2_delete_sensor(rs2_sensor *sensor)
Definition: rs.cpp:320
volatile int m_configId
Definition: Rs2Device.h:86
#define SCOPED_PROFILER
Definition: Rs2Base.h:32
const rs2_stream_profile * rs2_get_stream_profile(const rs2_stream_profile_list *list, int index, rs2_error **error)
Definition: rs.cpp:351
Definition: api.h:28
#define WORKER_THREAD_IDLE_MS
Definition: Rs2Device.cpp:5
unsigned __int64 uint64_t
Definition: stdint.h:90
#define NAMED_PROFILER(name)
Definition: Rs2Base.h:31
rs2_sensor_list * rs2_query_sensors(const rs2_device *device, rs2_error **error)
Definition: rs.cpp:236
void rs2_delete_processing_block(rs2_processing_block *block)
Definition: rs.cpp:2106
void releaseFrame(rs2_frame *frame)
Definition: Rs2Device.cpp:701
const char * get_message() const
Definition: Rs2Base.h:59
virtual StreamBase * createStream(OniSensorType sensorType)
Definition: Rs2Device.cpp:164
unsigned long long rs2_get_frame_number(const rs2_frame *frame, rs2_error **error)
Definition: rs.cpp:986
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:42
rs2_pipeline_profile * rs2_pipeline_start_with_config(rs2_pipeline *pipe, rs2_config *config, rs2_error **error)
Definition: rs.cpp:1834
#define rsLogDebug(format,...)
Definition: Rs2Base.h:43
std::vector< OniSensorInfo > m_sensorInfo
Definition: Rs2Device.h:91
#define rsTraceFunc(format,...)
Definition: Rs2Base.h:42
static void rs2_project_point_to_pixel(float pixel[2], const struct rs2_intrinsics *intrin, const float point[3])
Definition: rsutil.h:19
std::lock_guard< std::mutex > Rs2ScopedMutex
Definition: Rs2Base.h:48
void rs2_delete_stream_profiles_list(rs2_stream_profile_list *list)
Definition: rs.cpp:367
void rs2_pipeline_stop(rs2_pipeline *pipe, rs2_error **error)
Definition: rs.cpp:1766
void rs2_frame_add_ref(rs2_frame *frame, rs2_error **error)
Definition: rs.cpp:1015
class Rs2Driver * getDriver()
Definition: Rs2Device.h:30
bool success() const
Definition: Rs2Base.h:58
OniImageRegistrationMode m_registrationMode
Definition: Rs2Device.h:75
void rs2_delete_config(rs2_config *config)
Definition: rs.cpp:1920
rs2_intrinsics m_intrinsics
Definition: Rs2Stream.h:81
bool isEnabled() const
Definition: Rs2Stream.h:48
OniSensorType getOniType() const
Definition: Rs2Stream.h:44
void rs2_delete_sensor_list(rs2_sensor_list *info_list)
Definition: rs.cpp:282
int i
void rs2_config_enable_device(rs2_config *config, const char *serial, rs2_error **error)
Definition: rs.cpp:1949
OniStatus initialize(class Rs2Device *device, rs2_sensor *sensor, int sensorId, int streamId, std::vector< Rs2StreamProfileInfo > *profiles)
Definition: Rs2Stream.cpp:31
const rs2_stream_profile * profile
Definition: Rs2Stream.h:9
OniStatus addStream(rs2_sensor *sensor, OniSensorType sensorType, int sensorId, int streamId, std::vector< Rs2StreamProfileInfo > *profiles)
Definition: Rs2Device.cpp:831
std::list< class Rs2Stream * > m_streams
Definition: Rs2Device.h:92
void rs2_release_frame(rs2_frame *frame)
Definition: rs.cpp:993
int getSensorId() const
Definition: Rs2Stream.h:45
rs2_frame * rs2_extract_frame(rs2_frame *composite, int index, rs2_error **error)
Definition: rs.cpp:2114
int rs2_get_stream_profiles_count(const rs2_stream_profile_list *list, rs2_error **error)
Definition: rs.cpp:360
rs2_device * m_device
Definition: Rs2Device.h:74
OniPixelFormat convertPixelFormat(rs2_format type)
Definition: Rs2Base.cpp:76
virtual OniStatus tryManualTrigger()
Definition: Rs2Device.cpp:233
void rs2_start_processing_queue(rs2_processing_block *block, rs2_frame_queue *queue, rs2_error **error)
Definition: rs.cpp:2087
void rs2_delete_pipeline_profile(rs2_pipeline_profile *profile)
Definition: rs.cpp:1905
virtual void stop()
Definition: Rs2Stream.cpp:99
auto device
Definition: pyrs_net.cpp:17
Definition: parser.hpp:150
OniStatus initializeStreams()
Definition: Rs2Device.cpp:711
void rs2_delete_pipeline(rs2_pipeline *pipe)
Definition: rs.cpp:1819
rs2_pipeline_profile * m_pipelineProfile
Definition: Rs2Device.h:79
struct rs2_frame rs2_frame
Definition: rs_types.h:261
const void * rs2_get_frame_data(const rs2_frame *frame, rs2_error **error)
Definition: rs.cpp:940
#define WAIT_ALIGNED_DEPTH_TIMEOUT_MS
Definition: Rs2Device.cpp:8
rs2_stream getRsType() const
Definition: Rs2Stream.h:43
const OniVideoMode & getVideoMode() const
Definition: Rs2Stream.h:47
std::unique_ptr< std::thread > m_thread
Definition: Rs2Device.h:84
Rs2Stream * findStream(OniSensorType sensorType, int streamId)
Definition: Rs2Device.cpp:874
virtual OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode)
Definition: Rs2Device.cpp:238
const char * rs2_get_device_info(const rs2_device *device, rs2_camera_info info, rs2_error **error)
Definition: rs.cpp:703
rs2_config * m_config
Definition: Rs2Device.h:77


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:47:40