unit-tests-internal.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
3 
5 // The specific tests are configured to run only when the library is staticly-linked to test implementation
6 // of librealsense core classes
8 #include <cmath>
9 #include <iostream>
10 #include <chrono>
11 #include <ctime>
12 #include <algorithm>
13 
14 #include "unit-tests-common.h"
15 #include "../include/librealsense2/rs_advanced_mode.hpp"
16 #include "../include/librealsense2/hpp/rs_frame.hpp"
17 #include "../include/librealsense2/hpp/rs_processing.hpp"
19 #include <../src/proc/synthetic-stream.h>
20 #include <../src/proc/disparity-transform.h>
21 #include <../src/proc/spatial-filter.h>
22 #include <../src/proc/temporal-filter.h>
23 
24 using namespace rs2;
25 using namespace librealsense; // An internal namespace not acessible via the public API
26 
27 # define SECTION_FROM_TEST_NAME space_to_underscore(Catch::getCurrentContext().getResultCapture()->getCurrentTestName()).c_str()
28 
29 long long current_time()
30 {
31  return (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count() % 10000);
32 }
33 
37 {
40 
43 
45  {
47  REQUIRE_NOTHROW(range = sen.get_option_range(RS2_OPTION_EXPOSURE)); // TODO: fails sometimes with "Element Not Found!"
49  }
50 }
51 
53 {
54  for (auto&& s : dev.query_sensors())
56 }
57 
58 bool wait_for_reset(std::function<bool(void)> func, std::shared_ptr<device> dev)
59 {
60  if (func())
61  return true;
62 
63  WARN("Reset workaround");
64 
65  try {
66  dev->hardware_reset();
67  }
68  catch (...)
69  {
70  }
71  return func();
72 }
73 
74 bool is_usb3(const rs2::device& dev)
75 {
76  bool usb3_device = true;
77  try
78  {
80  // Device types "3.x" and also "" (for legacy playback records) will be recognized as USB3
81  usb3_device = (std::string::npos == usb_type.find("2."));
82  }
83  catch (...) // In case the feature not provided assume USB3 device
84  {
85  }
86 
87  return usb3_device;
88 }
89 
90 // Provides for Device PID , USB3/2 (true/false)
91 typedef std::pair<std::string, bool > dev_type;
92 
94 {
95  dev_type designator{ "",true };
97  bool usb_descriptor = false;
98 
99  REQUIRE_NOTHROW(designator.first = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID));
101  if (usb_descriptor)
102  {
104  designator.second = (std::string::npos == usb_type.find("2."));
105  }
106 
107  return designator;
108 }
109 
110 struct stream_request
111 {
114  int width;
115  int height;
116  int fps;
117  int index;
118 
119  bool operator==(const video_stream_profile& other) const
120  {
121  return stream == other.stream_type() &&
122  format == other.format() &&
123  width == other.width() &&
124  height == other.height() &&
125  index == other.stream_index();
126  }
127 };
128 
130 {
133  int width;
134  int height;
135  int index;
136 
137  bool operator==(const test_profile& other) const
138  {
139  return stream == other.stream &&
140  (format == 0 || other.format == 0 || format == other.format) &&
141  (width == 0 || other.width == 0 || width == other.width) &&
142  (height == 0 || other.height == 0 || height == other.height) &&
143  (index == 0 || other.index == 0 || index == other.index);
144 
145  }
146  bool operator!=(const test_profile& other) const
147  {
148  return !(*this == other);
149  }
150  bool operator<(const test_profile& other) const
151  {
152  return stream < other.stream;
153  }
154 
155 };
156 struct device_profiles
157 {
158  std::vector<test_profile> streams;
159  int fps;
160  bool sync;
161 };
162 
163 std::vector<test_profile> configure_all_supported_streams(rs2::sensor& sensor, int width = 640, int height = 480, int fps = 60)
164 {
165  std::vector<test_profile> all_profiles =
166  {
172  // {RS2_STREAM_GYRO, 0, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 0},
173  // {RS2_STREAM_ACCEL, 0, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 0}
174  };
175 
176  std::vector<test_profile> profiles;
177  std::vector<rs2::stream_profile> modes;
178  auto all_modes = sensor.get_stream_profiles();
179 
180  for (auto profile : all_profiles)
181  {
182  if (std::find_if(all_modes.begin(), all_modes.end(), [&](rs2::stream_profile p)
183  {
184  auto video = p.as<rs2::video_stream_profile>();
185  if (!video) return false;
186 
187  if (p.fps() == fps &&
188  p.stream_index() == profile.index &&
189  p.stream_type() == profile.stream &&
190  p.format() == profile.format &&
191  video.width() == profile.width &&
192  video.height() == profile.height)
193  {
194  modes.push_back(p);
195  return true;
196  }
197  return false;
198  }) != all_modes.end())
199  {
200  profiles.push_back(profile);
201 
202  }
203  }
204  if (modes.size() > 0)
205  REQUIRE_NOTHROW(sensor.open(modes));
206  return profiles;
207 }
208 
209 std::pair<std::vector<rs2::sensor>, std::vector<test_profile>> configure_all_supported_streams(rs2::device& dev, int width = 640, int height = 480, int fps = 30)
210 {
211  std::vector<test_profile > profiles;
212  std::vector<sensor > sensors;
213  auto sens = dev.query_sensors();
214  for (auto s : sens)
215  {
217  profiles.insert(profiles.end(), res.begin(), res.end());
218  if (res.size() > 0)
219  {
220  sensors.push_back(s);
221  }
222  }
223 
224  return{ sensors, profiles };
225 }
226 
227 TEST_CASE("Sync sanity", "[live]") {
228 
231  {
232  auto list = ctx.query_devices();
233  REQUIRE(list.size());
234 
235  auto dev = list[0];
237 
238  int fps = is_usb3(dev) ? 30 : 15; // In USB2 Mode the devices will switch to lower FPS rates
240  auto profiles = configure_all_supported_streams(dev,640,480, fps);
241 
242  for (auto s : dev.query_sensors())
243  {
244  s.start(sync);
245  }
246 
247  std::vector<std::vector<double>> all_timestamps;
248  auto actual_fps = fps;
249  bool hw_timestamp_domain = false;
250  bool system_timestamp_domain = false;
251  for (auto i = 0; i < 200; i++)
252  {
253  auto frames = sync.wait_for_frames(5000);
254  REQUIRE(frames.size() > 0);
255 
256  std::vector<double> timestamps;
257  for (auto&& f : frames)
258  {
259  if (f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS))
260  {
261  auto val = static_cast<int>(f.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS));
262  if (val < actual_fps)
263  actual_fps = val;
264  }
265  if (f.get_frame_timestamp_domain() == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK)
266  {
267  hw_timestamp_domain = true;
268  }
269  if (f.get_frame_timestamp_domain() == RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME)
270  {
271  system_timestamp_domain = true;
272  }
273  timestamps.push_back(f.get_timestamp());
274  }
275  all_timestamps.push_back(timestamps);
276 
277  }
278  for (auto i = 0; i < 30; i++)
279  {
280  auto frames = sync.wait_for_frames(500);
281  }
282 
283  CAPTURE(hw_timestamp_domain);
284  CAPTURE(system_timestamp_domain);
285  REQUIRE(hw_timestamp_domain != system_timestamp_domain);
286 
287  size_t num_of_partial_sync_sets = 0;
288  for (auto set_timestamps : all_timestamps)
289  {
290  if (set_timestamps.size() < profiles.second.size())
291  num_of_partial_sync_sets++;
292 
293  if (set_timestamps.size() <= 1)
294  continue;
295 
296  std::sort(set_timestamps.begin(), set_timestamps.end());
297  REQUIRE(set_timestamps[set_timestamps.size() - 1] - set_timestamps[0] <= (float)1000/(float)actual_fps);
298  }
299 
300  CAPTURE(num_of_partial_sync_sets);
301  CAPTURE(all_timestamps.size());
302  REQUIRE((float(num_of_partial_sync_sets) / all_timestamps.size()) < 0.9f);
303 
304  for (auto s : dev.query_sensors())
305  {
306  s.stop();
307  }
308  }
309 }
310 
311 TEST_CASE("Sync different fps", "[live][!mayfail]") {
312 
314 
316  {
317  auto list = ctx.query_devices();
318  REQUIRE(list.size());
319  auto dev = list[0];
320 
321  list = ctx.query_devices();
322  REQUIRE(list.size());
323  dev = list[0];
324 
325  syncer syncer;
327 
328  auto sensors = dev.query_sensors();
329 
330  for (auto s : sensors)
331  {
332  if (s.supports(RS2_OPTION_EXPOSURE))
333  {
334  auto range = s.get_option_range(RS2_OPTION_EXPOSURE);
335  REQUIRE_NOTHROW(s.set_option(RS2_OPTION_EXPOSURE, range.min));
336  }
337 
338  }
339 
340  std::map<rs2_stream, rs2::sensor*> profiles_sensors;
341  std::map<rs2::sensor*, rs2_stream> sensors_profiles;
342 
343  int fps_step = is_usb3(dev) ? 30 : 15; // USB2 Mode
344 
345  std::vector<int> fps(sensors.size(), 0);
346 
347  // The heuristic for FPS selection need to be better explained and probably refactored
348  for (auto i = 0; i < fps.size(); i++)
349  {
350  fps[i] = (fps.size() - i - 1) * fps_step % 90 + fps_step;
351  }
352  std::vector<std::vector<rs2::sensor*>> streams_groups(fps.size());
353  for (auto i = 0; i < sensors.size(); i++)
354  {
355  auto profs = configure_all_supported_streams(sensors[i], 640, 480, fps[i]);
356  for (auto p : profs)
357  {
358  profiles_sensors[p.stream] = &sensors[i];
359  sensors_profiles[&sensors[i]] = p.stream;
360  }
361  if (profs.size() > 0)
362  sensors[i].start(syncer);
363  }
364 
365  for (auto i = 0; i < sensors.size(); i++)
366  {
367  for (auto j = 0; j < sensors.size(); j++)
368  {
369  if ((float)fps[j] / (float)fps[i] >= 1)
370  {
371  streams_groups[i].push_back(&sensors[j]);
372  }
373 
374  }
375  }
376 
377  std::vector<std::vector<rs2_stream>> frames_arrived;
378 
379  for (auto i = 0; i < 200; i++)
380  {
381  auto frames = syncer.wait_for_frames(5000);
382  REQUIRE(frames.size() > 0);
383  std::vector<rs2_stream> streams_arrived;
384  for (auto&& f : frames)
385  {
386  auto s = f.get_profile().stream_type();
387  streams_arrived.push_back(s);
388  }
389  frames_arrived.push_back(streams_arrived);
390  }
391 
392  std::vector<int> streams_groups_arrrived(streams_groups.size(), 0);
393 
394  for (auto streams : frames_arrived)
395  {
396  std::set<rs2::sensor*> sensors;
397 
398  for (auto s : streams)
399  {
400  sensors.insert(profiles_sensors[s]);
401  }
402  std::vector<rs2::sensor*> sensors_vec(sensors.size());
403  std::copy(sensors.begin(), sensors.end(), sensors_vec.begin());
404  auto it = std::find(streams_groups.begin(), streams_groups.end(), sensors_vec);
405  if (it != streams_groups.end())
406  {
407  auto ind = std::distance(streams_groups.begin(), it);
408  streams_groups_arrrived[ind]++;
409  }
410  }
411 
412 
413  for (auto i = 0; i < streams_groups_arrrived.size(); i++)
414  {
415  for (auto j = 0; j < streams_groups_arrrived.size(); j++)
416  {
417  REQUIRE(streams_groups_arrrived[j]);
418  auto num1 = streams_groups_arrrived[i];
419  auto num2 = streams_groups_arrrived[j];
420  CAPTURE(sensors_profiles[&sensors[i]]);
421  CAPTURE(num1);
422  CAPTURE(sensors_profiles[&sensors[j]]);
423  CAPTURE(num2);
424  REQUIRE((float)num1 / (float)num2 <= 5 * (float)fps[i] / (float)fps[j]);
425  }
426 
427  }
428  }
429 }
430 
431 
432 bool get_mode(rs2::device& dev, rs2::stream_profile* profile, int mode_index = 0)
433 {
434  auto sensors = dev.query_sensors();
435  REQUIRE(sensors.size() > 0);
436 
437  for (auto i = 0; i < sensors.size(); i++)
438  {
439  auto modes = sensors[i].get_stream_profiles();
440  REQUIRE(modes.size() > 0);
441 
442  if (mode_index >= modes.size())
443  continue;
444 
445  *profile = modes[mode_index];
446  return true;
447  }
448  return false;
449 }
450 
451 TEST_CASE("Sync start stop", "[live]") {
453 
455  {
456  auto list = ctx.query_devices();
457  REQUIRE(list.size() > 0);
458  auto dev = list[0];
460 
461  syncer sync;
462 
464  auto mode_index = 0;
465  bool usb3_device = is_usb3(dev);
466  int fps = usb3_device ? 30 : 15; // In USB2 Mode the devices will switch to lower FPS rates
467  int req_fps = usb3_device ? 60 : 30; // USB2 Mode has only a single resolution for 60 fps which is not sufficient to run the test
468 
469  do
470  {
471  REQUIRE(get_mode(dev, &mode, mode_index));
472  mode_index++;
473  } while (mode.fps() != req_fps);
474 
475  auto video = mode.as<rs2::video_stream_profile>();
476  auto res = configure_all_supported_streams(dev, video.width(), video.height(), mode.fps());
477 
478  for (auto s : res.first)
479  {
480  REQUIRE_NOTHROW(s.start(sync));
481  }
482 
483 
485  for (auto i = 0; i < 30; i++)
486  {
487  frames = sync.wait_for_frames(10000);
488  REQUIRE(frames.size() > 0);
489  }
490 
491  frameset old_frames;
492 
493  while (sync.poll_for_frames(&old_frames));
494 
495  rs2::stream_profile other_mode;
496  mode_index = 0;
497 
498  REQUIRE(get_mode(dev, &other_mode, mode_index));
499  auto other_video = other_mode.as<rs2::video_stream_profile>();
500  while ((other_video.height() == video.height() && other_video.width() == video.width()) || other_video.fps() != req_fps)
501  {
502  CAPTURE(mode_index);
503  CAPTURE(video.format());
504  CAPTURE(video.width());
505  CAPTURE(video.height());
506  CAPTURE(video.fps());
507  CAPTURE(other_video.format());
508  CAPTURE(other_video.width());
509  CAPTURE(other_video.height());
510  CAPTURE(other_video.fps());
511  REQUIRE(get_mode(dev, &other_mode, mode_index));
512  mode_index++;
513  other_video = other_mode.as<rs2::video_stream_profile>();
514  REQUIRE(other_video);
515  }
516 
517  for (auto s : res.first)
518  {
519  REQUIRE_NOTHROW(s.stop());
520  REQUIRE_NOTHROW(s.close());
521  }
522  res = configure_all_supported_streams(dev, other_video.width(), other_video.height(), other_mode.fps());
523 
524  for (auto s : res.first)
525  {
526  REQUIRE_NOTHROW(s.start(sync));
527  }
528 
529  for (auto i = 0; i < 10; i++)
530  frames = sync.wait_for_frames(10000);
531 
532  REQUIRE(frames.size() > 0);
533  auto f = frames[0];
534  auto image = f.as<rs2::video_frame>();
535  REQUIRE(image);
536 
537  REQUIRE(image.get_width() == other_video.width());
538 
539  REQUIRE(image.get_height() == other_video.height());
540  }
541 }
542 TEST_CASE("Device metadata enumerates correctly", "[live]")
543 {
546  { // Require at least one device to be plugged in
547  std::vector<rs2::device> list;
548  REQUIRE_NOTHROW(list = ctx.query_devices());
549  REQUIRE(list.size() > 0);
550 
551  // For each device
552  for (auto&& dev : list)
553  {
554  SECTION("supported device metadata strings are nonempty, unsupported ones throw")
555  {
556  for (auto j = 0; j < RS2_CAMERA_INFO_COUNT; ++j) {
557  auto is_supported = false;
558  REQUIRE_NOTHROW(is_supported = dev.supports(rs2_camera_info(j)));
559  if (is_supported) REQUIRE(dev.get_info(rs2_camera_info(j)) != nullptr);
560  else REQUIRE_THROWS(dev.get_info(rs2_camera_info(j)));
561  }
562  }
563  }
564  }
565 }
566 
570 TEST_CASE("Start-Stop stream sequence", "[live]")
571 {
572  // Require at least one device to be plugged in
575  {
576  std::vector<sensor> list;
577  REQUIRE_NOTHROW(list = ctx.query_all_sensors());
578  REQUIRE(list.size() > 0);
579 
580  pipeline pipe(ctx);
581  device dev;
582  // Configure all supported streams to run at 30 frames per second
583 
584  for (auto i = 0; i < 5; i++)
585  {
588  REQUIRE_NOTHROW(profile = cfg.resolve(pipe));
589  REQUIRE(profile);
590  REQUIRE_NOTHROW(dev = profile.get_device());
591  REQUIRE(dev);
593 
594  // Test sequence
595  REQUIRE_NOTHROW(pipe.start(cfg));
596 
597  REQUIRE_NOTHROW(pipe.stop());
598  }
599  }
600 }
601 
605 
606 TEST_CASE("No extrinsic transformation between a stream and itself", "[live]")
607 {
608  // Require at least one device to be plugged in
611  {
612  std::vector<sensor> list;
613  REQUIRE_NOTHROW(list = ctx.query_all_sensors());
614  const size_t device_count = list.size();
615  REQUIRE(device_count > 0);
616 
617  // For each device
618  for (auto&& dev : list)
619  {
620  std::vector<rs2::stream_profile> profs;
621  REQUIRE_NOTHROW(profs = dev.get_stream_profiles());
622  REQUIRE(profs.size()>0);
623 
624  rs2_extrinsics extrin;
625  try {
626  auto prof = profs[0];
627  extrin = prof.get_extrinsics_to(prof);
628  }
629  catch (error &e) {
630  // if device isn't calibrated, get_extrinsics must error out (according to old comment. Might not be true under new API)
631  WARN(e.what());
632  continue;
633  }
634 
635  require_identity_matrix(extrin.rotation);
636  require_zero_vector(extrin.translation);
637  }
638  }
639 }
640 
641 TEST_CASE("Extrinsic transformation between two streams is a rigid transform", "[live]")
642 {
643  // Require at least one device to be plugged in
646  {
647  std::vector<sensor> list;
648  REQUIRE_NOTHROW(list = ctx.query_all_sensors());
649  const size_t device_count = list.size();
650  REQUIRE(device_count > 0);
651 
652  // For each device
653  for (int i = 0; i < device_count; ++i)
654  {
655  auto dev = list[i];
656  auto adj_devices = ctx.get_sensor_parent(dev).query_sensors();
657 
658  //REQUIRE(dev != nullptr);
659 
660  // For every pair of streams
661  for (auto j = 0; j < adj_devices.size(); ++j)
662  {
663  for (int k = j + 1; k < adj_devices.size(); ++k)
664  {
665  std::vector<rs2::stream_profile> profs_a, profs_b;
666  REQUIRE_NOTHROW(profs_a = adj_devices[j].get_stream_profiles());
667  REQUIRE(profs_a.size()>0);
668 
669  REQUIRE_NOTHROW(profs_b = adj_devices[k].get_stream_profiles());
670  REQUIRE(profs_b.size()>0);
671 
672  // Extrinsics from A to B should have an orthonormal 3x3 rotation matrix and a translation vector of magnitude less than 10cm
673  rs2_extrinsics a_to_b;
674 
675  try {
676  a_to_b = profs_a[0].get_extrinsics_to(profs_b[0]);
677  }
678  catch (error &e) {
679  WARN(e.what());
680  continue;
681  }
682 
683  require_rotation_matrix(a_to_b.rotation);
684  REQUIRE(vector_length(a_to_b.translation) < 0.1f);
685 
686  // Extrinsics from B to A should be the inverse of extrinsics from A to B
687  rs2_extrinsics b_to_a;
688  REQUIRE_NOTHROW(b_to_a = profs_b[0].get_extrinsics_to(profs_a[0]));
689 
690  require_transposed(a_to_b.rotation, b_to_a.rotation);
691  REQUIRE(b_to_a.rotation[0] * a_to_b.translation[0] + b_to_a.rotation[3] * a_to_b.translation[1] + b_to_a.rotation[6] * a_to_b.translation[2] == approx(-b_to_a.translation[0]));
692  REQUIRE(b_to_a.rotation[1] * a_to_b.translation[0] + b_to_a.rotation[4] * a_to_b.translation[1] + b_to_a.rotation[7] * a_to_b.translation[2] == approx(-b_to_a.translation[1]));
693  REQUIRE(b_to_a.rotation[2] * a_to_b.translation[0] + b_to_a.rotation[5] * a_to_b.translation[1] + b_to_a.rotation[8] * a_to_b.translation[2] == approx(-b_to_a.translation[2]));
694  }
695  }
696  }
697  }
698 }
699 
700 TEST_CASE("Extrinsic transformations are transitive", "[live]")
701 {
702  // Require at least one device to be plugged in
705  {
706  std::vector<sensor> list;
707  REQUIRE_NOTHROW(list = ctx.query_all_sensors());
708  REQUIRE(list.size() > 0);
709 
710  // For each device
711  for (auto&& dev : list)
712  {
713  auto adj_devices = ctx.get_sensor_parent(dev).query_sensors();
714 
715  // For every set of subdevices
716  for (auto a = 0; a < adj_devices.size(); ++a)
717  {
718  for (auto b = 0; b < adj_devices.size(); ++b)
719  {
720  for (auto c = 0; c < adj_devices.size(); ++c)
721  {
722  std::vector<rs2::stream_profile> profs_a, profs_b, profs_c ;
723  REQUIRE_NOTHROW(profs_a = adj_devices[a].get_stream_profiles());
724  REQUIRE(profs_a.size()>0);
725 
726  REQUIRE_NOTHROW(profs_b = adj_devices[b].get_stream_profiles());
727  REQUIRE(profs_b.size()>0);
728 
729  REQUIRE_NOTHROW(profs_c = adj_devices[c].get_stream_profiles());
730  REQUIRE(profs_c.size()>0);
731 
732  // Require that the composition of a_to_b and b_to_c is equal to a_to_c
733  rs2_extrinsics a_to_b, b_to_c, a_to_c;
734 
735  try {
736  a_to_b = profs_a[0].get_extrinsics_to(profs_b[0]);
737  b_to_c = profs_b[0].get_extrinsics_to(profs_c[0]);
738  a_to_c = profs_a[0].get_extrinsics_to(profs_c[0]);
739  }
740  catch (error &e)
741  {
742  WARN(e.what());
743  continue;
744  }
745 
746  // a_to_c.rotation == a_to_b.rotation * b_to_c.rotation
747  REQUIRE(a_to_c.rotation[0] == approx(a_to_b.rotation[0] * b_to_c.rotation[0] + a_to_b.rotation[3] * b_to_c.rotation[1] + a_to_b.rotation[6] * b_to_c.rotation[2]));
748  REQUIRE(a_to_c.rotation[2] == approx(a_to_b.rotation[2] * b_to_c.rotation[0] + a_to_b.rotation[5] * b_to_c.rotation[1] + a_to_b.rotation[8] * b_to_c.rotation[2]));
749  REQUIRE(a_to_c.rotation[1] == approx(a_to_b.rotation[1] * b_to_c.rotation[0] + a_to_b.rotation[4] * b_to_c.rotation[1] + a_to_b.rotation[7] * b_to_c.rotation[2]));
750  REQUIRE(a_to_c.rotation[3] == approx(a_to_b.rotation[0] * b_to_c.rotation[3] + a_to_b.rotation[3] * b_to_c.rotation[4] + a_to_b.rotation[6] * b_to_c.rotation[5]));
751  REQUIRE(a_to_c.rotation[4] == approx(a_to_b.rotation[1] * b_to_c.rotation[3] + a_to_b.rotation[4] * b_to_c.rotation[4] + a_to_b.rotation[7] * b_to_c.rotation[5]));
752  REQUIRE(a_to_c.rotation[5] == approx(a_to_b.rotation[2] * b_to_c.rotation[3] + a_to_b.rotation[5] * b_to_c.rotation[4] + a_to_b.rotation[8] * b_to_c.rotation[5]));
753  REQUIRE(a_to_c.rotation[6] == approx(a_to_b.rotation[0] * b_to_c.rotation[6] + a_to_b.rotation[3] * b_to_c.rotation[7] + a_to_b.rotation[6] * b_to_c.rotation[8]));
754  REQUIRE(a_to_c.rotation[7] == approx(a_to_b.rotation[1] * b_to_c.rotation[6] + a_to_b.rotation[4] * b_to_c.rotation[7] + a_to_b.rotation[7] * b_to_c.rotation[8]));
755  REQUIRE(a_to_c.rotation[8] == approx(a_to_b.rotation[2] * b_to_c.rotation[6] + a_to_b.rotation[5] * b_to_c.rotation[7] + a_to_b.rotation[8] * b_to_c.rotation[8]));
756 
757  // a_to_c.translation = a_to_b.transform(b_to_c.translation)
758  REQUIRE(a_to_c.translation[0] == approx(a_to_b.rotation[0] * b_to_c.translation[0] + a_to_b.rotation[3] * b_to_c.translation[1] + a_to_b.rotation[6] * b_to_c.translation[2] + a_to_b.translation[0]));
759  REQUIRE(a_to_c.translation[1] == approx(a_to_b.rotation[1] * b_to_c.translation[0] + a_to_b.rotation[4] * b_to_c.translation[1] + a_to_b.rotation[7] * b_to_c.translation[2] + a_to_b.translation[1]));
760  REQUIRE(a_to_c.translation[2] == approx(a_to_b.rotation[2] * b_to_c.translation[0] + a_to_b.rotation[5] * b_to_c.translation[1] + a_to_b.rotation[8] * b_to_c.translation[2] + a_to_b.translation[2]));
761  }
762  }
763  }
764  }
765  }
766 }
767 
768 std::shared_ptr<device> do_with_waiting_for_camera_connection(rs2::context ctx, std::shared_ptr<device> dev, std::string serial, std::function<void()> operation)
769 {
770  std::mutex m;
771  bool disconnected = false;
772  bool connected = false;
773  std::shared_ptr<device> result;
774  std::condition_variable cv;
775 
776  ctx.set_devices_changed_callback([&result, dev, &disconnected, &connected, &m, &cv, &serial](rs2::event_information info) mutable
777  {
778  if (info.was_removed(*dev))
779  {
780  std::unique_lock<std::mutex> lock(m);
781  disconnected = true;
782  cv.notify_all();
783  }
784  auto list = info.get_new_devices();
785  if (list.size() > 0)
786  {
787  for (auto cam : list)
788  {
789  if (serial == cam.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER))
790  {
791  std::unique_lock<std::mutex> lock(m);
792  connected = true;
793  result = std::make_shared<device>(cam);
794 
796  cv.notify_all();
797  break;
798  }
799  }
800  }
801  });
802 
803  operation();
804 
805  std::unique_lock<std::mutex> lock(m);
806  REQUIRE(wait_for_reset([&]() {
807  return cv.wait_for(lock, std::chrono::seconds(20), [&]() { return disconnected; });
808  }, dev));
809  REQUIRE(cv.wait_for(lock, std::chrono::seconds(20), [&]() { return connected; }));
810  REQUIRE(result);
811  return result;
812 }
813 
814 TEST_CASE("Toggle Advanced Mode", "[live][AdvMd]") {
815  for (int i = 0; i < 3; ++i)
816  {
819  {
820  device_list list;
821  REQUIRE_NOTHROW(list = ctx.query_devices());
822  REQUIRE(list.size() > 0);
823 
824  auto dev = std::make_shared<device>(list.front());
825 
827 
828  std::string serial;
829  REQUIRE_NOTHROW(serial = dev->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
830 
831  if (dev->is<rs400::advanced_mode>())
832  {
833  auto advanced = dev->as<rs400::advanced_mode>();
834 
835  if (!advanced.is_enabled())
836  {
837  dev = do_with_waiting_for_camera_connection(ctx, dev, serial, [&]()
838  {
839  REQUIRE_NOTHROW(advanced.toggle_advanced_mode(true));
840  });
841  }
843  advanced = dev->as<rs400::advanced_mode>();
844 
845  REQUIRE(advanced.is_enabled());
846 
847  dev = do_with_waiting_for_camera_connection(ctx, dev, serial, [&]()
848  {
849  REQUIRE_NOTHROW(advanced.toggle_advanced_mode(false));
850  });
851  advanced = dev->as<rs400::advanced_mode>();
852  REQUIRE(!advanced.is_enabled());
853  }
854  }
855  }
856 }
857 
858 
859 TEST_CASE("Advanced Mode presets", "[live][AdvMd]")
860 {
861  static const std::vector<res_type> resolutions = { low_resolution,
863  high_resolution };
864 
867  {
868  device_list list;
869  REQUIRE_NOTHROW(list = ctx.query_devices());
870  REQUIRE(list.size() > 0);
871 
872  auto dev = std::make_shared<device>(list.front());
873 
875 
876  std::string serial;
877  REQUIRE_NOTHROW(serial = dev->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
878 
879  if (dev->is<rs400::advanced_mode>())
880  {
881  auto advanced = dev->as<rs400::advanced_mode>();
882 
883  if (!advanced.is_enabled())
884  {
885  dev = do_with_waiting_for_camera_connection(ctx, dev, serial, [&]()
886  {
887  REQUIRE_NOTHROW(advanced.toggle_advanced_mode(true));
888  });
889  }
891  advanced = dev->as<rs400::advanced_mode>();
892 
893  REQUIRE(advanced.is_enabled());
894  auto sensors = dev->query_sensors();
895 
896  sensor presets_sensor;
897  for (sensor& elem : sensors)
898  {
899  auto supports = false;
900  REQUIRE_NOTHROW(supports = elem.supports(RS2_OPTION_VISUAL_PRESET));
901  if (supports)
902  {
903  presets_sensor = elem;
904  break;
905  }
906  }
907 
908 
909  for (auto& res : resolutions)
910  {
911  std::vector<rs2::stream_profile> sp = {get_profile_by_resolution_type(presets_sensor, res)};
912  presets_sensor.open(sp);
913  presets_sensor.start([](rs2::frame) {});
914  for (auto i = 0; i < RS2_RS400_VISUAL_PRESET_COUNT; ++i)
915  {
917  CAPTURE(res);
918  CAPTURE(preset);
920  float ret_preset;
921  REQUIRE_NOTHROW(ret_preset = presets_sensor.get_option(RS2_OPTION_VISUAL_PRESET));
922  REQUIRE(preset == (rs2_rs400_visual_preset)((int)ret_preset));
923  }
924  presets_sensor.stop();
925  presets_sensor.close();
926  }
927 
928  dev = do_with_waiting_for_camera_connection(ctx, dev, serial, [&]()
929  {
930  REQUIRE_NOTHROW(advanced.toggle_advanced_mode(false));
931  });
933  advanced = dev->as<rs400::advanced_mode>();
934  REQUIRE(!advanced.is_enabled());
935  }
936  }
937 }
938 
939 TEST_CASE("Advanced Mode JSON", "[live][AdvMd]") {
942  {
943  device_list list;
944  REQUIRE_NOTHROW(list = ctx.query_devices());
945  REQUIRE(list.size() > 0);
946 
947 
948  auto dev = std::make_shared<device>(list.front());
949 
951 
952  std::string serial;
953  REQUIRE_NOTHROW(serial = dev->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
954 
955  if (dev->is<rs400::advanced_mode>())
956  {
957  auto advanced = dev->as<rs400::advanced_mode>();
958 
959  if (!advanced.is_enabled())
960  {
961  dev = do_with_waiting_for_camera_connection(ctx, dev, serial, [&]()
962  {
963  REQUIRE_NOTHROW(advanced.toggle_advanced_mode(true));
964  });
965  }
966 
968  advanced = dev->as<rs400::advanced_mode>();
969  REQUIRE(advanced.is_enabled());
970 
971  auto sensors = dev->query_sensors();
972  sensor presets_sensor;
973  for (sensor& elem : sensors)
974  {
975  auto supports = false;
976  REQUIRE_NOTHROW(supports = elem.supports(RS2_OPTION_VISUAL_PRESET));
977  if (supports)
978  {
979  presets_sensor = elem;
980  break;
981  }
982  }
983 
984  std::string json1, json2;
985  REQUIRE_NOTHROW(json1 = advanced.serialize_json());
987  REQUIRE_NOTHROW(advanced.load_json(json1));
988  REQUIRE_NOTHROW(json2 = advanced.serialize_json());
989  REQUIRE_NOTHROW(json1 == json2);
990 
991  dev = do_with_waiting_for_camera_connection(ctx, dev, serial, [&]()
992  {
993  REQUIRE_NOTHROW(advanced.toggle_advanced_mode(false));
994  });
996  advanced = dev->as<rs400::advanced_mode>();
997  REQUIRE(!advanced.is_enabled());
998  }
999  }
1000 }
1001 
1002 TEST_CASE("Advanced Mode controls", "[live][AdvMd]") {
1003  rs2::context ctx;
1005  {
1006  device_list list;
1007  REQUIRE_NOTHROW(list = ctx.query_devices());
1008  REQUIRE(list.size() > 0);
1009 
1010  std::shared_ptr<device> dev = std::make_shared<device>(list.front());
1011  if (dev->is<rs400::advanced_mode>())
1012  {
1014  auto info = dev->get_info(RS2_CAMERA_INFO_NAME);
1015  CAPTURE(info);
1016 
1017  std::string serial;
1018  REQUIRE_NOTHROW(serial = dev->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
1019 
1020 
1021  auto advanced = dev->as<rs400::advanced_mode>();
1022  if (!advanced.is_enabled())
1023  {
1024 
1025  dev = do_with_waiting_for_camera_connection(ctx, dev, serial, [&]()
1026  {
1027  REQUIRE_NOTHROW(advanced.toggle_advanced_mode(true));
1028  });
1029  }
1030 
1031 
1032 
1034  advanced = dev->as<rs400::advanced_mode>();
1035  REQUIRE(advanced.is_enabled());
1036 
1037  {
1038  STDepthControlGroup ctrl_curr{};
1039  REQUIRE_NOTHROW(ctrl_curr = advanced.get_depth_control(0));
1040  STDepthControlGroup ctrl_min{};
1041  REQUIRE_NOTHROW(ctrl_min = advanced.get_depth_control(1));
1042  STDepthControlGroup ctrl_max{};
1043  REQUIRE_NOTHROW(ctrl_max = advanced.get_depth_control(2));
1044  REQUIRE_NOTHROW(advanced.set_depth_control(ctrl_min));
1045  REQUIRE_NOTHROW(ctrl_curr = advanced.get_depth_control(0));
1046  REQUIRE(ctrl_curr == ctrl_min);
1047  }
1048 
1049  {
1050  STRsm ctrl_curr{};
1051  REQUIRE_NOTHROW(ctrl_curr = advanced.get_rsm(0));
1052  STRsm ctrl_min{};
1053  REQUIRE_NOTHROW(ctrl_min = advanced.get_rsm(1));
1054  STRsm ctrl_max{};
1055  REQUIRE_NOTHROW(ctrl_max = advanced.get_rsm(2));
1056  REQUIRE_NOTHROW(advanced.set_rsm(ctrl_min));
1057  REQUIRE_NOTHROW(ctrl_curr = advanced.get_rsm(0));
1058  REQUIRE(ctrl_curr == ctrl_min);
1059  }
1060 
1061  {
1062  STRauSupportVectorControl ctrl_curr{};
1063  REQUIRE_NOTHROW(ctrl_curr = advanced.get_rau_support_vector_control(0));
1064  STRauSupportVectorControl ctrl_min{};
1065  REQUIRE_NOTHROW(ctrl_min = advanced.get_rau_support_vector_control(1));
1066  STRauSupportVectorControl ctrl_max{};
1067  REQUIRE_NOTHROW(ctrl_max = advanced.get_rau_support_vector_control(2));
1068  REQUIRE_NOTHROW(advanced.set_rau_support_vector_control(ctrl_min));
1069  REQUIRE_NOTHROW(ctrl_curr = advanced.get_rau_support_vector_control(0));
1070  REQUIRE(ctrl_curr == ctrl_min);
1071  }
1072 
1073  {
1074  STColorControl ctrl_curr{};
1075  REQUIRE_NOTHROW(ctrl_curr = advanced.get_color_control(0));
1076  STColorControl ctrl_min{};
1077  REQUIRE_NOTHROW(ctrl_min = advanced.get_color_control(1));
1078  STColorControl ctrl_max{};
1079  REQUIRE_NOTHROW(ctrl_max = advanced.get_color_control(2));
1080  REQUIRE_NOTHROW(advanced.set_color_control(ctrl_min));
1081  REQUIRE_NOTHROW(ctrl_curr = advanced.get_color_control(0));
1082  REQUIRE(ctrl_curr == ctrl_min);
1083  }
1084 
1085  {
1086  STRauColorThresholdsControl ctrl_curr{};
1087  REQUIRE_NOTHROW(ctrl_curr = advanced.get_rau_thresholds_control(0));
1088  STRauColorThresholdsControl ctrl_min{};
1089  REQUIRE_NOTHROW(ctrl_min = advanced.get_rau_thresholds_control(1));
1090  STRauColorThresholdsControl ctrl_max{};
1091  REQUIRE_NOTHROW(ctrl_max = advanced.get_rau_thresholds_control(2));
1092  REQUIRE_NOTHROW(advanced.set_rau_thresholds_control(ctrl_min));
1093  REQUIRE_NOTHROW(ctrl_curr = advanced.get_rau_thresholds_control(0));
1094  REQUIRE(ctrl_curr == ctrl_min);
1095  }
1096 
1097  {
1098  STSloColorThresholdsControl ctrl_curr{};
1099  REQUIRE_NOTHROW(ctrl_curr = advanced.get_slo_color_thresholds_control(0));
1100  STSloColorThresholdsControl ctrl_min{};
1101  REQUIRE_NOTHROW(ctrl_min = advanced.get_slo_color_thresholds_control(1));
1102  STSloColorThresholdsControl ctrl_max{};
1103  REQUIRE_NOTHROW(ctrl_max = advanced.get_slo_color_thresholds_control(2));
1104  REQUIRE_NOTHROW(advanced.set_slo_color_thresholds_control(ctrl_min));
1105  REQUIRE_NOTHROW(ctrl_curr = advanced.get_slo_color_thresholds_control(0));
1106  REQUIRE(ctrl_curr == ctrl_min);
1107  }
1108 
1109  {
1110  STSloPenaltyControl ctrl_curr{};
1111  REQUIRE_NOTHROW(ctrl_curr = advanced.get_slo_penalty_control(0));
1112  STSloPenaltyControl ctrl_min{};
1113  REQUIRE_NOTHROW(ctrl_min = advanced.get_slo_penalty_control(1));
1114  STSloPenaltyControl ctrl_max{};
1115  REQUIRE_NOTHROW(ctrl_max = advanced.get_slo_penalty_control(2));
1116  REQUIRE_NOTHROW(advanced.set_slo_penalty_control(ctrl_min));
1117  REQUIRE_NOTHROW(ctrl_curr = advanced.get_slo_penalty_control(0));
1118  REQUIRE(ctrl_curr == ctrl_min);
1119  }
1120 
1121  {
1122  STHdad ctrl_curr1{};
1123  REQUIRE_NOTHROW(ctrl_curr1 = advanced.get_hdad(0));
1124  REQUIRE_NOTHROW(advanced.set_hdad(ctrl_curr1));
1125  STHdad ctrl_curr2{};
1126  REQUIRE_NOTHROW(ctrl_curr2 = advanced.get_hdad(0));
1127  REQUIRE(ctrl_curr1 == ctrl_curr2);
1128  }
1129 
1130  {
1131  STColorCorrection ctrl_curr{};
1132  REQUIRE_NOTHROW(ctrl_curr = advanced.get_color_correction(0));
1133  STColorCorrection ctrl_min{};
1134  REQUIRE_NOTHROW(ctrl_min = advanced.get_color_correction(1));
1135  STColorCorrection ctrl_max{};
1136  REQUIRE_NOTHROW(ctrl_max = advanced.get_color_correction(2));
1137  REQUIRE_NOTHROW(advanced.set_color_correction(ctrl_min));
1138  REQUIRE_NOTHROW(ctrl_curr = advanced.get_color_correction(0));
1139  REQUIRE(ctrl_curr == ctrl_min);
1140  }
1141 
1142  {
1143  STAEControl ctrl_curr1{};
1144  REQUIRE_NOTHROW(ctrl_curr1 = advanced.get_ae_control(0));
1145  REQUIRE_NOTHROW(advanced.set_ae_control(ctrl_curr1));
1146  STAEControl ctrl_curr2{};
1147  REQUIRE_NOTHROW(ctrl_curr2 = advanced.get_ae_control(0));
1148  REQUIRE(ctrl_curr1 == ctrl_curr2);
1149  }
1150 
1151  {
1152  STDepthTableControl ctrl_curr{};
1153  REQUIRE_NOTHROW(ctrl_curr = advanced.get_depth_table(0));
1154  STDepthTableControl ctrl_min{};
1155  REQUIRE_NOTHROW(ctrl_min = advanced.get_depth_table(1));
1156  STDepthTableControl ctrl_max{};
1157  REQUIRE_NOTHROW(ctrl_max = advanced.get_depth_table(2));
1158  REQUIRE_NOTHROW(advanced.set_depth_table(ctrl_min));
1159  REQUIRE_NOTHROW(ctrl_curr = advanced.get_depth_table(0));
1160  REQUIRE(ctrl_curr == ctrl_min);
1161  }
1162 
1163  {
1164  STCensusRadius ctrl_curr{};
1165  REQUIRE_NOTHROW(ctrl_curr = advanced.get_census(0));
1166  STCensusRadius ctrl_min{};
1167  REQUIRE_NOTHROW(ctrl_min = advanced.get_census(1));
1168  STCensusRadius ctrl_max{};
1169  REQUIRE_NOTHROW(ctrl_max = advanced.get_census(2));
1170  REQUIRE_NOTHROW(advanced.set_census(ctrl_min));
1171  REQUIRE_NOTHROW(ctrl_curr = advanced.get_census(0));
1172  REQUIRE(ctrl_curr == ctrl_min);
1173  }
1174 
1175 
1176  dev = do_with_waiting_for_camera_connection(ctx, dev, serial, [&]()
1177  {
1178  REQUIRE_NOTHROW(advanced.toggle_advanced_mode(false));
1179  });
1180 
1182  advanced = dev->as<rs400::advanced_mode>();
1183 
1184  REQUIRE(!advanced.is_enabled());
1185  }
1186  }
1187 }
1188 
1189 // the tests may incorrectly interpret changes to librealsense-core, namely default profiles selections
1190 TEST_CASE("Streaming modes sanity check", "[live][!mayfail]")
1191 {
1192  // Require at least one device to be plugged in
1193  rs2::context ctx;
1195  {
1196  std::vector<sensor> list;
1197  REQUIRE_NOTHROW(list = ctx.query_all_sensors());
1198  REQUIRE(list.size() > 0);
1199 
1200  // For each device
1201  for (auto&& dev : list)
1202  {
1204 
1205  std::string PID;
1206  REQUIRE_NOTHROW(PID = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID));
1207 
1208  // make sure they provide at least one streaming mode
1209  std::vector<rs2::stream_profile> stream_profiles;
1210  REQUIRE_NOTHROW(stream_profiles = dev.get_stream_profiles());
1211  REQUIRE(stream_profiles.size() > 0);
1212 
1213  SECTION("check stream profile settings are sane")
1214  {
1215  // for each stream profile provided:
1216  for (auto profile : stream_profiles) {
1217 
1218  // require that the settings are sane
1221  REQUIRE(profile.fps() >= 2);
1222  REQUIRE(profile.fps() <= 300);
1223 
1224  if (auto video = profile.as<video_stream_profile>())
1225  {
1226  REQUIRE(video.width() >= 320);
1227  REQUIRE(video.width() <= 1920);
1228  REQUIRE(video.height() >= 180);
1229  REQUIRE(video.height() <= 1080);
1230  }
1231 
1232  // require that we can start streaming this mode
1233  REQUIRE_NOTHROW(dev.open({ profile }));
1234  // TODO: make callback confirm stream format/dimensions/framerate
1235  REQUIRE_NOTHROW(dev.start([](rs2::frame fref) {}));
1236 
1237  // Require that we can disable the stream afterwards
1238  REQUIRE_NOTHROW(dev.stop());
1239  REQUIRE_NOTHROW(dev.close());
1240  }
1241  }
1242  SECTION("check stream intrinsics are sane")
1243  {
1244  for (auto profile : stream_profiles)
1245  {
1246  if (auto video = profile.as<video_stream_profile>())
1247  {
1249  CAPTURE(video.stream_type());
1250  CAPTURE(video.format());
1251  CAPTURE(video.width());
1252  CAPTURE(video.height());
1253 
1254  bool calib_format = ((PID != "0AA5") &&
1255  (RS2_FORMAT_Y16 == video.format()) &&
1256  (RS2_STREAM_INFRARED == video.stream_type()));
1257  if (!calib_format) // intrinsics are not available for calibration formats
1258  {
1259  REQUIRE_NOTHROW(intrin = video.get_intrinsics());
1260 
1261  // Intrinsic width/height must match width/height of streaming mode we requested
1262  REQUIRE(intrin.width == video.width());
1263  REQUIRE(intrin.height == video.height());
1264 
1265  // Principal point must be within center 20% of image
1266  REQUIRE(intrin.ppx > video.width() * 0.4f);
1267  REQUIRE(intrin.ppx < video.width() * 0.6f);
1268  REQUIRE(intrin.ppy > video.height() * 0.4f);
1269  REQUIRE(intrin.ppy < video.height() * 0.6f);
1270 
1271  // Focal length must be non-negative (todo - Refine requirements based on known expected FOV)
1272  REQUIRE(intrin.fx > 0.0f);
1273  REQUIRE(intrin.fy > 0.0f);
1274  }
1275  else
1276  {
1277  REQUIRE_THROWS(intrin = video.get_intrinsics());
1278  }
1279  }
1280  }
1281  }
1282  }
1283  }
1284 }
1285 
1286 TEST_CASE("Motion profiles sanity", "[live]")
1287 {
1288  rs2::context ctx;
1290  {
1291  std::vector<sensor> list;
1292  REQUIRE_NOTHROW(list = ctx.query_all_sensors());
1293  REQUIRE(list.size() > 0);
1294 
1295  // For each device
1296  for (auto&& dev : list)
1297  {
1299 
1300  // make sure they provide at least one streaming mode
1301  std::vector<rs2::stream_profile> stream_profiles;
1302  REQUIRE_NOTHROW(stream_profiles = dev.get_stream_profiles());
1303  REQUIRE(stream_profiles.size() > 0);
1304 
1305  // for each stream profile provided:
1306  for (auto profile : stream_profiles)
1307  {
1308  SECTION("check motion intrisics") {
1309 
1310  auto stream = profile.stream_type();
1312 
1313  CAPTURE(stream);
1314 
1316  {
1317  auto motion = profile.as<motion_stream_profile>();
1318  REQUIRE_NOTHROW(mm_int = motion.get_motion_intrinsics());
1319 
1320  for (int j = 0; j < 3; j++)
1321  {
1322  auto scale = mm_int.data[j][j];
1323  CAPTURE(scale);
1324  // Make sure scale value is "sane"
1325  // We don't expect Motion Device to require adjustment of more then 20%
1326  REQUIRE(scale > 0.8);
1327  REQUIRE(scale < 1.2);
1328 
1329  auto bias = mm_int.data[0][3];
1330  CAPTURE(bias);
1331  // Make sure bias is "sane"
1332  REQUIRE(bias > -0.5);
1333  REQUIRE(bias < 0.5);
1334  }
1335  }
1336  }
1337  }
1338  }
1339  }
1340 }
1341 
1342 TEST_CASE("Check width and height of stream intrinsics", "[live][AdvMd]")
1343 {
1344  rs2::context ctx;
1346  {
1347  std::vector<device> devs;
1348  REQUIRE_NOTHROW(devs = ctx.query_devices());
1349 
1350  for (auto&& dev : devs)
1351  {
1352  auto shared_dev = std::make_shared<device>(dev);
1354  std::string serial;
1355  REQUIRE_NOTHROW(serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
1356  std::string PID;
1357  REQUIRE_NOTHROW(PID = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID));
1358  if (shared_dev->is<rs400::advanced_mode>())
1359  {
1360  auto advanced = shared_dev->as<rs400::advanced_mode>();
1361 
1362  if (advanced.is_enabled())
1363  {
1364  shared_dev = do_with_waiting_for_camera_connection(ctx, shared_dev, serial, [&]()
1365  {
1366  REQUIRE_NOTHROW(advanced.toggle_advanced_mode(false));
1367  });
1368  }
1369  disable_sensitive_options_for(*shared_dev);
1370  advanced = shared_dev->as<rs400::advanced_mode>();
1371 
1372  REQUIRE(advanced.is_enabled() == false);
1373  }
1374 
1375  std::vector<sensor> list;
1376  REQUIRE_NOTHROW(list = shared_dev->query_sensors());
1377  REQUIRE(list.size() > 0);
1378 
1379  for (auto&& dev : list)
1380  {
1382  auto module_name = dev.get_info(RS2_CAMERA_INFO_NAME);
1383  // TODO: if FE
1384  std::vector<rs2::stream_profile> stream_profiles;
1385  REQUIRE_NOTHROW(stream_profiles = dev.get_stream_profiles());
1386  REQUIRE(stream_profiles.size() > 0);
1387 
1388  // for each stream profile provided:
1389  int i=0;
1390  for (const auto& profile : stream_profiles)
1391  {
1392  i++;
1393  if (auto video = profile.as<video_stream_profile>())
1394  {
1396 
1397  CAPTURE(video.stream_type());
1398  CAPTURE(video.format());
1399  CAPTURE(video.width());
1400  CAPTURE(video.height());
1401 
1402  // Calibration formats does not provide intrinsic data, except for SR300
1403  bool calib_format = ((PID != "0AA5") &&
1404  (RS2_FORMAT_Y16 == video.format()) &&
1405  (RS2_STREAM_INFRARED == video.stream_type()));
1406  if (!calib_format)
1407  REQUIRE_NOTHROW(intrin = video.get_intrinsics());
1408  else
1409  REQUIRE_THROWS(intrin = video.get_intrinsics());
1410 
1411  // Intrinsic width/height must match width/height of streaming mode we requested
1412  REQUIRE(intrin.width == video.width());
1413  REQUIRE(intrin.height == video.height());
1414  }
1415  }
1416  }
1417  }
1418  }
1419 }
1420 
1421 TEST_CASE("Check option API", "[live][options]")
1422 {
1423  // Require at least one device to be plugged in
1424  rs2::context ctx;
1426  {
1427  std::vector<sensor> list;
1428  REQUIRE_NOTHROW(list = ctx.query_all_sensors());
1429  REQUIRE(list.size() > 0);
1430 
1431  // for each device
1432  for (auto&& dev : list)
1433  {
1434 
1435  // for each option
1436  for (auto i = 0; i < RS2_OPTION_COUNT; ++i) {
1437  auto opt = rs2_option(i);
1438  bool is_opt_supported;
1439  REQUIRE_NOTHROW(is_opt_supported = dev.supports(opt));
1440 
1441 
1442  SECTION("Ranges are sane")
1443  {
1444  if (!is_opt_supported)
1445  {
1446  REQUIRE_THROWS_AS(dev.get_option_range(opt), error);
1447  }
1448  else
1449  {
1451  REQUIRE_NOTHROW(range = dev.get_option_range(opt));
1452 
1453  // a couple sanity checks
1454  REQUIRE(range.min < range.max);
1455  REQUIRE(range.min + range.step <= range.max);
1456  REQUIRE(range.step > 0);
1457  REQUIRE(range.def <= range.max);
1458  REQUIRE(range.min <= range.def);
1459 
1460  // TODO: check that range.def == range.min + k*range.step for some k?
1461  // TODO: some sort of bounds checking against constants?
1462  }
1463  }
1464  SECTION("get_option returns a legal value")
1465  {
1466  if (!is_opt_supported)
1467  {
1468  REQUIRE_THROWS_AS(dev.get_option(opt), error);
1469  }
1470  else
1471  {
1472  auto range = dev.get_option_range(opt);
1473  float value;
1474  REQUIRE_NOTHROW(value = dev.get_option(opt));
1475 
1476  // value in range. Do I need to account for epsilon in lt[e]/gt[e] comparisons?
1477  REQUIRE(value >= range.min);
1478  REQUIRE(value <= range.max);
1479 
1480  // value doesn't change between two gets (if no additional threads are calling set)
1481  REQUIRE(dev.get_option(opt) == approx(value));
1482 
1483  // REQUIRE(value == approx(range.def)); // Not sure if this is a reasonable check
1484  // TODO: make sure value == range.min + k*range.step for some k?
1485  }
1486  }
1487  SECTION("set opt doesn't like bad values") {
1488  if (!is_opt_supported)
1489  {
1490  REQUIRE_THROWS_AS(dev.set_option(opt, 1), error);
1491  }
1492  else
1493  {
1494  auto range = dev.get_option_range(opt);
1495 
1496  // minimum should work, as should maximum
1497  REQUIRE_NOTHROW(dev.set_option(opt, range.min));
1498  REQUIRE_NOTHROW(dev.set_option(opt, range.max));
1499 
1500  int n_steps = int((range.max - range.min) / range.step);
1501 
1502  // check a few arbitrary points along the scale
1503  REQUIRE_NOTHROW(dev.set_option(opt, range.min + (1 % n_steps)*range.step));
1504  REQUIRE_NOTHROW(dev.set_option(opt, range.min + (11 % n_steps)*range.step));
1505  REQUIRE_NOTHROW(dev.set_option(opt, range.min + (111 % n_steps)*range.step));
1506  REQUIRE_NOTHROW(dev.set_option(opt, range.min + (1111 % n_steps)*range.step));
1507 
1508  // below min and above max shouldn't work
1509  REQUIRE_THROWS_AS(dev.set_option(opt, range.min - range.step), error);
1510  REQUIRE_THROWS_AS(dev.set_option(opt, range.max + range.step), error);
1511 
1512  // make sure requesting value in the range, but not a legal step doesn't work
1513  // TODO: maybe something for range.step < 1 ?
1514  for (auto j = 1; j < range.step; j++) {
1515  CAPTURE(range.step);
1516  CAPTURE(j);
1517  REQUIRE_THROWS_AS(dev.set_option(opt, range.min + j), error);
1518  }
1519  }
1520  }
1521  SECTION("check get/set sequencing works as expected") {
1522  if (!is_opt_supported) continue;
1523  auto range = dev.get_option_range(opt);
1524 
1525  // setting a valid value lets you get that value back
1526  dev.set_option(opt, range.min);
1527  REQUIRE(dev.get_option(opt) == approx(range.min));
1528 
1529  // setting an invalid value returns the last set valid value.
1530  REQUIRE_THROWS(dev.set_option(opt, range.max + range.step));
1531  REQUIRE(dev.get_option(opt) == approx(range.min));
1532 
1533  dev.set_option(opt, range.max);
1534  REQUIRE_THROWS(dev.set_option(opt, range.min - range.step));
1535  REQUIRE(dev.get_option(opt) == approx(range.max));
1536 
1537  }
1538  SECTION("get_description returns a non-empty, non-null string") {
1539  if (!is_opt_supported) {
1540  REQUIRE_THROWS_AS(dev.get_option_description(opt), error);
1541  }
1542  else
1543  {
1544  REQUIRE(dev.get_option_description(opt) != nullptr);
1545  REQUIRE(std::string(dev.get_option_description(opt)) != std::string(""));
1546  }
1547  }
1548  // TODO: tests for get_option_value_description? possibly too open a function to have useful tests
1549  }
1550  }
1551  }
1552 }
1553 
1556 TEST_CASE("Multiple devices", "[live][multicam][!mayfail]")
1557 {
1558  rs2::context ctx;
1560  {
1561  // Require at least one device to be plugged in
1562  std::vector<sensor> list;
1563  REQUIRE_NOTHROW(list = ctx.query_all_sensors());
1564  REQUIRE(list.size() > 0);
1565 
1566  SECTION("subdevices on a single device")
1567  {
1568  for (auto & dev : list)
1569  {
1571 
1572  SECTION("opening the same subdevice multiple times")
1573  {
1574  auto modes = dev.get_stream_profiles();
1575  REQUIRE(modes.size() > 0);
1576  CAPTURE(modes.front().stream_type());
1577  REQUIRE_NOTHROW(dev.open(modes.front()));
1578 
1579  SECTION("same mode")
1580  {
1581  // selected, but not streaming
1582  REQUIRE_THROWS_AS(dev.open({ modes.front() }), error);
1583 
1584  // streaming
1585  REQUIRE_NOTHROW(dev.start([](rs2::frame fref) {}));
1586  REQUIRE_THROWS_AS(dev.open({ modes.front() }), error);
1587  }
1588 
1589  SECTION("different modes")
1590  {
1591  if (modes.size() == 1)
1592  {
1593  WARN("device " << dev.get_info(RS2_CAMERA_INFO_NAME) << " S/N: " << dev.get_info(
1594  RS2_CAMERA_INFO_SERIAL_NUMBER) << " w/ FW v" << dev.get_info(
1596  WARN("subdevice has only 1 supported streaming mode. Skipping Same Subdevice, different modes test.");
1597  }
1598  else
1599  {
1600  // selected, but not streaming
1601  REQUIRE_THROWS_AS(dev.open({ modes[1] }), error);
1602 
1603  // streaming
1604  REQUIRE_NOTHROW(dev.start([](rs2::frame fref) {}));
1605  REQUIRE_THROWS_AS(dev.open({ modes[1] }), error);
1606  }
1607  }
1608 
1609  REQUIRE_NOTHROW(dev.stop());
1610  }
1611  // TODO: Move
1612  SECTION("opening different subdevices") {
1613  for (auto&& subdevice1 : ctx.get_sensor_parent(dev).query_sensors())
1614  {
1615  disable_sensitive_options_for(subdevice1);
1616  for (auto&& subdevice2 : ctx.get_sensor_parent(dev).query_sensors())
1617  {
1618  disable_sensitive_options_for(subdevice2);
1619 
1620  if (subdevice1 == subdevice2)
1621  continue;
1622 
1623  // get first lock
1624  REQUIRE_NOTHROW(subdevice1.open(subdevice1.get_stream_profiles().front()));
1625 
1626  // selected, but not streaming
1627  {
1628  auto profile = subdevice2.get_stream_profiles().front();
1629 
1630  CAPTURE(profile.stream_type());
1631  CAPTURE(profile.format());
1632  CAPTURE(profile.fps());
1633  auto vid_p = profile.as<rs2::video_stream_profile>();
1634  CAPTURE(vid_p.width());
1635  CAPTURE(vid_p.height());
1636 
1637  REQUIRE_NOTHROW(subdevice2.open(subdevice2.get_stream_profiles().front()));
1638  REQUIRE_NOTHROW(subdevice2.start([](rs2::frame fref) {}));
1639  REQUIRE_NOTHROW(subdevice2.stop());
1640  REQUIRE_NOTHROW(subdevice2.close());
1641  }
1642 
1643  // streaming
1644  {
1645  REQUIRE_NOTHROW(subdevice1.start([](rs2::frame fref) {}));
1646  REQUIRE_NOTHROW(subdevice2.open(subdevice2.get_stream_profiles().front()));
1647  REQUIRE_NOTHROW(subdevice2.start([](rs2::frame fref) {}));
1648  // stop streaming in opposite order just to be sure that works too
1649  REQUIRE_NOTHROW(subdevice1.stop());
1650  REQUIRE_NOTHROW(subdevice2.stop());
1651  REQUIRE_NOTHROW(subdevice2.close());
1652  }
1653 
1654  REQUIRE_NOTHROW(subdevice1.close());
1655  }
1656  }
1657  }
1658  }
1659  }
1660  SECTION("multiple devices")
1661  {
1662  if (list.size() == 1)
1663  {
1664  WARN("Only one device connected. Skipping multi-device test");
1665  }
1666  else
1667  {
1668  for (auto & dev1 : list)
1669  {
1671  for (auto & dev2 : list)
1672  {
1673  // couldn't think of a better way to compare the two...
1674  if (dev1 == dev2)
1675  continue;
1676 
1678 
1679  auto dev1_profiles = dev1.get_stream_profiles();
1680  auto dev2_profiles = dev2.get_stream_profiles();
1681 
1682  if (!dev1_profiles.size() || !dev2_profiles.size())
1683  continue;
1684 
1685  auto dev1_profile = dev1_profiles.front();
1686  auto dev2_profile = dev2_profiles.front();
1687 
1688  CAPTURE(dev1_profile.stream_type());
1689  CAPTURE(dev1_profile.format());
1690  CAPTURE(dev1_profile.fps());
1691  auto vid_p1 = dev1_profile.as<rs2::video_stream_profile>();
1692  CAPTURE(vid_p1.width());
1693  CAPTURE(vid_p1.height());
1694 
1695  CAPTURE(dev2_profile.stream_type());
1696  CAPTURE(dev2_profile.format());
1697  CAPTURE(dev2_profile.fps());
1698  auto vid_p2 = dev2_profile.as<rs2::video_stream_profile>();
1699  CAPTURE(vid_p2.width());
1700  CAPTURE(vid_p2.height());
1701 
1702  REQUIRE_NOTHROW(dev1.open(dev1_profile));
1703  REQUIRE_NOTHROW(dev2.open(dev2_profile));
1704 
1705  REQUIRE_NOTHROW(dev1.start([](rs2::frame fref) {}));
1706  REQUIRE_NOTHROW(dev2.start([](rs2::frame fref) {}));
1707  REQUIRE_NOTHROW(dev1.stop());
1708  REQUIRE_NOTHROW(dev2.stop());
1709 
1710  REQUIRE_NOTHROW(dev1.close());
1711  REQUIRE_NOTHROW(dev2.close());
1712  }
1713  }
1714  }
1715  }
1716  }
1717 }
1718 
1719 // On Windows 10 RS2 there is an unusual behaviour that may fail this test:
1720 // When trying to enable the second instance of Source Reader, instead of failing, the Media Foundation allows it
1721 // and sends an HR to the first Source Reader instead (something about the application being preempted)
1722 TEST_CASE("Multiple applications", "[live][multicam][!mayfail]")
1723 {
1724  rs2::context ctx1;
1726  {
1727  // Require at least one device to be plugged in
1728  std::vector<sensor> list1;
1729  REQUIRE_NOTHROW(list1 = ctx1.query_all_sensors());
1730  REQUIRE(list1.size() > 0);
1731 
1732  rs2::context ctx2;
1733  REQUIRE(make_context("two_contexts", &ctx2));
1734  std::vector<sensor> list2;
1735  REQUIRE_NOTHROW(list2 = ctx2.query_all_sensors());
1736  REQUIRE(list2.size() == list1.size());
1737  SECTION("subdevices on a single device")
1738  {
1739  for (auto&& dev1 : list1)
1740  {
1742  for (auto&& dev2 : list2)
1743  {
1745 
1746  if (dev1 == dev2)
1747  {
1748  bool skip_section = false;
1749 #ifdef _WIN32
1750  skip_section = true;
1751 #endif
1752  if (!skip_section)
1753  {
1754  SECTION("same subdevice") {
1755  // get modes
1756  std::vector<rs2::stream_profile> modes1, modes2;
1757  REQUIRE_NOTHROW(modes1 = dev1.get_stream_profiles());
1758  REQUIRE_NOTHROW(modes2 = dev2.get_stream_profiles());
1759  REQUIRE(modes1.size() > 0);
1760  REQUIRE(modes1.size() == modes2.size());
1761  // require that the lists are the same (disregarding order)
1762  for (auto profile : modes1) {
1763  REQUIRE(std::any_of(begin(modes2), end(modes2), [&profile](const rs2::stream_profile & p)
1764  {
1765  return profile == p;
1766  }));
1767  }
1768 
1769  // grab first lock
1770  CAPTURE(modes1.front().stream_name());
1771  REQUIRE_NOTHROW(dev1.open(modes1.front()));
1772 
1773  SECTION("same mode")
1774  {
1775  // selected, but not streaming
1776  REQUIRE_THROWS_AS(dev2.open({ modes1.front() }), error);
1777 
1778  // streaming
1779  REQUIRE_NOTHROW(dev1.start([](rs2::frame fref) {}));
1780  REQUIRE_THROWS_AS(dev2.open({ modes1.front() }), error);
1781  }
1782  SECTION("different modes")
1783  {
1784  if (modes1.size() == 1)
1785  {
1786  WARN("device " << dev1.get_info(RS2_CAMERA_INFO_NAME) << " S/N: " << dev1.get_info(
1787  RS2_CAMERA_INFO_SERIAL_NUMBER) << " w/ FW v" << dev1.get_info(
1789  WARN("Device has only 1 supported streaming mode. Skipping Same Subdevice, different modes test.");
1790  }
1791  else
1792  {
1793  // selected, but not streaming
1794  REQUIRE_THROWS_AS(dev2.open({ modes1[1] }), error);
1795 
1796  // streaming
1797  REQUIRE_NOTHROW(dev1.start([](rs2::frame fref) {}));
1798  REQUIRE_THROWS_AS(dev2.open({ modes1[1] }), error);
1799  }
1800  }
1801  REQUIRE_NOTHROW(dev1.stop());
1802  }
1803  }
1804  }
1805  else
1806  {
1807  SECTION("different subdevice")
1808  {
1809  // get first lock
1810  REQUIRE_NOTHROW(dev1.open(dev1.get_stream_profiles().front()));
1811 
1812  // selected, but not streaming
1813  {
1814  CAPTURE(dev2.get_stream_profiles().front().stream_type());
1815  REQUIRE_NOTHROW(dev2.open(dev2.get_stream_profiles().front()));
1816  REQUIRE_NOTHROW(dev2.start([](rs2::frame fref) {}));
1817  REQUIRE_NOTHROW(dev2.stop());
1818  REQUIRE_NOTHROW(dev2.close());
1819  }
1820 
1821  // streaming
1822  {
1823  REQUIRE_NOTHROW(dev1.start([](rs2::frame fref) {}));
1824  REQUIRE_NOTHROW(dev2.open(dev2.get_stream_profiles().front()));
1825  REQUIRE_NOTHROW(dev2.start([](rs2::frame fref) {}));
1826  // stop streaming in opposite order just to be sure that works too
1827  REQUIRE_NOTHROW(dev1.stop());
1828  REQUIRE_NOTHROW(dev2.stop());
1829 
1830  REQUIRE_NOTHROW(dev1.close());
1831  REQUIRE_NOTHROW(dev2.close());
1832  }
1833  }
1834  }
1835  }
1836  }
1837  }
1838  SECTION("subdevices on separate devices")
1839  {
1840  if (list1.size() == 1)
1841  {
1842  WARN("Only one device connected. Skipping multi-device test");
1843  }
1844  else
1845  {
1846  for (auto & dev1 : list1)
1847  {
1849  for (auto & dev2 : list2)
1850  {
1852 
1853  if (dev1 == dev2)
1854  continue;
1855 
1856  // get modes
1857  std::vector<rs2::stream_profile> modes1, modes2;
1858  REQUIRE_NOTHROW(modes1 = dev1.get_stream_profiles());
1859  REQUIRE_NOTHROW(modes2 = dev2.get_stream_profiles());
1860  REQUIRE(modes1.size() > 0);
1861  REQUIRE(modes2.size() > 0);
1862 
1863  // grab first lock
1864  CAPTURE(modes1.front().stream_type());
1865  CAPTURE(dev1.get_info(RS2_CAMERA_INFO_NAME));
1866  CAPTURE(dev1.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
1867  CAPTURE(dev2.get_info(RS2_CAMERA_INFO_NAME));
1868  CAPTURE(dev2.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
1869  REQUIRE_NOTHROW(dev1.open(modes1.front()));
1870 
1871  // try to acquire second lock
1872 
1873  // selected, but not streaming
1874  {
1875  REQUIRE_NOTHROW(dev2.open({ modes2.front() }));
1876  REQUIRE_NOTHROW(dev2.start([](rs2::frame fref) {}));
1877  REQUIRE_NOTHROW(dev2.stop());
1878  REQUIRE_NOTHROW(dev2.close());
1879  }
1880 
1881  // streaming
1882  {
1883  REQUIRE_NOTHROW(dev1.start([](rs2::frame fref) {}));
1884  REQUIRE_NOTHROW(dev2.open({ modes2.front() }));
1885  REQUIRE_NOTHROW(dev2.start([](rs2::frame fref) {}));
1886  // stop streaming in opposite order just to be sure that works too
1887  REQUIRE_NOTHROW(dev1.stop());
1888  REQUIRE_NOTHROW(dev2.stop());
1889  REQUIRE_NOTHROW(dev2.close());
1890  }
1891 
1892  REQUIRE_NOTHROW(dev1.close());
1893  }
1894  }
1895  }
1896  }
1897  }
1898 }
1899 
1900 
1901 //
1903 void metadata_verification(const std::vector<internal_frame_additional_data>& data)
1904 {
1905  // Heuristics that we use to verify metadata
1906  // Metadata sanity
1907  // Frame numbers and timestamps increase monotonically
1908  // Sensor timestamp should be less or equal to frame timestamp
1909  // Exposure time and gain values are greater than zero
1910  // Sensor framerate is bounded to >0 and < 200 fps for uvc streams
1911  int64_t last_val[3] = { -1, -1, -1 };
1912 
1913  for (size_t i = 0; i < data.size(); i++)
1914  {
1915 
1916  // Check that Frame/Sensor timetamps, frame number rise monotonically
1918  {
1919  if (data[i].frame_md.md_attributes[j].first)
1920  {
1921  int64_t value = data[i].frame_md.md_attributes[j].second;
1922  CAPTURE(value);
1923  CAPTURE(last_val[j]);
1924 
1925  REQUIRE_NOTHROW((value > last_val[0]));
1926  if (RS2_FRAME_METADATA_FRAME_COUNTER == j) // In addition, there shall be no frame number gaps
1927  {
1928  REQUIRE_NOTHROW((1 == (value - last_val[j])));
1929  }
1930 
1931  last_val[j] = data[i].frame_md.md_attributes[j].second;
1932  }
1933  }
1934 
1935  // // Exposure time and gain values are greater than zero
1936  // if (data[i].frame_md.md_attributes[RS2_FRAME_METADATA_ACTUAL_EXPOSURE].first)
1937  // REQUIRE(data[i].frame_md.md_attributes[RS2_FRAME_METADATA_ACTUAL_EXPOSURE].second > 0);
1938  // if (data[i].frame_md.md_attributes[RS2_FRAME_METADATA_GAIN_LEVEL].first)
1939  // REQUIRE(data[i].frame_md.md_attributes[RS2_FRAME_METADATA_GAIN_LEVEL].second > 0);
1940  }
1941 }
1942 
1943 
1945 void trigger_error(const rs2::device& dev, int num)
1946 {
1947  std::vector<uint8_t> raw_data(24, 0);
1948  raw_data[0] = 0x14;
1949  raw_data[2] = 0xab;
1950  raw_data[3] = 0xcd;
1951  raw_data[4] = 0x4d;
1952  raw_data[8] = num;
1953  if (auto debug = dev.as<debug_protocol>())
1954  debug.send_and_receive_raw_data(raw_data);
1955 }
1956 
1957 
1958 
1959 TEST_CASE("Error handling sanity", "[live][!mayfail]") {
1960 
1961  //Require at least one device to be plugged in
1962  rs2::context ctx;
1964  {
1965  std::vector<sensor> list;
1966  REQUIRE_NOTHROW(list = ctx.query_all_sensors());
1967  REQUIRE(list.size() > 0);
1968 
1969  std::string notification_description;
1971  std::condition_variable cv;
1972  std::mutex m;
1973 
1974  // An exempt of possible error values - note that the full list is available to Librealsenes core
1975  const std::map< uint8_t, std::string> error_report = {
1976  { 0, "Success" },
1977  { 1, "Laser hot - power reduce" },
1978  { 2, "Laser hot - disabled" },
1979  { 3, "Flag B - laser disabled" },
1980  };
1981 
1982  //enable error polling
1983  for (auto && subdevice : list) {
1984 
1985  if (subdevice.supports(RS2_OPTION_ERROR_POLLING_ENABLED))
1986  {
1987  disable_sensitive_options_for(subdevice);
1988 
1989  subdevice.set_notifications_callback([&](rs2::notification n)
1990  {
1991  std::unique_lock<std::mutex> lock(m);
1992 
1993  notification_description = n.get_description();
1994  severity = n.get_severity();
1995  cv.notify_one();
1996  });
1997 
1998  REQUIRE_NOTHROW(subdevice.set_option(RS2_OPTION_ERROR_POLLING_ENABLED, 1));
1999 
2000  // The first entry with success value cannot be emulated
2001  for (auto i = 1; i < error_report.size(); i++)
2002  {
2003  trigger_error(ctx.get_sensor_parent(subdevice), i);
2004  std::unique_lock<std::mutex> lock(m);
2005  CAPTURE(i);
2006  CAPTURE(error_report.at(i));
2007  CAPTURE(severity);
2008  auto pred = [&]() {
2009  return notification_description.compare(error_report.at(i)) == 0
2010  && severity == RS2_LOG_SEVERITY_ERROR;
2011  };
2012  REQUIRE(cv.wait_for(lock, std::chrono::seconds(10), pred));
2013 
2014  }
2015  REQUIRE_NOTHROW(subdevice.set_option(RS2_OPTION_ERROR_POLLING_ENABLED, 0));
2016  }
2017  }
2018  }
2019 }
2020 
2021 TEST_CASE("Auto disabling control behavior", "[live]") {
2022  //Require at least one device to be plugged in
2023  rs2::context ctx;
2025  {
2026  std::vector<sensor> list;
2027  REQUIRE_NOTHROW(list = ctx.query_all_sensors());
2028  REQUIRE(list.size() > 0);
2029 
2030  for (auto && subdevice : list)
2031  {
2032  disable_sensitive_options_for(subdevice);
2033  auto info = subdevice.get_info(RS2_CAMERA_INFO_NAME);
2034  CAPTURE(info);
2035 
2037  float val{};
2038  if (subdevice.supports(RS2_OPTION_ENABLE_AUTO_EXPOSURE))
2039  {
2040  SECTION("Disable auto exposure when setting a value")
2041  {
2042  REQUIRE_NOTHROW(subdevice.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, 1));
2043  REQUIRE_NOTHROW(range = subdevice.get_option_range(RS2_OPTION_EXPOSURE));
2044  REQUIRE_NOTHROW(subdevice.set_option(RS2_OPTION_EXPOSURE, range.max));
2045  CAPTURE(range.max);
2046  REQUIRE_NOTHROW(val = subdevice.get_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE));
2047  REQUIRE(val == 0);
2048  }
2049  }
2050 
2051  if (subdevice.supports(RS2_OPTION_EMITTER_ENABLED))
2052  {
2053  SECTION("Disable emitter when setting a value")
2054  {
2055  for (auto elem : { 0.f, 2.f })
2056  {
2057  REQUIRE_NOTHROW(subdevice.set_option(RS2_OPTION_EMITTER_ENABLED, elem));
2058  REQUIRE_NOTHROW(range = subdevice.get_option_range(RS2_OPTION_LASER_POWER));
2059  REQUIRE_NOTHROW(subdevice.set_option(RS2_OPTION_LASER_POWER, range.max));
2060  CAPTURE(range.max);
2061  REQUIRE_NOTHROW(val = subdevice.get_option(RS2_OPTION_EMITTER_ENABLED));
2062  REQUIRE(val == 1);
2063  }
2064  }
2065  }
2066 
2067  if (subdevice.supports(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE)) // TODO: Add auto-disabling to SR300 options
2068  {
2069  SECTION("Disable white balance when setting a value")
2070  {
2071  if (subdevice.supports(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE) && subdevice.supports(RS2_OPTION_WHITE_BALANCE))
2072  {
2073 
2074  REQUIRE_NOTHROW(subdevice.set_option(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, 1));
2075  REQUIRE_NOTHROW(range = subdevice.get_option_range(RS2_OPTION_WHITE_BALANCE));
2076  REQUIRE_NOTHROW(subdevice.set_option(RS2_OPTION_WHITE_BALANCE, range.max));
2077  CAPTURE(range.max);
2078  REQUIRE_NOTHROW(val = subdevice.get_option(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE));
2079  REQUIRE(val == 0);
2080  }
2081  }
2082  }
2083  }
2084  }
2085 }
2086 
2087 
2088 std::pair<std::shared_ptr<rs2::device>, std::weak_ptr<rs2::device>> make_device(device_list& list)
2089 {
2090  REQUIRE(list.size() > 0);
2091 
2092  std::shared_ptr<rs2::device> dev;
2093  REQUIRE_NOTHROW(dev = std::make_shared<rs2::device>(list[0]));
2094  std::weak_ptr<rs2::device> weak_dev(dev);
2095 
2097 
2098  return std::pair<std::shared_ptr<rs2::device>, std::weak_ptr<rs2::device>>(dev, weak_dev);
2099 }
2100 
2101 void reset_device(std::shared_ptr<rs2::device>& strong, std::weak_ptr<rs2::device>& weak, device_list& list, const rs2::device& new_dev)
2102 {
2103  strong.reset();
2104  weak.reset();
2105  list = nullptr;
2106  strong = std::make_shared<rs2::device>(new_dev);
2107  weak = strong;
2109 }
2110 
2111 TEST_CASE("Disconnect events works", "[live]") {
2112 
2113  rs2::context ctx;
2115  {
2116  device_list list;
2117  REQUIRE_NOTHROW(list = ctx.query_devices());
2118 
2119  auto dev = make_device(list);
2120  auto dev_strong = dev.first;
2121  auto dev_weak = dev.second;
2122 
2123 
2124  auto disconnected = false;
2125  auto connected = false;
2126 
2127  std::string serial;
2128 
2129  REQUIRE_NOTHROW(serial = dev_strong->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
2130 
2131  std::condition_variable cv;
2132  std::mutex m;
2133 
2134  //Setting up devices change callback to notify the test about device disconnection
2136  {
2137  auto&& strong = dev_weak.lock();
2138  {
2139  if (strong)
2140  {
2141  if (info.was_removed(*strong))
2142  {
2143  std::unique_lock<std::mutex> lock(m);
2144  disconnected = true;
2145  cv.notify_one();
2146  }
2147 
2148 
2149  for (auto d : info.get_new_devices())
2150  {
2151  for (auto&& s : d.query_sensors())
2152  disable_sensitive_options_for(s);
2153 
2154  if (serial == d.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER))
2155  {
2156  try
2157  {
2158  std::unique_lock<std::mutex> lock(m);
2159  connected = true;
2160  cv.notify_one();
2161  break;
2162  }
2163  catch (...)
2164  {
2165 
2166  }
2167  }
2168  }
2169  }
2170 
2171  }}));
2172 
2173  //forcing hardware reset to simulate device disconnection
2174  do_with_waiting_for_camera_connection(ctx, dev_strong, serial, [&]()
2175  {
2176  dev_strong->hardware_reset();
2177  });
2178 
2179  //Check that after the library reported device disconnection, operations on old device object will return error
2180  REQUIRE_THROWS(dev_strong->query_sensors().front().close());
2181  }
2182 }
2183 
2184 TEST_CASE("Connect events works", "[live]") {
2185 
2186 
2187  rs2::context ctx;
2189  {
2190  device_list list;
2191  REQUIRE_NOTHROW(list = ctx.query_devices());
2192 
2193  auto dev = make_device(list);
2194  auto dev_strong = dev.first;
2195  auto dev_weak = dev.second;
2196 
2197  std::string serial;
2198 
2199  REQUIRE_NOTHROW(serial = dev_strong->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
2200 
2201  auto disconnected = false;
2202  auto connected = false;
2203  std::condition_variable cv;
2204  std::mutex m;
2205 
2206  //Setting up devices change callback to notify the test about device disconnection and connection
2208  {
2209  auto&& strong = dev_weak.lock();
2210  {
2211  if (strong)
2212  {
2213  if (info.was_removed(*strong))
2214  {
2215  std::unique_lock<std::mutex> lock(m);
2216  disconnected = true;
2217  cv.notify_one();
2218  }
2219 
2220 
2221  for (auto d : info.get_new_devices())
2222  {
2223  if (serial == d.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER))
2224  {
2225  try
2226  {
2227  std::unique_lock<std::mutex> lock(m);
2228 
2229  reset_device(dev_strong, dev_weak, list, d);
2230 
2231  connected = true;
2232  cv.notify_one();
2233  break;
2234  }
2235  catch (...)
2236  {
2237 
2238  }
2239  }
2240  }
2241  }
2242 
2243  }}));
2244 
2245  //forcing hardware reset to simulate device disconnection
2246  do_with_waiting_for_camera_connection(ctx, dev_strong, serial, [&]()
2247  {
2248  dev_strong->hardware_reset();
2249  });
2250  }
2251 }
2252 
2253 std::shared_ptr<std::function<void(rs2::frame fref)>> check_stream_sanity(const rs2::context& ctx, const rs2::sensor& sub, int num_of_frames, bool infinite = false)
2254 {
2255  std::shared_ptr<std::condition_variable> cv = std::make_shared<std::condition_variable>();
2256  std::shared_ptr<std::mutex> m = std::make_shared<std::mutex>();
2257  std::shared_ptr<std::map<rs2_stream, int>> streams_frames = std::make_shared<std::map<rs2_stream, int>>();
2258 
2259  std::shared_ptr<std::function<void(rs2::frame fref)>> func;
2260 
2261  std::vector<rs2::stream_profile> modes;
2262  REQUIRE_NOTHROW(modes = sub.get_stream_profiles());
2263 
2264  auto streaming = false;
2265 
2266  for (auto p : modes)
2267  {
2268  if (auto video = p.as<video_stream_profile>())
2269  {
2270  if (video.width() == 640 && video.height() == 480 && video.fps() == 60 && video.format())
2271  {
2272  if ((video.stream_type() == RS2_STREAM_DEPTH && video.format() == RS2_FORMAT_Z16) ||
2273  (video.stream_type() == RS2_STREAM_FISHEYE && video.format() == RS2_FORMAT_RAW8))
2274  {
2275  streaming = true;
2276  (*streams_frames)[p.stream_type()] = 0;
2277 
2278  REQUIRE_NOTHROW(sub.open(p));
2279 
2280  func = std::make_shared< std::function<void(rs2::frame fref)>>([num_of_frames, m, streams_frames, cv](rs2::frame fref) mutable
2281  {
2282  std::unique_lock<std::mutex> lock(*m);
2283  auto stream = fref.get_profile().stream_type();
2284  streams_frames->at(stream)++;
2285  if (streams_frames->at(stream) >= num_of_frames)
2286  cv->notify_one();
2287 
2288  });
2289  REQUIRE_NOTHROW(sub.start(*func));
2290  }
2291  }
2292  }
2293  }
2294 
2295 
2296  std::unique_lock<std::mutex> lock(*m);
2297  cv->wait_for(lock, std::chrono::seconds(30), [&]
2298  {
2299  for (auto f : (*streams_frames))
2300  {
2301  if (f.second < num_of_frames)
2302  return false;
2303  }
2304  return true;
2305  });
2306 
2307  if (!infinite && streaming)
2308  {
2309  REQUIRE_NOTHROW(sub.stop());
2310  REQUIRE_NOTHROW(sub.close());
2311 
2312  }
2313 
2314  return func;
2315 }
2316 
2317 TEST_CASE("Connect Disconnect events while streaming", "[live]") {
2318  rs2::context ctx;
2320  {
2321  device_list list;
2322  REQUIRE_NOTHROW(list = ctx.query_devices());
2323 
2324  std::string serial;
2325 
2326  auto dev = make_device(list);
2327  auto dev_strong = dev.first;
2328  auto dev_weak = dev.second;
2329 
2330 
2331  REQUIRE_NOTHROW(serial = dev_strong->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
2332 
2333 
2334  auto disconnected = false;
2335  auto connected = false;
2336  std::condition_variable cv;
2337  std::mutex m;
2338 
2339  //Setting up devices change callback to notify the test about device disconnection and connection
2341  {
2342  auto&& strong = dev_weak.lock();
2343  {
2344  if (strong)
2345  {
2346  if (info.was_removed(*strong))
2347  {
2348  std::unique_lock<std::mutex> lock(m);
2349  disconnected = true;
2350  cv.notify_one();
2351  }
2352 
2353 
2354  for (auto d : info.get_new_devices())
2355  {
2356  if (serial == d.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER))
2357  {
2358  try
2359  {
2360  std::unique_lock<std::mutex> lock(m);
2361 
2362  reset_device(dev_strong, dev_weak, list, d);
2363 
2364  connected = true;
2365  cv.notify_one();
2366  break;
2367  }
2368  catch (...)
2369  {
2370 
2371  }
2372  }
2373  }
2374  }
2375 
2376  }}));
2377 
2378  for (auto&& s : dev_strong->query_sensors())
2379  auto func = check_stream_sanity(ctx, s, 1, true);
2380 
2381  for (auto i = 0; i < 3; i++)
2382  {
2383  //forcing hardware reset to simulate device disconnection
2384  dev_strong = do_with_waiting_for_camera_connection(ctx, dev_strong, serial, [&]()
2385  {
2386  dev_strong->hardware_reset();
2387  });
2388 
2389  for (auto&& s : dev_strong->query_sensors())
2390  auto func = check_stream_sanity(ctx, s, 10);
2391 
2392  disconnected = connected = false;
2393  }
2394 
2395  }
2396 }
2397 
2399 {
2400  for (auto d : ctx.get_sensor_parent(dev).query_sensors())
2401  {
2402  for (auto i = 0; i < RS2_OPTION_COUNT; i++)
2403  {
2404  if (d.supports((rs2_option)i))
2405  REQUIRE_NOTHROW(d.get_option((rs2_option)i));
2406  }
2407  }
2408 }
2409 //
2410 TEST_CASE("Connect Disconnect events while controls", "[live]")
2411 {
2412  rs2::context ctx;
2414  {
2415  device_list list;
2416  REQUIRE_NOTHROW(list = ctx.query_devices());
2417 
2418  auto dev = make_device(list);
2419  auto dev_strong = dev.first;
2420  auto dev_weak = dev.second;
2421 
2422  std::string serial;
2423 
2424  REQUIRE_NOTHROW(serial = dev_strong->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
2425 
2426 
2427  auto disconnected = false;
2428  auto connected = false;
2429  std::condition_variable cv;
2430  std::mutex m;
2431 
2432  //Setting up devices change callback to notify the test about device disconnection and connection
2434  {
2435  auto&& strong = dev_weak.lock();
2436  {
2437  if (strong)
2438  {
2439  if (info.was_removed(*strong))
2440  {
2441  std::unique_lock<std::mutex> lock(m);
2442  disconnected = true;
2443  cv.notify_one();
2444  }
2445 
2446  for (auto d : info.get_new_devices())
2447  {
2448  if (serial == d.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER))
2449  {
2450  try
2451  {
2452  std::unique_lock<std::mutex> lock(m);
2453 
2454  reset_device(dev_strong, dev_weak, list, d);
2455  connected = true;
2456  cv.notify_one();
2457  break;
2458  }
2459  catch (...)
2460  {
2461 
2462  }
2463  }
2464  }
2465  }
2466 
2467  }}));
2468 
2469  //forcing hardware reset to simulate device disconnection
2470  dev_strong = do_with_waiting_for_camera_connection(ctx, dev_strong, serial, [&]()
2471  {
2472  dev_strong->hardware_reset();
2473  });
2474 
2475  for (auto&& s : dev_strong->query_sensors())
2476  check_controls_sanity(ctx, s);
2477  }
2478 
2479 }
2480 
2481 TEST_CASE("Basic device_hub flow", "[live][!mayfail]") {
2482 
2483  rs2::context ctx;
2484 
2485  std::shared_ptr<rs2::device> dev;
2486 
2488  {
2489  device_hub hub(ctx);
2490  REQUIRE_NOTHROW(dev = std::make_shared<rs2::device>(hub.wait_for_device()));
2491 
2492  std::weak_ptr<rs2::device> weak(dev);
2493 
2495 
2496  dev->hardware_reset();
2497  int i = 300;
2498  while (i > 0 && hub.is_connected(*dev))
2499  {
2500  std::this_thread::sleep_for(std::chrono::milliseconds(10));
2501  --i;
2502  }
2503  /*if (i == 0)
2504  {
2505  WARN("Reset workaround");
2506  dev->hardware_reset();
2507  while (hub.is_connected(*dev))
2508  std::this_thread::sleep_for(std::chrono::milliseconds(10));
2509  }*/
2510 
2511  // Don't exit the test in unknown state
2513  }
2514 }
2515 
2516 
2518 {
2520  int width;
2521  int height;
2522  int fps;
2524  int index;
2525 };
2526 
2527 TEST_CASE("Auto-complete feature works", "[offline][util::config]") {
2528  // dummy device can provide the following profiles:
2529  rs2::context ctx;
2530 
2532  {
2533  struct Test {
2534  std::vector<stream_format> given; // We give these profiles to the config class
2535  std::vector<stream_format> expected; // pool of profiles the config class can return. Leave empty if auto-completer is expected to fail
2536  };
2537  std::vector<Test> tests = {
2538  // Test 0 (Depth always has RS2_FORMAT_Z16)
2539  { { { RS2_STREAM_DEPTH , 0, 0, 0, RS2_FORMAT_ANY, 0 } }, // given
2540  { { RS2_STREAM_DEPTH , 0, 0, 0, RS2_FORMAT_Z16, 0 } } }, // expected
2541  // Test 1 (IR always has RS2_FORMAT_Y8)
2542  { { { RS2_STREAM_INFRARED, 0, 0, 0, RS2_FORMAT_ANY, 1 } }, // given
2543  { { RS2_STREAM_INFRARED, 0, 0, 0, RS2_FORMAT_Y8 , 1 } } }, // expected
2544  // Test 2 (No 200 fps depth)
2545  { { { RS2_STREAM_DEPTH , 0, 0, 200, RS2_FORMAT_ANY, -1 } }, // given
2546  { } }, // expected
2547  // Test 3 (Can request 60 fps IR)
2548  { { { RS2_STREAM_INFRARED, 0, 0, 60, RS2_FORMAT_ANY, 1 } }, // given
2549  { { RS2_STREAM_INFRARED, 0, 0, 60, RS2_FORMAT_ANY, 1 } } }, // expected
2550  // Test 4 (requesting IR@200fps + depth fails
2551  { { { RS2_STREAM_INFRARED, 0, 0, 200, RS2_FORMAT_ANY, 1 }, { RS2_STREAM_DEPTH , 0, 0, 0, RS2_FORMAT_ANY, -1 } }, // given
2552  { } }, // expected
2553  // Test 5 (Can't do 640x480@110fps a)
2554  { { { RS2_STREAM_INFRARED, 640, 480, 110, RS2_FORMAT_ANY, -1 } }, // given
2555  { } }, // expected
2556  // Test 6 (Can't do 640x480@110fps b)
2557  { { { RS2_STREAM_DEPTH , 640, 480, 0, RS2_FORMAT_ANY, -1 }, { RS2_STREAM_INFRARED, 0, 0, 110, RS2_FORMAT_ANY, 1 } }, // given
2558  { } }, // expected
2559  // Test 7 (Pull extra details from second stream a)
2560  { { { RS2_STREAM_DEPTH , 640, 480, 0, RS2_FORMAT_ANY, -1 }, { RS2_STREAM_INFRARED, 0, 0, 30, RS2_FORMAT_ANY, 1 } }, // given
2561  { { RS2_STREAM_DEPTH , 640, 480, 30, RS2_FORMAT_ANY, 0 }, { RS2_STREAM_INFRARED, 640, 480, 30, RS2_FORMAT_ANY, 1 } } }, // expected
2562  // Test 8 (Pull extra details from second stream b) [IR also supports 200, could fail if that gets selected]
2563  { { { RS2_STREAM_INFRARED, 640, 480, 0, RS2_FORMAT_ANY, 1 }, { RS2_STREAM_DEPTH , 0, 0, 0, RS2_FORMAT_ANY , 0 } }, // given
2564  { { RS2_STREAM_INFRARED, 640, 480, 10, RS2_FORMAT_ANY, 1 }, { RS2_STREAM_INFRARED, 640, 480, 30, RS2_FORMAT_ANY , 1 }, // expected - options for IR stream
2565  { RS2_STREAM_DEPTH , 640, 480, 10, RS2_FORMAT_ANY, 0 }, { RS2_STREAM_DEPTH , 640, 480, 30, RS2_FORMAT_ANY , 0 } } } // expected - options for depth stream
2566 
2567  };
2568 
2569  pipeline pipe(ctx);
2570  rs2::config cfg;
2571  for (int i = 0; i < tests.size(); ++i)
2572  {
2573  cfg.disable_all_streams();
2574  for (auto & profile : tests[i].given) {
2576  }
2577 
2578  CAPTURE(i);
2579  if (tests[i].expected.size() == 0) {
2580  REQUIRE_THROWS_AS(pipe.start(cfg), std::runtime_error);
2581  }
2582  else
2583  {
2584  rs2::pipeline_profile pipe_profile;
2585  REQUIRE_NOTHROW(pipe_profile = pipe.start(cfg));
2586  //REQUIRE()s are in here
2587  REQUIRE(pipe_profile);
2588  REQUIRE_NOTHROW(pipe.stop());
2589  }
2590  }
2591  }
2592 }
2593 
2594 
2595 //TODO: make it work
2596 //TEST_CASE("Sync connect disconnect", "[live]") {
2597 // rs2::context ctx;
2598 //
2599 // if (make_context(SECTION_FROM_TEST_NAME, &ctx))
2600 // {
2601 // auto list = ctx.query_devices();
2602 // REQUIRE(list.size());
2603 // pipeline pipe(ctx);
2604 // auto dev = pipe.get_device();
2605 //
2606 // disable_sensitive_options_for(dev);
2607 //
2608 //
2609 // auto profiles = configure_all_supported_streams(dev, dev);
2610 //
2611 //
2612 // pipe.start();
2613 //
2614 // std::string serial;
2615 // REQUIRE_NOTHROW(serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
2616 //
2617 // auto disconnected = false;
2618 // auto connected = false;
2619 // std::condition_variable cv;
2620 // std::mutex m;
2621 //
2622 // //Setting up devices change callback to notify the test about device disconnection and connection
2623 // REQUIRE_NOTHROW(ctx.set_devices_changed_callback([&](event_information& info) mutable
2624 // {
2625 // std::unique_lock<std::mutex> lock(m);
2626 // if (info.was_removed(dev))
2627 // {
2628 //
2629 // try {
2630 // pipe.stop();
2631 // pipe.disable_all();
2632 // dev = nullptr;
2633 // }
2634 // catch (...) {};
2635 // disconnected = true;
2636 // cv.notify_one();
2637 // }
2638 //
2639 // auto devs = info.get_new_devices();
2640 // if (devs.size() > 0)
2641 // {
2642 // dev = pipe.get_device();
2643 // std::string new_serial;
2644 // REQUIRE_NOTHROW(new_serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
2645 // if (serial == new_serial)
2646 // {
2647 // disable_sensitive_options_for(dev);
2648 //
2649 // auto profiles = configure_all_supported_streams(dev, pipe);
2650 //
2651 // pipe.start();
2652 //
2653 // connected = true;
2654 // cv.notify_one();
2655 // }
2656 // }
2657 //
2658 // }));
2659 //
2660 //
2661 // for (auto i = 0; i < 5; i++)
2662 // {
2663 // auto frames = pipe.wait_for_frames(10000);
2664 // REQUIRE(frames.size() > 0);
2665 // }
2666 //
2667 // {
2668 // std::unique_lock<std::mutex> lock(m);
2669 // disconnected = connected = false;
2670 // auto shared_dev = std::make_shared<rs2::device>(dev);
2671 // dev.hardware_reset();
2672 //
2673 // REQUIRE(wait_for_reset([&]() {
2674 // return cv.wait_for(lock, std::chrono::seconds(20), [&]() {return disconnected; });
2675 // }, shared_dev));
2676 // REQUIRE(cv.wait_for(lock, std::chrono::seconds(20), [&]() {return connected; }));
2677 // }
2678 //
2679 // for (auto i = 0; i < 5; i++)
2680 // {
2681 // auto frames = pipe.wait_for_frames(10000);
2682 // REQUIRE(frames.size() > 0);
2683 //
2684 // }
2685 // }
2686 //}
2687 
2688 
2689 void validate(std::vector<std::vector<rs2::stream_profile>> frames, std::vector<std::vector<double>> timestamps, device_profiles requests, int actual_fps)
2690 {
2691  REQUIRE(frames.size() > 0);
2692 
2693  int successful = 0;
2694 
2695  auto gap = (float)1000 / (float)actual_fps;
2696 
2697  auto ts = 0;
2698 
2699  std::vector<test_profile> actual_streams_arrived;
2700  for (auto i = 0; i < frames.size(); i++)
2701  {
2702  auto frame = frames[i];
2703  auto ts = timestamps[i];
2704  if (frame.size() == 0)
2705  {
2706  CAPTURE(frame.size());
2707  continue;
2708  }
2709 
2710  std::vector<test_profile> stream_arrived;
2711 
2712  for (auto f : frame)
2713  {
2714  auto image = f.as<rs2::video_stream_profile>();
2715 
2716  stream_arrived.push_back({ image.stream_type(), image.format(), image.width(), image.height() });
2717  REQUIRE(image.fps());
2718  }
2719 
2720  std::sort(ts.begin(), ts.end());
2721 
2722  if (ts[ts.size() - 1] - ts[0] > (float)gap / (float)2)
2723  {
2724  CAPTURE(gap);
2725  CAPTURE((float)gap / (float)2);
2726  CAPTURE(ts[ts.size() - 1]);
2727  CAPTURE(ts[0]);
2728  CAPTURE(ts[ts.size() - 1] - ts[0]);
2729  continue;
2730  }
2731 
2732  for (auto& str : stream_arrived)
2733  actual_streams_arrived.push_back(str);
2734 
2735  if (stream_arrived.size() != requests.streams.size())
2736  continue;
2737 
2738  std::sort(stream_arrived.begin(), stream_arrived.end());
2739  std::sort(requests.streams.begin(), requests.streams.end());
2740 
2741  auto equals = true;
2742  for (auto i = 0; i < requests.streams.size(); i++)
2743  {
2744  if (stream_arrived[i] != requests.streams[i])
2745  {
2746  equals = false;
2747  break;
2748  }
2749 
2750  }
2751  if (!equals)
2752  continue;
2753 
2754  successful++;
2755 
2756  }
2757 
2758  std::stringstream ss;
2759  ss << "Requested profiles : " << std::endl;
2760  for (auto prof : requests.streams)
2761  {
2762  //ss << STRINGIFY(prof.stream) << " = " << prof.stream << std::endl;
2763  //ss << STRINGIFY(prof.format) << " = " << prof.format << std::endl;
2764  ss << STRINGIFY(prof.width) << " = " << prof.width << std::endl;
2765  ss << STRINGIFY(prof.height) << " = " << prof.height << std::endl;
2766  ss << STRINGIFY(prof.index) << " = " << prof.index << std::endl;
2767  }
2768  CAPTURE(ss.str());
2769  CAPTURE(requests.fps);
2770  CAPTURE(requests.sync);
2771 
2772  ss.str("");
2773  ss << "\n\nReceived profiles : " << std::endl;
2774  std::sort(actual_streams_arrived.begin(), actual_streams_arrived.end());
2775  auto last = std::unique(actual_streams_arrived.begin(), actual_streams_arrived.end());
2776  actual_streams_arrived.erase(last, actual_streams_arrived.end());
2777 
2778  for (auto prof : actual_streams_arrived)
2779  {
2780  ss << STRINGIFY(prof.stream) << " = " << int(prof.stream) << std::endl;
2781  ss << STRINGIFY(prof.format) << " = " << int(prof.format) << std::endl;
2782  ss << STRINGIFY(prof.width) << " = " << prof.width << std::endl;
2783  ss << STRINGIFY(prof.height) << " = " << prof.height << std::endl;
2784  ss << STRINGIFY(prof.index) << " = " << prof.index << std::endl;
2785  }
2786  CAPTURE(ss.str());
2787 
2788  REQUIRE(successful > 0);
2789 }
2790 
2791 static const std::map< dev_type, device_profiles> pipeline_default_configurations = {
2792 /* RS400/PSR*/ { { "0AD1", true} ,{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 1280, 720, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 1280, 720, 0 } }, 30, true}},
2793 /* RS410/ASR*/ { { "0AD2", true } ,{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 1280, 720, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 1280, 720, 0 } }, 30, true }},
2794 /* D410/USB2*/ { { "0AD2", false },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 15, true } },
2795 /* RS415/ASRC*/ { { "0AD3", true } ,{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 1280, 720, 0 },{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true }},
2796 /* D415/USB2*/ { { "0AD3", false },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 640, 480, 0 } }, 15, true } },
2797 /* RS430/AWG*/ { { "0AD4", true } ,{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 1280, 720, 0 } }, 30, true }},
2798 /* RS430_MM/AWGT*/ { { "0AD5", true } ,{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 1280, 720, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 1280, 720, 0 } }, 30, true }},
2799 /* RS420/PWG*/ { { "0AF6", true } ,{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 1280, 720, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 1280, 720, 0 } }, 30, true }},
2800 /* RS420_MM/PWGT*/ { { "0AFE", true } ,{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 1280, 720, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 1280, 720, 0 } }, 30, true }},
2801 /* RS410_MM/ASRT*/ { { "0AFF", true } ,{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 1280, 720, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 1280, 720, 0 } }, 30, true } },
2802 /* RS400_MM/PSR*/ { { "0B00", true } ,{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 1280, 720, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 1280, 720, 0 } }, 30, true } },
2803 /* RS430_MM_RGB/AWGTC*/ { { "0B01", true } ,{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 1280, 720, 0 },{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 1280, 720, 0 } }, 30, true }},
2804 /* RS405/DS5U*/ { { "0B03", true } ,{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 1280, 720, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 1280, 720, 0 } }, 30, true }},
2805 /* RS435_RGB/AWGC*/ { { "0B07", true } ,{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 1280, 720, 0 },{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true }},
2806 /* D435/USB2*/ { { "0B07", false },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 640, 480, 0 } }, 15, true } },
2807 /* SR300*/ { { "0AA5", true } ,{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 1920, 1080, 0 } }, 30, true } },
2808 };
2809 
2810 TEST_CASE("Pipeline wait_for_frames", "[live]") {
2811 
2812  rs2::context ctx;
2813 
2815  {
2816  rs2::device dev;
2817  rs2::pipeline pipe(ctx);
2818  rs2::config cfg;
2820  REQUIRE_NOTHROW(profile = cfg.resolve(pipe));
2821  REQUIRE(profile);
2822  REQUIRE_NOTHROW(dev = profile.get_device());
2823  REQUIRE(dev);
2825  dev_type PID = get_PID(dev);
2826  CAPTURE(PID.first);
2827  CAPTURE(PID.second);
2828 
2830  {
2831  WARN("Skipping test - the Device-Under-Test profile is not defined for PID " << PID.first << (PID.second ? " USB3" : " USB2"));
2832  }
2833  else
2834  {
2835  REQUIRE(pipeline_default_configurations.at(PID).streams.size() > 0);
2836 
2837  REQUIRE_NOTHROW(pipe.start(cfg));
2838 
2839  std::vector<std::vector<rs2::stream_profile>> frames;
2840  std::vector<std::vector<double>> timestamps;
2841 
2842  for (auto i = 0; i < 30; i++)
2843  REQUIRE_NOTHROW(pipe.wait_for_frames(10000));
2844 
2845  auto actual_fps = pipeline_default_configurations.at(PID).fps;
2846 
2847  while (frames.size() < 100)
2848  {
2849  frameset frame;
2850  REQUIRE_NOTHROW(frame = pipe.wait_for_frames(10000));
2851  std::vector<rs2::stream_profile> frames_set;
2852  std::vector<double> ts;
2853 
2854  for (auto f : frame)
2855  {
2856  if (f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS))
2857  {
2858  auto val = static_cast<int>(f.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS));
2859  if (val < actual_fps)
2860  actual_fps = val;
2861  }
2862  frames_set.push_back(f.get_profile());
2863  ts.push_back(f.get_timestamp());
2864  }
2865  frames.push_back(frames_set);
2866  timestamps.push_back(ts);
2867  }
2868 
2869 
2870  REQUIRE_NOTHROW(pipe.stop());
2871  validate(frames, timestamps, pipeline_default_configurations.at(PID), actual_fps);
2872  }
2873  }
2874 }
2875 
2876 TEST_CASE("Pipeline poll_for_frames", "[live]")
2877 {
2878  rs2::context ctx;
2879 
2881  {
2882  auto list = ctx.query_devices();
2883  REQUIRE(list.size());
2884  rs2::device dev;
2885  rs2::pipeline pipe(ctx);
2886  rs2::config cfg;
2888  REQUIRE_NOTHROW(profile = cfg.resolve(pipe));
2889  REQUIRE(profile);
2890  REQUIRE_NOTHROW(dev = profile.get_device());
2891  REQUIRE(dev);
2893  dev_type PID = get_PID(dev);
2894  CAPTURE(PID.first);
2895  CAPTURE(PID.second);
2896 
2898  {
2899  WARN("Skipping test - the Device-Under-Test profile is not defined for PID " << PID.first << (PID.second ? " USB3" : " USB2"));
2900  }
2901  else
2902  {
2903  REQUIRE(pipeline_default_configurations.at(PID).streams.size() > 0);
2904 
2905  REQUIRE_NOTHROW(pipe.start(cfg));
2906 
2907  std::vector<std::vector<rs2::stream_profile>> frames;
2908  std::vector<std::vector<double>> timestamps;
2909 
2910  for (auto i = 0; i < 30; i++)
2911  REQUIRE_NOTHROW(pipe.wait_for_frames(5000));
2912 
2913  auto actual_fps = pipeline_default_configurations.at(PID).fps;
2914  while (frames.size() < 100)
2915  {
2916  frameset frame;
2917  if (pipe.poll_for_frames(&frame))
2918  {
2919  std::vector<rs2::stream_profile> frames_set;
2920  std::vector<double> ts;
2921  for (auto f : frame)
2922  {
2923  if (f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS))
2924  {
2925  auto val = static_cast<int>(f.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS));
2926  if (val < actual_fps)
2927  actual_fps = val;
2928  }
2929  frames_set.push_back(f.get_profile());
2930  ts.push_back(f.get_timestamp());
2931  }
2932  frames.push_back(frames_set);
2933  timestamps.push_back(ts);
2934  }
2935  }
2936 
2937  REQUIRE_NOTHROW(pipe.stop());
2938  validate(frames, timestamps, pipeline_default_configurations.at(PID), actual_fps);
2939  }
2940 
2941  }
2942 }
2943 
2944 static const std::map<dev_type, device_profiles> pipeline_custom_configurations = {
2945  /* RS400/PSR*/ { {"0AD1", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
2946  /* RS410/ASR*/ { {"0AD2", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
2947  /* D410/USB2*/ { {"0AD2", false },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 480, 270, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 480, 270, 0 } }, 30, true } },
2948  /* RS415/ASRC*/ { {"0AD3", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 1280, 720, 0 } }, 30, true } },
2949  /* D415/USB2*/ { {"0AD3", false },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 480, 270, 0 },{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 424, 240, 0 } }, 30, true } },
2950  /* RS430/AWG*/ { {"0AD4", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_Y8, 640, 480, 1 } }, 30, true } },
2951  /* RS430_MM/AWGT*/ { {"0AD5", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
2952  /* RS420/PWG*/ { {"0AF6", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
2953  /* RS420_MM/PWGT*/ { {"0AFE", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
2954  /* RS410_MM/ASRT*/ { {"0AFF", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
2955  /* RS400_MM/PSR*/ { {"0B00", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
2956  /* RS430_MC/AWGTC*/ { {"0B01", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 1280, 720, 0 } }, 30, true } },
2957  /* RS405/DS5U*/ { {"0B03", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
2958  /* RS435_RGB/AWGC*/ { {"0B07", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 1280, 720, 0 } }, 30, true } },
2959  /* D435/USB2*/ { {"0B07", false },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 480, 270, 0 },{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 424, 240, 0 } }, 30, true } },
2960 
2961  /* SR300*/ { {"0AA5", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 240, 0 },
2962  { RS2_STREAM_INFRARED, RS2_FORMAT_Y8, 640, 240, 1 },
2963  { RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
2964 
2965  /* SR300*/ { {"0AA5", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },
2966  { RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 1920, 1080, 0 } }, 30, true } },
2967 };
2968 
2969 TEST_CASE("Pipeline enable stream", "[live]") {
2970 
2971  auto dev_requests = pipeline_custom_configurations;
2972 
2973  rs2::context ctx;
2975  {
2976  auto list = ctx.query_devices();
2977  REQUIRE(list.size());
2978 
2979  rs2::device dev;
2980  rs2::pipeline pipe(ctx);
2981  rs2::config cfg;
2983  REQUIRE_NOTHROW(profile = cfg.resolve(pipe));
2984  REQUIRE(profile);
2985  REQUIRE_NOTHROW(dev = profile.get_device());
2986  REQUIRE(dev);
2988  dev_type PID = get_PID(dev);
2989  CAPTURE(PID.first);
2990  CAPTURE(PID.second);
2991 
2992  if (dev_requests.end() == dev_requests.find(PID))
2993  {
2994  WARN("Skipping test - the Device-Under-Test profile is not defined for PID " << PID.first << (PID.second ? " USB3" : " USB2"));
2995  }
2996  else
2997  {
2998  REQUIRE(dev_requests[PID].streams.size() > 0);
2999 
3000  for (auto req : pipeline_custom_configurations.at(PID).streams)
3001  REQUIRE_NOTHROW(cfg.enable_stream(req.stream, req.index, req.width, req.height, req.format, dev_requests[PID].fps));
3002 
3003  REQUIRE_NOTHROW(profile = pipe.start(cfg));
3004  REQUIRE(profile);
3006  std::vector<std::vector<rs2::stream_profile>> frames;
3007  std::vector<std::vector<double>> timestamps;
3008 
3009  for (auto i = 0; i < 30; i++)
3010  REQUIRE_NOTHROW(pipe.wait_for_frames(5000));
3011 
3012  auto actual_fps = dev_requests[PID].fps;
3013 
3014  while (frames.size() < 100)
3015  {
3016  frameset frame;
3017  REQUIRE_NOTHROW(frame = pipe.wait_for_frames(5000));
3018  std::vector<rs2::stream_profile> frames_set;
3019  std::vector<double> ts;
3020 
3021  for (auto f : frame)
3022  {
3023  if (f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS))
3024  {
3025  auto val = static_cast<int>(f.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS));
3026  if (val < actual_fps)
3027  actual_fps = val;
3028  }
3029  frames_set.push_back(f.get_profile());
3030  ts.push_back(f.get_timestamp());
3031  }
3032  frames.push_back(frames_set);
3033  timestamps.push_back(ts);
3034  }
3035 
3036  REQUIRE_NOTHROW(pipe.stop());
3037  validate(frames, timestamps, dev_requests[PID], actual_fps);
3038  }
3039  }
3040 }
3041 
3042 static const std::map<dev_type, device_profiles> pipeline_autocomplete_configurations = {
3043  /* RS400/PSR*/ { {"0AD1", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_ANY, 0, 0, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_ANY, 0, 0, 1 } }, 30, true } },
3044  /* RS410/ASR*/ { {"0AD2", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_ANY, 0, 0, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_ANY, 0, 0, 1 } }, 30, true } },
3045  /* D410/USB2*/ { {"0AD2", false },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 0, 0, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 0, 0, 0 } }, 15, true } },
3046  // FW issues render streaming Depth:HD + Color:FHD as not feasible. Applies for AWGC and ASRC
3047  /* AWGC/ASRC should be invoked with 30 fps in order to avoid selecting FullHD for Color sensor at least with FW 5.9.6*/
3048  /* RS415/ASRC*/ { {"0AD3", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_ANY, 0, 0, 0 }/*,{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 0, 0, 0 }*/ }, 30, true } },
3049  /* D415/USB2*/ { {"0AD3", false },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 0, 0, 0 },{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 0, 0, 0 } }, 60, true } },
3050  /* RS430/AWG*/ { {"0AD4", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_ANY, 0, 0, 0 } }, 30, true } },
3051  /* RS430_MM/AWGT*/ { {"0AD5", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_ANY, 0, 0, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_ANY, 0, 0, 1 } }, 30, true } },
3052  /* RS420/PWG*/ { {"0AF6", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_ANY, 0, 0, 0 } }, 30, true } },
3053  /* RS420_MM/PWGT*/ { {"0AFE", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 0, 0, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 0, 0, 0 } }, 30, true } },
3054  /* RS410_MM/ASRT*/ { {"0AFF", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 0, 0, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 0, 0, 0 } }, 30, true } },
3055  /* RS400_MM/PSR*/ { {"0B00", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 0, 0, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 0, 0, 0 } }, 30, true } },
3056  /* RS430_MM_RGB/AWGTC*/{ {"0B01", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 0, 0, 0 },{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 0, 0, 0 } }, 30, true } },
3057  /* RS405/DS5U*/ { {"0B03", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 0, 0, 0 },{ RS2_STREAM_INFRARED, RS2_FORMAT_ANY, 0, 0, 1 } }, 30, true } },
3058  /* RS435_RGB/AWGC*/ { {"0B07", true },{ { /*{ RS2_STREAM_DEPTH, RS2_FORMAT_ANY, 0, 0, 0 },*/{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 0, 0, 0 } }, 30, true } },
3059  /* D435/USB2*/ { {"0B07", false },{ { /*{ RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 0, 0, 0 },*/{ RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 0, 0, 0 } }, 60, true } },
3060 
3061  /* SR300*/ { {"0AA5", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_ANY, 0, 0, 0 },
3062  { RS2_STREAM_INFRARED, RS2_FORMAT_ANY, 0, 0, 1 },
3063  { RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 0, 0, 0 } }, 30, true } },
3064 };
3065 
3066 TEST_CASE("Pipeline enable stream auto complete", "[live]")
3067 {
3069 
3070  rs2::context ctx;
3071 
3073  {
3074  auto list = ctx.query_devices();
3075  REQUIRE(list.size());
3076 
3077  rs2::device dev;
3078  rs2::pipeline pipe(ctx);
3079  rs2::config cfg;
3081  REQUIRE_NOTHROW(profile = cfg.resolve(pipe));
3082  REQUIRE(profile);
3083  REQUIRE_NOTHROW(dev = profile.get_device());
3084  REQUIRE(dev);
3086  dev_type PID = get_PID(dev);
3087  CAPTURE(PID.first);
3088  CAPTURE(PID.second);
3089 
3090  if (configurations.end() == configurations.find(PID))
3091  {
3092  WARN("Skipping test - the Device-Under-Test profile is not defined for PID " << PID.first << (PID.second ? " USB3" : " USB2"));
3093  }
3094  else
3095  {
3096  REQUIRE(configurations[PID].streams.size() > 0);
3097 
3098  for (auto req : configurations[PID].streams)
3099  REQUIRE_NOTHROW(cfg.enable_stream(req.stream, req.index, req.width, req.height, req.format, configurations[PID].fps));
3100 
3101  REQUIRE_NOTHROW(profile = pipe.start(cfg));
3102  REQUIRE(profile);
3103  REQUIRE(profile.get_device());
3105 
3106  std::vector<std::vector<rs2::stream_profile>> frames;
3107  std::vector<std::vector<double>> timestamps;
3108 
3109  for (auto i = 0; i < 30; i++)
3110  REQUIRE_NOTHROW(pipe.wait_for_frames(5000));
3111 
3112  auto actual_fps = configurations[PID].fps;
3113 
3114  while (frames.size() < 100)
3115  {
3116  frameset frame;
3117  REQUIRE_NOTHROW(frame = pipe.wait_for_frames(5000));
3118  std::vector<rs2::stream_profile> frames_set;
3119  std::vector<double> ts;
3120  for (auto f : frame)
3121  {
3122  if (f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS))
3123  {
3124  auto val = static_cast<int>(f.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS));
3125  if (val < actual_fps)
3126  actual_fps = val;
3127  }
3128  frames_set.push_back(f.get_profile());
3129  ts.push_back(f.get_timestamp());
3130  }
3131  frames.push_back(frames_set);
3132  timestamps.push_back(ts);
3133  }
3134 
3135  REQUIRE_NOTHROW(pipe.stop());
3136  validate(frames, timestamps, configurations[PID], actual_fps);
3137  }
3138  }
3139 }
3140 
3141 TEST_CASE("Pipeline disable_all", "[live]") {
3142 
3143  auto not_default_configurations = pipeline_custom_configurations;
3144  auto default_configurations = pipeline_default_configurations;
3145 
3146  rs2::context ctx;
3147 
3149  {
3150  auto list = ctx.query_devices();
3151  REQUIRE(list.size());
3152 
3153  rs2::device dev;
3154  rs2::pipeline pipe(ctx);
3155  rs2::config cfg;
3157  REQUIRE_NOTHROW(profile = cfg.resolve(pipe));
3158  REQUIRE(profile);
3159  REQUIRE_NOTHROW(dev = profile.get_device());
3160  REQUIRE(dev);
3162  dev_type PID = get_PID(dev);
3163  CAPTURE(PID.first);
3164  CAPTURE(PID.second);
3165 
3166  if ((not_default_configurations.end() == not_default_configurations.find(PID)) || ((default_configurations.find(PID) == default_configurations.end())))
3167  {
3168  WARN("Skipping test - the Device-Under-Test profile is not defined properly for PID " << PID.first << (PID.second ? " USB3" : " USB2"));
3169  }
3170  else
3171  {
3172  REQUIRE(not_default_configurations[PID].streams.size() > 0);
3173 
3174  for (auto req : not_default_configurations[PID].streams)
3175  REQUIRE_NOTHROW(cfg.enable_stream(req.stream, req.index, req.width, req.height, req.format, not_default_configurations[PID].fps));
3176 
3178 
3179  REQUIRE_NOTHROW(profile = pipe.start(cfg));
3180  REQUIRE(profile);
3182 
3183 
3184  std::vector<std::vector<rs2::stream_profile>> frames;
3185  std::vector<std::vector<double>> timestamps;
3186 
3187  for (auto i = 0; i < 30; i++)
3188  REQUIRE_NOTHROW(pipe.wait_for_frames(5000));
3189 
3190  auto actual_fps = default_configurations[PID].fps;
3191 
3192  while (frames.size() < 100)
3193  {
3194  frameset frame;
3195  REQUIRE_NOTHROW(frame = pipe.wait_for_frames(5000));
3196  std::vector<rs2::stream_profile> frames_set;
3197  std::vector<double> ts;
3198  for (auto f : frame)
3199  {
3200  if (f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS))
3201  {
3202  auto val = static_cast<int>(f.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS));
3203  if (val < actual_fps)
3204  actual_fps = val;
3205  }
3206  frames_set.push_back(f.get_profile());
3207  ts.push_back(f.get_timestamp());
3208  }
3209  frames.push_back(frames_set);
3210  timestamps.push_back(ts);
3211  }
3212 
3213  REQUIRE_NOTHROW(pipe.stop());
3214  validate(frames, timestamps, default_configurations[PID], actual_fps);
3215  }
3216  }
3217 }
3218 
3219 TEST_CASE("Pipeline disable stream", "[live]") {
3220 
3222 
3223  rs2::context ctx;
3224 
3226  {
3227  auto list = ctx.query_devices();
3228  REQUIRE(list.size());
3229 
3230  rs2::device dev;
3231  rs2::pipeline pipe(ctx);
3232  rs2::config cfg;
3234  REQUIRE_NOTHROW(profile = cfg.resolve(pipe));
3235  REQUIRE(profile);
3236  REQUIRE_NOTHROW(dev = profile.get_device());
3237  REQUIRE(dev);
3239  dev_type PID = get_PID(dev);
3240  CAPTURE(PID.first);
3241  CAPTURE(PID.second);
3242 
3243  if (configurations.end() == configurations.find(PID))
3244  {
3245  WARN("Skipping test - the Device-Under-Test profile is not defined for PID " << PID.first << (PID.second ? " USB3" : " USB2"));
3246  }
3247  else
3248  {
3249  REQUIRE(configurations[PID].streams.size() > 0);
3250 
3251  for (auto req : configurations[PID].streams)
3252  REQUIRE_NOTHROW(cfg.enable_stream(req.stream, req.index, req.width, req.height, req.format, configurations[PID].fps));
3253 
3254  auto stream_to_be_removed = configurations[PID].streams[configurations[PID].streams.size() - 1].stream;
3255  REQUIRE_NOTHROW(cfg.disable_stream(stream_to_be_removed));
3256 
3257  auto& streams = configurations[PID].streams;
3258  streams.erase(streams.end() - 1);
3259 
3260  REQUIRE_NOTHROW(profile = pipe.start(cfg));
3261  REQUIRE(profile);
3263 
3264  std::vector<std::vector<rs2::stream_profile>> frames;
3265  std::vector<std::vector<double>> timestamps;
3266 
3267  for (auto i = 0; i < 30; i++)
3268  REQUIRE_NOTHROW(pipe.wait_for_frames(5000));
3269 
3270  auto actual_fps = configurations[PID].fps;
3271  while (frames.size() < 100)
3272  {
3273  frameset frame;
3274  REQUIRE_NOTHROW(frame = pipe.wait_for_frames(5000));
3275  std::vector<rs2::stream_profile> frames_set;
3276  std::vector<double> ts;
3277  for (auto f : frame)
3278  {
3279  if (f.supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS))
3280  {
3281  auto val = static_cast<int>(f.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS));
3282  if (val < actual_fps)
3283  actual_fps = val;
3284  }
3285  frames_set.push_back(f.get_profile());
3286  ts.push_back(f.get_timestamp());
3287  }
3288  frames.push_back(frames_set);
3289  timestamps.push_back(ts);
3290  }
3291 
3292  REQUIRE_NOTHROW(pipe.stop());
3293  validate(frames, timestamps, configurations[PID], actual_fps);
3294  }
3295  }
3296 }
3297 // The test relies on default profiles that may alter
3298 TEST_CASE("Pipeline with specific device", "[live]")
3299 {
3300 
3301  rs2::context ctx;
3302 
3304  {
3305  auto list = ctx.query_devices();
3306  REQUIRE(list.size());
3307 
3308  rs2::device dev;
3309  REQUIRE_NOTHROW(dev = list[0]);
3311  std::string serial, serial1, serial2;
3312  REQUIRE_NOTHROW(serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
3313 
3314  rs2::pipeline pipe(ctx);
3315  rs2::config cfg;
3316  REQUIRE_NOTHROW(cfg.enable_device(serial));
3318  REQUIRE_NOTHROW(profile = cfg.resolve(pipe));
3319  REQUIRE(profile);
3320  REQUIRE_NOTHROW(dev = profile.get_device());
3321  REQUIRE(dev);
3323  REQUIRE_NOTHROW(serial1 = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
3324  CAPTURE(serial);
3325  CAPTURE(serial1);
3326  REQUIRE(serial1 == serial);
3327 
3328  REQUIRE_NOTHROW(profile = pipe.start(cfg));
3329  REQUIRE(profile);
3330  REQUIRE_NOTHROW(dev = profile.get_device());
3331  REQUIRE_NOTHROW(serial2 = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
3332  CAPTURE(serial);
3333  CAPTURE(serial2);
3334  REQUIRE(serial2 == serial);
3335  REQUIRE_NOTHROW(pipe.stop());
3336 
3337  }
3338 }
3339 
3340 bool operator==(std::vector<test_profile> streams1, std::vector<test_profile> streams2)
3341 {
3342  if (streams1.size() != streams2.size())
3343  return false;
3344 
3345  std::sort(streams1.begin(), streams1.end());
3346  std::sort(streams2.begin(), streams2.end());
3347 
3348  auto equals = true;
3349  for (auto i = 0; i < streams1.size(); i++)
3350  {
3351  if (streams1[i] != streams2[i])
3352  {
3353  equals = false;
3354  break;
3355  }
3356  }
3357  return equals;
3358 }
3359 
3360 TEST_CASE("Pipeline start stop", "[live]") {
3361 
3363 
3364  rs2::context ctx;
3365 
3367  {
3368  auto list = ctx.query_devices();
3369  REQUIRE(list.size());
3370 
3371  rs2::device dev;
3372  rs2::pipeline pipe(ctx);
3373  rs2::config cfg;
3374  rs2::pipeline_profile pipe_profile;
3375  REQUIRE_NOTHROW(pipe_profile = cfg.resolve(pipe));
3376  REQUIRE(pipe_profile);
3377  REQUIRE_NOTHROW(dev = pipe_profile.get_device());
3378  REQUIRE(dev);
3380  dev_type PID = get_PID(dev);
3381  CAPTURE(PID.first);
3382  CAPTURE(PID.second);
3383 
3384  if (configurations.end() == configurations.find(PID))
3385  {
3386  WARN("Skipping test - the Device-Under-Test profile is not defined for PID " << PID.first << (PID.second ? " USB3" : " USB2"));
3387  }
3388  else
3389  {
3390  REQUIRE(configurations[PID].streams.size() > 0);
3391 
3392  for (auto req : configurations[PID].streams)
3393  REQUIRE_NOTHROW(cfg.enable_stream(req.stream, req.index, req.width, req.height, req.format, configurations[PID].fps));
3394 
3395  auto& streams = configurations[PID].streams;
3396 
3397  REQUIRE_NOTHROW(pipe.start(cfg));
3398 
3399  std::vector<device_profiles> frames;
3400 
3401  for (auto i = 0; i < 10; i++)
3402  REQUIRE_NOTHROW(pipe.wait_for_frames(5000));
3403 
3404 
3405  REQUIRE_NOTHROW(pipe.stop());
3406  REQUIRE_NOTHROW(pipe.start(cfg));
3407 
3408  for (auto i = 0; i < 20; i++)
3409  REQUIRE_NOTHROW(pipe.wait_for_frames(5000));
3410 
3411  std::vector<test_profile> profiles;
3412  auto equals = 0;
3413  for (auto i = 0; i < 30; i++)
3414  {
3415  frameset frame;
3416  REQUIRE_NOTHROW(frame = pipe.wait_for_frames(5000));
3417  REQUIRE(frame.size() > 0);
3418 
3419  for (auto f : frame)
3420  {
3421  auto profile = f.get_profile();
3422  auto video_profile = profile.as<video_stream_profile>();
3423 
3424  profiles.push_back({ profile.stream_type(),
3425  profile.format(),
3426  video_profile.width(),
3427  video_profile.height(),
3428  video_profile.stream_index() });
3429 
3430  }
3431  if (profiles == streams)
3432  equals++;
3433  profiles.clear();
3434  }
3435 
3436  REQUIRE_NOTHROW(pipe.stop());
3437  REQUIRE(equals > 1);
3438  }
3439  }
3440 }
3441 
3442 bool compare(const rs2_extrinsics& first, const rs2_extrinsics& second, double delta = 0)
3443 {
3444  for (auto i = 0; i < 9; i++)
3445  {
3446  if (std::abs(first.rotation[i] - second.rotation[i]) > delta)
3447  {
3448  return false;
3449  }
3450  }
3451  for (auto i = 0; i < 3; i++)
3452  {
3453  if (std::abs(first.translation[i] - second.translation[i]) > delta)
3454  {
3455  return false;
3456  }
3457  }
3458  return true;
3459 }
3460 
3461 
3462 static const std::map<dev_type, device_profiles> pipeline_configurations_for_extrinsic = {
3463  /* D400/PSR*/ { {"0AD1", true},{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },
3464  { RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
3465  /* D410/ASR*/ { {"0AD2", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },
3466  { RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
3467  /* D410/USB2*/ { {"0AD2", false },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 480, 270, 0 },
3468  { RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 480, 270, 0 } }, 30, true } },
3469  /* D415/ASRC*/ { {"0AD3", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },
3470  { RS2_STREAM_INFRARED, RS2_FORMAT_Y8, 640, 480, 1 } ,
3471  { RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 1920, 1080, 0 } }, 30, true } },
3472  /* D415/USB2*/ { {"0AD3", false },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 480, 270, 0 },
3473  { RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 424, 240, 0 } }, 30, true } },
3474  /* RS430/AWG*/ { {"0AD4", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },
3475  { RS2_STREAM_INFRARED, RS2_FORMAT_Y8, 640, 480, 1 } }, 30, true } },
3476  /* RS430_MM/AWGT*/ { {"0AD5", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },
3477  { RS2_STREAM_INFRARED, RS2_FORMAT_Y8, 640, 480, 1 } }, 30, true } },
3478  /* RS420/PWG*/ { {"0AF6", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },
3479  { RS2_STREAM_INFRARED, RS2_FORMAT_Y8, 640, 480, 1 } }, 30, true } },
3480  /* RS420_MM/PWGT*/ { {"0AFE", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },
3481  { RS2_STREAM_INFRARED, RS2_FORMAT_Y8, 640, 480, 1 } }, 30, true } },
3482  /* RS410_MM/ASRT*/ { {"0AFF", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },
3483  { RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
3484  /* RS400_MM/PSR*/ { {"0B00", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },
3485  { RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
3486  /* RS430_MM_RGB/AWGTC*/{ {"0B01", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },
3487  { RS2_STREAM_INFRARED, RS2_FORMAT_Y8, 640, 480, 1 } ,
3488  { RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 1920, 1080, 0 } }, 30, true } },
3489  /* RS405/DS5U*/ { {"0B03", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },
3490  { RS2_STREAM_INFRARED, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
3491  /* RS435_RGB/AWGC*/ { {"0B07", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 480, 0 },
3492  { RS2_STREAM_INFRARED, RS2_FORMAT_Y8, 640, 480, 1 } ,
3493  { RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 1920, 1080, 0 } }, 30, true } },
3494  /* D435/USB2*/ { {"0B07", false },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 480, 270, 0 },
3495  { RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 424, 240, 0 } }, 30, true } },
3496 
3497  /* SR300*/ { {"0AA5", true },{ { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16, 640, 240, 0 },
3498  { RS2_STREAM_INFRARED, RS2_FORMAT_Y8, 640, 240, 1 },
3499  { RS2_STREAM_COLOR, RS2_FORMAT_RGB8, 640, 480, 0 } }, 30, true } },
3500  };
3501 
3502 TEST_CASE("Pipeline get selection", "[live]") {
3503  rs2::context ctx;
3505 
3507  {
3508  auto list = ctx.query_devices();
3509  REQUIRE(list.size());
3510 
3511  rs2::device dev;
3512  rs2::pipeline pipe(ctx);
3513  rs2::config cfg;
3514  rs2::pipeline_profile pipe_profile;
3515  REQUIRE_NOTHROW(pipe_profile = cfg.resolve(pipe));
3516  REQUIRE(pipe_profile);
3517  REQUIRE_NOTHROW(dev = pipe_profile.get_device());
3518  REQUIRE(dev);
3520  dev_type PID = get_PID(dev);
3521  CAPTURE(PID.first);
3522  CAPTURE(PID.second);
3523 
3524  if (configurations.end() == configurations.find(PID))
3525  {
3526  WARN("Skipping test - the Device-Under-Test profile is not defined for PID " << PID.first << (PID.second ? " USB3" : " USB2"));
3527  }
3528  else
3529  {
3530  REQUIRE(configurations[PID].streams.size() > 0);
3531 
3532  for (auto req : configurations[PID].streams)
3533  REQUIRE_NOTHROW(cfg.enable_stream(req.stream, req.index, req.width, req.height, req.format, configurations[PID].fps));
3534 
3535  REQUIRE_NOTHROW(pipe.start(cfg));
3536  std::vector<rs2::stream_profile> profiles;
3537  REQUIRE_NOTHROW(pipe_profile = pipe.get_active_profile());
3538  REQUIRE(pipe_profile);
3539  REQUIRE_NOTHROW(profiles = pipe_profile.get_streams());
3540 
3541  auto streams = configurations[PID].streams;
3542  std::vector<test_profile> pipe_streams;
3543  for (auto s : profiles)
3544  {
3546  auto video = s.as<video_stream_profile>();
3547 
3548  pipe_streams.push_back({ video.stream_type(), video.format(), video.width(), video.height(), video.stream_index() });
3549  }
3550  REQUIRE(pipe_streams.size() == streams.size());
3551 
3552  std::sort(pipe_streams.begin(), pipe_streams.end());
3553  std::sort(streams.begin(), streams.end());
3554 
3555  for (auto i = 0; i < pipe_streams.size(); i++)
3556  {
3557  REQUIRE(pipe_streams[i] == streams[i]);
3558  }
3559  }
3560  }
3561 }
3562 
3563 TEST_CASE("Per-frame metadata sanity check", "[live][!mayfail]") {
3564  //Require at least one device to be plugged in
3565  rs2::context ctx;
3567  {
3568  std::vector<sensor> list;
3569  REQUIRE_NOTHROW(list = ctx.query_all_sensors());
3570  REQUIRE(list.size() > 0);
3571 
3572  const int frames_before_start_measure = 10;
3573  const int frames_for_fps_measure = 50;
3574  const double msec_to_sec = 0.001;
3575  const int num_of_profiles_for_each_subdevice = 2;
3576  const float max_diff_between_real_and_metadata_fps = 1.0f;
3577 
3578  for (auto && subdevice : list) {
3579  std::vector<rs2::stream_profile> modes;
3580  REQUIRE_NOTHROW(modes = subdevice.get_stream_profiles());
3581 
3582  REQUIRE(modes.size() > 0);
3583  CAPTURE(subdevice.get_info(RS2_CAMERA_INFO_NAME));
3584 
3585  //the test will be done only on sub set of profile for each sub device
3586  for (int i = 0; i < modes.size(); i += static_cast<int>(std::ceil((float)modes.size() / (float)num_of_profiles_for_each_subdevice)))
3587  {
3588  // Full-HD is often times too heavy for the build machine to handle
3589  if (auto video_profile = modes[i].as<video_stream_profile>())
3590  {
3591  if (video_profile.width() == 1920 && video_profile.height() == 1080 && video_profile.fps() == 60)
3592  {
3593  continue; // Disabling for now
3594  }
3595  }
3596  // GPIO Requires external triggers to produce events
3597  if (RS2_STREAM_GPIO == modes[i].stream_type())
3598  continue; // Disabling for now
3599 
3600  CAPTURE(modes[i].format());
3601  CAPTURE(modes[i].fps());
3602  CAPTURE(modes[i].stream_type());
3603  CAPTURE(modes[i].stream_index());
3604  if (auto video = modes[i].as<video_stream_profile>())
3605  {
3606  CAPTURE(video.width());
3607  CAPTURE(video.height());
3608  }
3609 
3610  std::vector<internal_frame_additional_data> frames_additional_data;
3611  auto frames = 0;
3612  double start;
3613  std::condition_variable cv;
3614  std::mutex m;
3615  auto first = true;
3616 
3617  REQUIRE_NOTHROW(subdevice.open({ modes[i] }));
3618  disable_sensitive_options_for(subdevice);
3619 
3620  REQUIRE_NOTHROW(subdevice.start([&](rs2::frame f)
3621  {
3622 
3623  if ((frames >= frames_before_start_measure) && (frames_additional_data.size() < frames_for_fps_measure))
3624  {
3625  if (first)
3626  {
3627  start = internal::get_time();
3628  }
3629  first = false;
3630 
3631  internal_frame_additional_data data{ f.get_timestamp(),
3632  f.get_frame_number(),
3633  f.get_frame_timestamp_domain(),
3634  f.get_profile().stream_type(),
3635  f.get_profile().format() };
3636 
3637  // Store frame metadata attributes, verify API behavior correctness
3638  for (auto i = 0; i < rs2_frame_metadata_value::RS2_FRAME_METADATA_COUNT; i++)
3639  {
3640  CAPTURE(i);
3641  bool supported = false;
3642  REQUIRE_NOTHROW(supported = f.supports_frame_metadata((rs2_frame_metadata_value)i));
3643  if (supported)
3644  {
3645  rs2_metadata_type val{};
3646  REQUIRE_NOTHROW(val = f.get_frame_metadata((rs2_frame_metadata_value)i));
3647  data.frame_md.md_attributes[i] = std::make_pair(true, val);
3648  }
3649  else
3650  {
3651 
3652  REQUIRE_THROWS(f.get_frame_metadata((rs2_frame_metadata_value)i));
3653  data.frame_md.md_attributes[i].first = false;
3654  }
3655  }
3656 
3657 
3658  std::unique_lock<std::mutex> lock(m);
3659  frames_additional_data.push_back(data);
3660  }
3661  frames++;
3662  if (frames_additional_data.size() >= frames_for_fps_measure)
3663  {
3664  cv.notify_one();
3665  }
3666  }));
3667 
3668 
3669  CAPTURE(frames_additional_data.size());
3670  CAPTURE(frames_for_fps_measure);
3671  std::unique_lock<std::mutex> lock(m);
3672  cv.wait_for(lock, std::chrono::seconds(15), [&] {return ((frames_additional_data.size() >= frames_for_fps_measure)); });
3673 
3674  REQUIRE_NOTHROW(subdevice.stop());
3675  REQUIRE_NOTHROW(subdevice.close());
3676 
3677  auto end = internal::get_time();
3678  lock.unlock();
3679 
3680  auto seconds = (end - start)*msec_to_sec;
3681 
3682  CAPTURE(start);
3683  CAPTURE(end);
3684  CAPTURE(seconds);
3685 
3686  REQUIRE(seconds > 0);
3687 
3688  if (frames_additional_data.size())
3689  {
3690  auto actual_fps = (double)frames_additional_data.size() / (double)seconds;
3691  double metadata_seconds = frames_additional_data[frames_additional_data.size() - 1].timestamp - frames_additional_data[0].timestamp;
3692  metadata_seconds *= msec_to_sec;
3693 
3694  if (metadata_seconds <= 0)
3695  {
3696  std::cout << "Start metadata " << std::fixed << frames_additional_data[0].timestamp << "\n";
3697  std::cout << "End metadata " << std::fixed << frames_additional_data[frames_additional_data.size() - 1].timestamp << "\n";
3698  }
3699  REQUIRE(metadata_seconds > 0);
3700 
3701  auto metadata_frames = frames_additional_data[frames_additional_data.size() - 1].frame_number - frames_additional_data[0].frame_number;
3702  auto metadata_fps = (double)metadata_frames / (double)metadata_seconds;
3703 
3704  for (auto i = 0; i < frames_additional_data.size() - 1; i++)
3705  {
3706  CAPTURE(i);
3707  CAPTURE(frames_additional_data[i].timestamp_domain);
3708  CAPTURE(frames_additional_data[i + 1].timestamp_domain);
3709  REQUIRE((frames_additional_data[i].timestamp_domain == frames_additional_data[i + 1].timestamp_domain));
3710 
3711  CAPTURE(frames_additional_data[i].frame_number);
3712  CAPTURE(frames_additional_data[i + 1].frame_number);
3713 
3714  REQUIRE((frames_additional_data[i].frame_number < frames_additional_data[i + 1].frame_number));
3715  }
3716 
3717  CAPTURE(metadata_frames);
3718  CAPTURE(metadata_seconds);
3719  CAPTURE(metadata_fps);
3720  CAPTURE(frames_additional_data.size());
3722 
3723  //it the diff in percentage between metadata fps and actual fps is bigger than max_diff_between_real_and_metadata_fps
3724  //the test will fail
3725  REQUIRE(std::fabs(metadata_fps / actual_fps - 1) < max_diff_between_real_and_metadata_fps);
3726 
3727  // Verify per-frame metadata attributes
3728  metadata_verification(frames_additional_data);
3729  }
3730  }
3731  }
3732  }
3733 }
3734 // the tests may incorrectly interpret changes to librealsense-core, namely default profiles selections
3735 TEST_CASE("All suggested profiles can be opened", "[live][!mayfail]") {
3736 
3737  //Require at least one device to be plugged in
3738  rs2::context ctx;
3740  {
3741 
3742  const int num_of_profiles_for_each_subdevice = 2;
3743 
3744  std::vector<sensor> list;
3745  REQUIRE_NOTHROW(list = ctx.query_all_sensors());
3746  REQUIRE(list.size() > 0);
3747 
3748  for (auto && subdevice : list) {
3749 
3750  disable_sensitive_options_for(subdevice);
3751 
3752  std::vector<rs2::stream_profile> modes;
3753  REQUIRE_NOTHROW(modes = subdevice.get_stream_profiles());
3754 
3755  REQUIRE(modes.size() > 0);
3756  //the test will be done only on sub set of profile for each sub device
3757  for (int i = 0; i < modes.size(); i += (int)std::ceil((float)modes.size() / (float)num_of_profiles_for_each_subdevice)) {
3758  //CAPTURE(rs2_subdevice(subdevice));
3759  CAPTURE(modes[i].format());
3760  CAPTURE(modes[i].fps());
3761  CAPTURE(modes[i].stream_type());
3762  REQUIRE_NOTHROW(subdevice.open({ modes[i] }));
3763  REQUIRE_NOTHROW(subdevice.start([](rs2::frame fref) {}));
3764  REQUIRE_NOTHROW(subdevice.stop());
3765  REQUIRE_NOTHROW(subdevice.close());
3766  }
3767  }
3768  }
3769 }
3770 
3771 TEST_CASE("Pipeline config enable resolve start flow", "[live]") {
3772  rs2::context ctx;
3773 
3775  {
3776  auto list = ctx.query_devices();
3777  REQUIRE(list.size());
3778 
3779  rs2::device dev;
3780  rs2::pipeline pipe(ctx);
3781  rs2::config cfg;
3783  REQUIRE_NOTHROW(profile = cfg.resolve(pipe));
3784  REQUIRE(profile);
3785  REQUIRE_NOTHROW(dev = profile.get_device());
3786  REQUIRE(dev);
3788  REQUIRE_NOTHROW(profile = cfg.resolve(pipe));
3789  REQUIRE(profile);
3790  REQUIRE_NOTHROW(dev = profile.get_device());
3791  REQUIRE(dev);
3792 
3793  REQUIRE_NOTHROW(profile = cfg.resolve(pipe));
3794  REQUIRE(profile);
3795  REQUIRE_NOTHROW(dev = profile.get_device());
3796  REQUIRE(dev);
3797 
3799  REQUIRE_NOTHROW(pipe.start(cfg));
3800 
3801  REQUIRE_NOTHROW(profile = pipe.get_active_profile());
3803  CAPTURE(depth_profile.stream_index());
3804  CAPTURE(depth_profile.stream_type());
3805  CAPTURE(depth_profile.format());
3806  CAPTURE(depth_profile.fps());
3807  CAPTURE(depth_profile.width());
3808  CAPTURE(depth_profile.height());
3809  std::vector<device_profiles> frames;
3810 
3811 
3812  uint32_t timeout = is_usb3(dev) ? 500 : 2000; // for USB2 it takes longer to produce frames
3813 
3814  for (auto i = 0; i < 5; i++)
3815  REQUIRE_NOTHROW(pipe.wait_for_frames(timeout));
3816 
3817  REQUIRE_NOTHROW(pipe.stop());
3818  REQUIRE_NOTHROW(pipe.start(cfg));
3819 
3820  for (auto i = 0; i < 5; i++)
3821  REQUIRE_NOTHROW(pipe.wait_for_frames(timeout));
3822 
3824 
3826  REQUIRE_NOTHROW(profile = cfg.resolve(pipe));
3827  REQUIRE(profile);
3828  REQUIRE_NOTHROW(dev = profile.get_device());
3829  REQUIRE(dev);
3830 
3832  REQUIRE_NOTHROW(profile = cfg.resolve(pipe));
3833  REQUIRE(profile);
3834  REQUIRE_NOTHROW(dev = profile.get_device());
3835  REQUIRE(dev);
3836  }
3837 }
3838 
3839 TEST_CASE("Pipeline - multicam scenario with specific devices", "[live][multicam]") {
3840  rs2::context ctx;
3841 
3843  {
3844 
3845  auto list = ctx.query_devices();
3846  int realsense_devices_count = 0;
3847  rs2::device d;
3848  for (auto&& dev : list)
3849  {
3850  if (dev.supports(RS2_CAMERA_INFO_NAME))
3851  {
3853  if (name != "Platform Camera")
3854  {
3855  realsense_devices_count++;
3856  d = dev;
3857  }
3858  }
3859  }
3860  if (realsense_devices_count < 2)
3861  {
3862  WARN("Skipping test! This test requires multiple RealSense devices connected");
3863  return;
3864  }
3865 
3867  //After getting the device, find a serial and a profile it can use
3868  std::string required_serial;
3870  rs2::stream_profile required_profile;
3871  //find the a video profile of some sensor
3872  for (auto&& sensor : d.query_sensors())
3873  {
3874  for (auto&& profile : sensor.get_stream_profiles())
3875  {
3876  auto vid_profile = profile.as<video_stream_profile>();
3877  if (vid_profile)
3878  {
3879  required_profile = vid_profile;
3880  break;
3881  }
3882  }
3883  if (required_profile)
3884  break;
3885  }
3886  REQUIRE(required_profile);
3887  CAPTURE(required_profile);
3888  auto vid_profile = required_profile.as<video_stream_profile>();
3889 
3890  rs2::device dev;
3891  rs2::pipeline pipe(ctx);
3892  rs2::config cfg;
3893 
3894  //Using the config object to request the serial and stream that we found above
3895  REQUIRE_NOTHROW(cfg.enable_device(required_serial));
3896  REQUIRE_NOTHROW(cfg.enable_stream(vid_profile.stream_type(), vid_profile.stream_index(), vid_profile.width(), vid_profile.height(), vid_profile.format(), vid_profile.fps()));
3897 
3898  //Testing that config.resolve() returns the right data
3899  rs2::pipeline_profile resolved_profile;
3900  REQUIRE_NOTHROW(resolved_profile = cfg.resolve(pipe));
3901  REQUIRE(resolved_profile);
3902  rs2::pipeline_profile resolved_profile_from_start;
3903  REQUIRE_NOTHROW(resolved_profile_from_start = pipe.start(cfg));
3904  REQUIRE(resolved_profile_from_start);
3905 
3906  REQUIRE_NOTHROW(dev = resolved_profile.get_device());
3907  REQUIRE(dev);
3908  rs2::device dev_from_start;
3909  REQUIRE_NOTHROW(dev_from_start = resolved_profile_from_start.get_device());
3910  REQUIRE(dev_from_start);
3911 
3912  //Compare serial number
3913  std::string actual_serial;
3914  std::string actual_serial_from_start;
3915  REQUIRE_NOTHROW(actual_serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
3916  REQUIRE_NOTHROW(actual_serial_from_start = dev_from_start.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
3917  REQUIRE(actual_serial == required_serial);
3918  REQUIRE(actual_serial == actual_serial_from_start);
3919 
3920  //Compare Stream
3921  std::vector<rs2::stream_profile> actual_streams;
3922  REQUIRE_NOTHROW(actual_streams = resolved_profile.get_streams());
3923  REQUIRE(actual_streams.size() == 1);
3924  REQUIRE(actual_streams[0] == required_profile);
3925  std::vector<rs2::stream_profile> actual_streams_from_start;
3926  REQUIRE_NOTHROW(actual_streams_from_start = resolved_profile_from_start.get_streams());
3927  REQUIRE(actual_streams_from_start.size() == 1);
3928  REQUIRE(actual_streams_from_start[0] == required_profile);
3929 
3930  pipe.stop();
3931 
3932  //Using the config object to request the serial and stream that we found above, and test the pipeline.start() returns the right data
3933  rs2::device started_dev;
3934  rs2::pipeline_profile strarted_profile;
3935  cfg = rs2::config(); //Clean previous config
3936 
3937  //Using the config object to request the serial and stream that we found above
3938  REQUIRE_NOTHROW(cfg.enable_device(required_serial));
3939  REQUIRE_NOTHROW(cfg.enable_stream(vid_profile.stream_type(), vid_profile.stream_index(), vid_profile.width(), vid_profile.height(), vid_profile.format(), vid_profile.fps()));
3940 
3941  //Testing that pipeline.start(cfg) returns the right data
3942  REQUIRE_NOTHROW(strarted_profile = pipe.start(cfg));
3943  REQUIRE(strarted_profile);
3944  REQUIRE_NOTHROW(started_dev = strarted_profile.get_device());
3945  REQUIRE(started_dev);
3946 
3947  //Compare serial number
3948  std::string started_serial;
3949  REQUIRE_NOTHROW(started_serial = started_dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
3950  REQUIRE(started_serial == required_serial);
3951 
3952  //Compare Stream
3953  std::vector<rs2::stream_profile> started_streams;
3954  REQUIRE_NOTHROW(started_streams = strarted_profile.get_streams());
3955  REQUIRE(started_streams.size() == 1);
3956  REQUIRE(started_streams[0] == required_profile);
3957  pipe.stop();
3958  }
3959 }
3960 
3961 TEST_CASE("Empty Pipeline Profile", "[live]") {
3962  rs2::context ctx;
3963 
3965  {
3967  rs2::pipeline_profile prof;
3968  REQUIRE_FALSE(prof);
3969  rs2::device dev;
3970  CHECK_THROWS(dev = prof.get_device());
3971  REQUIRE(!dev);
3972  for (int i = 0; i < (int)RS2_STREAM_COUNT; i++)
3973  {
3974  rs2_stream stream = static_cast<rs2_stream>(i);
3975  CAPTURE(stream);
3977  CHECK_THROWS(sp = prof.get_stream(stream));
3978  REQUIRE(!sp);
3979  }
3980  std::vector<rs2::stream_profile> spv;
3981  CHECK_THROWS(spv = prof.get_streams());
3982  REQUIRE(spv.size() == 0);
3983  }
3984 }
3985 
3987 {
3988  rs2::device d1 = profile1.get_device();
3989  rs2::device d2 = profile2.get_device();
3990 
3991  REQUIRE(d1.get().get());
3992  REQUIRE(d2.get().get());
3993  std::string serial1, serial2;
3996  if (serial1 != serial2)
3997  {
3998  throw std::runtime_error(serial1 + " is different than " + serial2);
3999  }
4000 
4001  std::string name1, name2;
4004  if (name1 != name2)
4005  {
4006  throw std::runtime_error(name1 + " is different than " + name2);
4007  }
4008 
4009  auto streams1 = profile1.get_streams();
4010  auto streams2 = profile2.get_streams();
4011  if (streams1.size() != streams2.size())
4012  {
4013  throw std::runtime_error(std::string("Profiles contain different number of streams ") + std::to_string(streams1.size()) + " vs " + std::to_string(streams2.size()));
4014  }
4015 
4016  auto streams1_and_2_equals = true;
4017  for (auto&& s : streams1)
4018  {
4019  auto it = std::find_if(streams2.begin(), streams2.end(), [&s](const rs2::stream_profile& sp) {
4020  return
4021  s.format() == sp.format() &&
4022  s.fps() == sp.fps() &&
4023  s.is_default() == sp.is_default() &&
4024  s.stream_index() == sp.stream_index() &&
4025  s.stream_type() == sp.stream_type() &&
4026  s.stream_name() == sp.stream_name();
4027  });
4028  if (it == streams2.end())
4029  {
4030  streams1_and_2_equals = false;
4031  }
4032  }
4033 
4034  if (!streams1_and_2_equals)
4035  {
4036  throw std::runtime_error(std::string("Profiles contain different streams"));
4037  }
4038 }
4039 TEST_CASE("Pipeline empty Config", "[live]") {
4040  rs2::context ctx;
4041 
4043  {
4045  //Empty config
4046  rs2::pipeline p(ctx);
4047  rs2::config c1;
4048  REQUIRE(c1.get().get() != nullptr);
4049  bool can_resolve = false;
4050  REQUIRE_NOTHROW(can_resolve = c1.can_resolve(p));
4051  REQUIRE(true == can_resolve);
4052  REQUIRE_THROWS(c1.resolve(nullptr));
4054  REQUIRE_NOTHROW(profile = c1.resolve(p));
4055  REQUIRE(true == profile);
4056  }
4057 }
4058 
4059 TEST_CASE("Pipeline 2 Configs", "[live]") {
4060  rs2::context ctx;
4061 
4063  {
4064  rs2::pipeline p(ctx);
4067  rs2::config c1;
4068  rs2::config c2;
4069  bool can_resolve1 = false;
4070  bool can_resolve2 = false;
4071  REQUIRE_NOTHROW(can_resolve1 = c1.can_resolve(p));
4072  REQUIRE_NOTHROW(can_resolve2 = c2.can_resolve(p));
4073  REQUIRE(can_resolve1);
4074  REQUIRE(can_resolve2);
4075  rs2::pipeline_profile profile1;
4076  rs2::pipeline_profile profile2;
4077 
4078  REQUIRE_NOTHROW(profile1 = c1.resolve(p));
4079  REQUIRE(profile1);
4080  REQUIRE_NOTHROW(profile2 = c2.resolve(p));
4081  REQUIRE(profile2);
4082 
4083  REQUIRE_NOTHROW(require_pipeline_profile_same(profile1, profile2));
4084  }
4085 }
4086 
4087 TEST_CASE("Pipeline start after resolve uses the same profile", "[live]") {
4088  rs2::context ctx;
4089 
4091  {
4092  rs2::pipeline pipe(ctx);
4093  rs2::config cfg;
4095  rs2::pipeline_profile profile_from_cfg;
4096  REQUIRE_NOTHROW(profile_from_cfg = cfg.resolve(pipe));
4097  REQUIRE(profile_from_cfg);
4098  rs2::pipeline_profile profile_from_start;
4099  REQUIRE_NOTHROW(profile_from_start = pipe.start(cfg));
4100  REQUIRE(profile_from_start);
4101  REQUIRE_NOTHROW(require_pipeline_profile_same(profile_from_cfg, profile_from_start));
4102 
4103  }
4104 }
4105 
4106 TEST_CASE("Pipeline start ignores previous config if it was changed", "[live]") {
4107  rs2::context ctx;
4109  {
4110  rs2::pipeline pipe(ctx);
4111  rs2::config cfg;
4112  rs2::pipeline_profile profile_from_cfg;
4113  REQUIRE_NOTHROW(profile_from_cfg = cfg.resolve(pipe));
4114  REQUIRE(profile_from_cfg);
4115  cfg.enable_stream(RS2_STREAM_INFRARED, RS2_FORMAT_ANY, 60); //enable a single stream (unlikely to be the default one)
4116  rs2::pipeline_profile profile_from_start;
4117  REQUIRE_NOTHROW(profile_from_start = pipe.start(cfg));
4118  REQUIRE(profile_from_start);
4119  REQUIRE_THROWS(require_pipeline_profile_same(profile_from_cfg, profile_from_start));
4120  }
4121 }
4122 TEST_CASE("Pipeline Config disable all is a nop with empty config", "[live]") {
4123  rs2::context ctx;
4124 
4126  {
4127  rs2::pipeline p(ctx);
4128  rs2::config c1;
4129  c1.disable_all_streams();
4130  rs2::config c2;
4131  rs2::pipeline_profile profile1;
4132  rs2::pipeline_profile profile2;
4133 
4134  REQUIRE_NOTHROW(profile1 = c1.resolve(p));
4135  REQUIRE(profile1);
4136  REQUIRE_NOTHROW(profile2 = c2.resolve(p));
4137  REQUIRE(profile2);
4138  REQUIRE_NOTHROW(require_pipeline_profile_same(profile1, profile2));
4139  }
4140 }
4141 TEST_CASE("Pipeline Config disable each stream is nop on empty config", "[live]") {
4142  rs2::context ctx;
4143 
4145  {
4146  rs2::pipeline p(ctx);
4147  rs2::config c1;
4148  for (int i = 0; i < (int)RS2_STREAM_COUNT; i++)
4149  {
4150  REQUIRE_NOTHROW(c1.disable_stream(static_cast<rs2_stream>(i)));
4151  }
4152  rs2::config c2;
4153  rs2::pipeline_profile profile1;
4154  rs2::pipeline_profile profile2;
4155 
4156  REQUIRE_NOTHROW(profile1 = c1.resolve(p));
4157  REQUIRE(profile1);
4158  REQUIRE_NOTHROW(profile2 = c2.resolve(p));
4159  REQUIRE(profile2);
4160  REQUIRE_NOTHROW(require_pipeline_profile_same(profile1, profile2));
4161  }
4162 }
4163 
4164 TEST_CASE("Pipeline record and playback", "[live]") {
4165  rs2::context ctx;
4166 
4168  {
4170  //Scoping the below code to make sure no one holds the device
4171  {
4172  rs2::pipeline p(ctx);
4173  rs2::config cfg;
4174  REQUIRE_NOTHROW(cfg.enable_record_to_file(filename));
4176  REQUIRE_NOTHROW(profile = cfg.resolve(p));
4177  REQUIRE(profile);
4178  auto dev = profile.get_device();
4179  REQUIRE(dev);
4181  REQUIRE_NOTHROW(p.start(cfg));
4182  std::this_thread::sleep_for(std::chrono::seconds(5));
4184  REQUIRE_NOTHROW(frames = p.wait_for_frames(200));
4185  REQUIRE(frames);
4186  REQUIRE(frames.size() > 0);
4187  REQUIRE_NOTHROW(p.stop());
4188  }
4189  //Scoping the above code to make sure no one holds the device
4190  REQUIRE(file_exists(filename));
4191 
4192  {
4193  rs2::pipeline p(ctx);
4194  rs2::config cfg;
4197  REQUIRE_NOTHROW(profile = cfg.resolve(p));
4198  REQUIRE(profile);
4199  REQUIRE_NOTHROW(p.start(cfg));
4200  std::this_thread::sleep_for(std::chrono::seconds(5));
4202  REQUIRE_NOTHROW(frames = p.wait_for_frames(200));
4203  REQUIRE(frames);
4204  REQUIRE_NOTHROW(p.stop());
4205  }
4206  }
4207 }
4208 
4209 
4210 TEST_CASE("Syncer sanity with software-device device", "[live][software-device]") {
4211  rs2::context ctx;
4213  {
4214 
4215  const int W = 640;
4216  const int H = 480;
4217  const int BPP = 2;
4218  std::shared_ptr<software_device> dev = std::move(std::make_shared<software_device>());
4219  auto s = dev->add_sensor("software_sensor");
4220 
4221  rs2_intrinsics intrinsics{ W, H, 0, 0, 0, 0, RS2_DISTORTION_NONE ,{ 0,0,0,0,0 } };
4222 
4223  s.add_video_stream({ RS2_STREAM_DEPTH, 0, 0, W, H, 60, BPP, RS2_FORMAT_Z16, intrinsics });
4224  s.add_video_stream({ RS2_STREAM_INFRARED, 1, 1, W, H,60, BPP, RS2_FORMAT_Y8, intrinsics });
4225  dev->create_matcher(RS2_MATCHER_DI);
4226 
4227  frame_queue q;
4228 
4229  auto profiles = s.get_stream_profiles();
4230  auto depth = profiles[0];
4231  auto ir = profiles[1];
4232 
4233  syncer sync;
4234  s.open(profiles);
4235  s.start(sync);
4236 
4237  std::vector<uint8_t> pixels(W * H * BPP, 0);
4238  std::weak_ptr<rs2::software_device> weak_dev(dev);
4239 
4240  std::thread t([&s, weak_dev, pixels, depth, ir]() mutable {
4241 
4242  auto shared_dev = weak_dev.lock();
4243  if (shared_dev == nullptr)
4244  return;
4245  s.on_video_frame({ pixels.data(), [](void*) {}, 0,0,0, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 7, depth });
4246  s.on_video_frame({ pixels.data(), [](void*) {}, 0,0,0, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 5, ir });
4247 
4248  s.on_video_frame({ pixels.data(), [](void*) {},0,0, 0, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 8, depth });
4249  s.on_video_frame({ pixels.data(), [](void*) {},0,0, 0, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 6, ir });
4250 
4251  s.on_video_frame({ pixels.data(), [](void*) {},0,0, 0, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 8, ir });
4252  });
4253  t.detach();
4254 
4255  std::vector<std::vector<std::pair<rs2_stream, int>>> expected =
4256  {
4257  { { RS2_STREAM_DEPTH , 7}},
4258  { { RS2_STREAM_INFRARED , 5 } },
4259  { { RS2_STREAM_INFRARED , 6 } },
4260  { { RS2_STREAM_DEPTH , 8 },{ RS2_STREAM_INFRARED , 8 } }
4261  };
4262 
4263  std::vector<std::vector<std::pair<rs2_stream, int>>> results;
4264 
4265  for (auto i = 0; i < expected.size(); i++)
4266  {
4267  frameset fs;
4268  REQUIRE_NOTHROW(fs = sync.wait_for_frames(5000));
4269  std::vector < std::pair<rs2_stream, int>> curr;
4270 
4271  for (auto f : fs)
4272  {
4273  curr.push_back({ f.get_profile().stream_type(), f.get_frame_number() });
4274  }
4275  results.push_back(curr);
4276  }
4277 
4278  CAPTURE(results.size());
4279  CAPTURE(expected.size());
4280  REQUIRE(results.size() == expected.size());
4281 
4282  for (auto i = 0; i < expected.size(); i++)
4283  {
4284  auto exp = expected[i];
4285  auto curr = results[i];
4286  CAPTURE(i);
4287  CAPTURE(exp.size());
4288  CAPTURE(curr.size());
4289  REQUIRE(exp.size() == curr.size());
4290 
4291  for (auto j = 0; j < exp.size(); j++)
4292  {
4293  CAPTURE(j);
4294  CAPTURE(exp[j].first);
4295  CAPTURE(exp[j].second);
4296  CAPTURE(curr[j].first);
4297  CAPTURE(curr[j].second);
4298  REQUIRE(std::find(curr.begin(), curr.end(), exp[j]) != curr.end());
4299  }
4300  }
4301  }
4302 }
4303 
4304 TEST_CASE("Syncer clean_inactive_streams by frame number with software-device device", "[live][software-device]") {
4305  rs2::context ctx;
4307  {
4309  const int W = 640;
4310  const int H = 480;
4311  const int BPP = 2;
4312 
4313  std::shared_ptr<software_device> dev = std::make_shared<software_device>();
4314  auto s = dev->add_sensor("software_sensor");
4315 
4316  rs2_intrinsics intrinsics{ W, H, 0, 0, 0, 0, RS2_DISTORTION_NONE ,{ 0,0,0,0,0 } };
4317  s.add_video_stream({ RS2_STREAM_DEPTH, 0, 0, W, H, 60, BPP, RS2_FORMAT_Z16, intrinsics });
4318  s.add_video_stream({ RS2_STREAM_INFRARED, 1, 1, W, H,60, BPP, RS2_FORMAT_Y8, intrinsics });
4319  dev->create_matcher(RS2_MATCHER_DI);
4320  frame_queue q;
4321 
4322  auto profiles = s.get_stream_profiles();
4323  auto depth = profiles[0];
4324  auto ir = profiles[1];
4325 
4326  syncer sync(10);
4327  s.open(profiles);
4328  s.start(sync);
4329 
4330  std::vector<uint8_t> pixels(W * H * BPP, 0);
4331  std::weak_ptr<rs2::software_device> weak_dev(dev);
4332  std::thread t([s, weak_dev, pixels, depth, ir]() mutable {
4333  auto shared_dev = weak_dev.lock();
4334  if (shared_dev == nullptr)
4335  return;
4336  s.on_video_frame({ pixels.data(), [](void*) {}, 0,0,0, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 1, depth });
4337  s.on_video_frame({ pixels.data(), [](void*) {}, 0,0, 0, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 1, ir });
4338 
4339  s.on_video_frame({ pixels.data(), [](void*) {}, 0,0,0, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 3, depth });
4340 
4341  s.on_video_frame({ pixels.data(), [](void*) {}, 0,0, 0, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 4, depth });
4342 
4343  s.on_video_frame({ pixels.data(), [](void*) {},0,0, 0, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 5, depth });
4344 
4345  s.on_video_frame({ pixels.data(), [](void*) {}, 0,0,0, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 6, depth });
4346 
4347  s.on_video_frame({ pixels.data(), [](void*) {}, 0, 0, 0, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 7, depth });
4348  });
4349 
4350  t.detach();
4351 
4352  std::vector<std::vector<std::pair<rs2_stream, int>>> expected =
4353  {
4354  { { RS2_STREAM_DEPTH , 1 } },
4355  { { RS2_STREAM_INFRARED , 1 } },
4356  { { RS2_STREAM_DEPTH , 3 } },
4357  { { RS2_STREAM_DEPTH , 4 } },
4358  { { RS2_STREAM_DEPTH , 5 } },
4359  { { RS2_STREAM_DEPTH , 6 } },
4360  { { RS2_STREAM_DEPTH , 7 } },
4361  };
4362 
4363  std::vector<std::vector<std::pair<rs2_stream, int>>> results;
4364 
4365  for (auto i = 0; i < expected.size(); i++)
4366  {
4367  frameset fs;
4368  CAPTURE(i);
4369  REQUIRE_NOTHROW(fs = sync.wait_for_frames(5000));
4370  std::vector < std::pair<rs2_stream, int>> curr;
4371 
4372  for (auto f : fs)
4373  {
4374  curr.push_back({ f.get_profile().stream_type(), f.get_frame_number() });
4375  }
4376  results.push_back(curr);
4377  }
4378 
4379  CAPTURE(results.size());
4380  CAPTURE(expected.size());
4381  REQUIRE(results.size() == expected.size());
4382 
4383  for (auto i = 0; i < expected.size(); i++)
4384  {
4385  auto exp = expected[i];
4386  auto curr = results[i];
4387  CAPTURE(i);
4388  CAPTURE(exp.size());
4389  CAPTURE(curr.size());
4390  REQUIRE(exp.size() == exp.size());
4391 
4392  for (auto j = 0; j < exp.size(); j++)
4393  {
4394  CAPTURE(j);
4395  CAPTURE(exp[j].first);
4396  CAPTURE(exp[j].second);
4397  CAPTURE(curr[j].first);
4398  CAPTURE(curr[j].second);
4399  REQUIRE(std::find(curr.begin(), curr.end(), exp[j]) != curr.end());
4400  }
4401  }
4402  }
4403 }
4404 
4405 void dev_changed(rs2_device_list* removed_devs, rs2_device_list* added_devs, void* ptr) {}
4406 TEST_CASE("C API Compilation", "[live]") {
4407  rs2_error* e;
4409  REQUIRE(e != nullptr);
4410 }
static const textual_icon lock
Definition: model-views.h:218
rs2_camera_info
Read-only strings that can be queried from the device. Not all information attributes are available o...
Definition: rs_sensor.h:22
frameset wait_for_frames(unsigned int timeout_ms=RS2_DEFAULT_TIMEOUT) const
bool operator==(const test_profile &other) const
GLuint GLuint end
GLboolean GLboolean GLboolean b
void reset_device(std::shared_ptr< rs2::device > &strong, std::weak_ptr< rs2::device > &weak, device_list &list, const rs2::device &new_dev)
Approx approx(F f)
Definition: approx.h:106
bool is_connected(const device &dev) const
Definition: rs_context.hpp:256
GLenum GLuint GLenum severity
rs2_stream stream
void check_controls_sanity(const rs2::context &ctx, const sensor &dev)
GLuint const GLchar * name
std::vector< sensor > query_all_sensors() const
Generate a flat list of all available sensors from all RealSense devices.
Definition: rs_context.hpp:142
std::vector< sensor > query_sensors() const
Definition: rs_device.hpp:25
const std::shared_ptr< rs2_device > & get() const
Definition: rs_device.hpp:116
rs2::stream_profile get_profile_by_resolution_type(rs2::sensor &s, res_type res)
rs2_option
Defines general configuration controls. These can generally be mapped to camera UVC controls...
Definition: rs_option.h:22
bool operator==(const plane &lhs, const plane &rhs)
Definition: rendering.h:134
GLdouble s
device front() const
Definition: rs_device.hpp:719
long long current_time()
void validate(std::vector< std::vector< rs2::stream_profile >> frames, std::vector< std::vector< double >> timestamps, device_profiles requests, int actual_fps)
GLfloat GLfloat p
Definition: glext.h:12687
const GLfloat * m
Definition: glext.h:6814
device get_sensor_parent(const sensor &s) const
Definition: rs_context.hpp:154
float translation[3]
Definition: rs_sensor.h:99
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:10806
bool wait_for_reset(std::function< bool(void)> func, std::shared_ptr< device > dev)
bool was_removed(const rs2::device &dev) const
Definition: rs_context.hpp:23
GLfloat value
#define REQUIRE_THROWS_AS(expr, exceptionType)
Definition: catch.hpp:17402
void disable_sensitive_options_for(sensor &sen)
device_list query_devices() const
Definition: rs_context.hpp:112
rs2_log_severity get_severity() const
Definition: rs_sensor.hpp:64
TEST_CASE("Sync sanity","[live]")
std::string get_folder_path(special_folder f)
Definition: os.cpp:247
uint32_t size() const
Definition: rs_device.hpp:711
rs2_intrinsics intrin
GLint GLint GLsizei GLsizei GLsizei depth
Definition: cah-model.h:10
std::pair< std::string, bool > dev_type
GLsizei const GLchar *const * string
d
Definition: rmse.py:171
std::string get_description() const
Definition: rs_sensor.hpp:46
void enable_device(const std::string &serial)
bool get_mode(rs2::device &dev, rs2::stream_profile *profile, int mode_index=0)
GLuint GLuint stream
Definition: glext.h:1790
GLdouble n
Definition: glext.h:1966
e
Definition: rmse.py:177
void trigger_error(const rs2::device &dev, int num)
GLsizei GLsizei GLfloat distance
Definition: glext.h:10563
void require_zero_vector(const float(&vector)[3])
void sort(sort_type m_sort_type, const std::string &in, const std::string &out)
GLuint num
Definition: glext.h:5648
float rotation[9]
Definition: rs_sensor.h:98
GLenum GLenum GLsizei void * image
frameset wait_for_frames(unsigned int timeout_ms=5000) const
GLuint index
#define STRINGIFY(arg)
Definition: rs.h:31
GLdouble t
GLboolean GLboolean GLboolean GLboolean a
GLuint GLfloat * val
static const textual_icon usb_type
Definition: model-views.h:249
const int H
def info(name, value, persistent=False)
Definition: test.py:301
static const std::map< dev_type, device_profiles > pipeline_default_configurations
not_this_one begin(...)
GLdouble f
bool poll_for_frames(frameset *f) const
GLenum mode
const int W
GLfloat bias
Definition: glext.h:7937
dictionary streams
Definition: t265_stereo.py:140
std::shared_ptr< rs2_config > get() const
bool is_usb3(const rs2::device &dev)
std::ostream & cout()
std::shared_ptr< device > do_with_waiting_for_camera_connection(rs2::context ctx, std::shared_ptr< device > dev, std::string serial, std::function< void()> operation)
const GLubyte * c
Definition: glext.h:12690
#define SECTION(...)
Definition: catch.hpp:17438
#define WARN(msg)
Definition: catch.hpp:17431
#define CAPTURE(...)
Definition: catch.hpp:17432
REQUIRE(n_callbacks==1)
void disable_all_streams()
unsigned int uint32_t
Definition: stdint.h:80
void dev_changed(rs2_device_list *removed_devs, rs2_device_list *added_devs, void *ptr)
dev_type get_PID(rs2::device &dev)
bool operator==(const video_stream_profile &other) const
const char * get_info(rs2_camera_info info) const
Definition: rs_device.hpp:79
void require_rotation_matrix(const float(&matrix)[9])
GLint GLsizei GLsizei height
static std::condition_variable cv
GLint GLint GLsizei GLint GLenum format
#define REQUIRE_THROWS(...)
Definition: catch.hpp:17401
void require_transposed(const float(&a)[9], const float(&b)[9])
stream_profile get_stream(rs2_stream stream_type, int stream_index=-1) const
Definition: rs_pipeline.hpp:60
def find(dir, mask)
Definition: file.py:25
void disable_stream(rs2_stream stream, int index=-1)
float vector_length(const float(&v)[3])
GLuint start
GLint j
rs2_format
A stream&#39;s format identifies how binary data is encoded within a frame.
Definition: rs_sensor.h:59
GLint first
#define CHECK_THROWS(...)
Definition: catch.hpp:17415
bool compare(const rs2_extrinsics &first, const rs2_extrinsics &second, double delta=0)
std::pair< std::string, bool > dev_type
int stream_index() const
Definition: rs_frame.hpp:34
bool operator!=(const test_profile &other) const
GLint GLint GLsizei GLint GLenum GLenum const void * pixels
void log_to_file(rs2_log_severity min_severity, const char *file_path=nullptr)
Definition: rs.hpp:26
bool supports(rs2_camera_info info) const
Definition: rs_sensor.hpp:125
void enable_stream(rs2_stream stream_type, int stream_index, int width, int height, rs2_format format=RS2_FORMAT_ANY, int framerate=0)
device_list get_new_devices() const
Definition: rs_context.hpp:57
std::vector< std::pair< stream_profile, frame_callback > > configurations
Definition: recorder.h:485
bool can_resolve(std::shared_ptr< rs2_pipeline > p) const
std::vector< std::shared_ptr< stream_profile_interface >> stream_profiles
Definition: streaming.h:165
dictionary intrinsics
Definition: t265_stereo.py:142
GLenum func
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:42
std::vector< profile > streams
device get_device() const
Definition: rs_pipeline.hpp:83
bool poll_for_frames(frameset *fs) const
bool supports(rs2_camera_info info) const
Definition: rs_device.hpp:66
bool make_context(const char *id, rs2::context *ctx, std::string min_api_version="0.0.0")
std::vector< test_profile > configure_all_supported_streams(rs2::sensor &sensor, int width=640, int height=480, int fps=60)
void require_identity_matrix(const float(&matrix)[9])
void require_pipeline_profile_same(const rs2::pipeline_profile &profile1, const rs2::pipeline_profile &profile2)
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented...
Definition: rs_sensor.h:96
rs2_format format() const
Definition: rs_frame.hpp:44
GLdouble GLdouble GLdouble q
pipeline_profile resolve(std::shared_ptr< rs2_pipeline > p) const
std::vector< stream_profile > get_streams() const
Definition: rs_pipeline.hpp:29
void enable_record_to_file(const std::string &file_name)
GLbitfield GLuint64 timeout
static auto it
std::pair< std::shared_ptr< rs2::device >, std::weak_ptr< rs2::device > > make_device(device_list &list)
signed __int64 int64_t
Definition: stdint.h:89
static const std::map< dev_type, device_profiles > pipeline_autocomplete_configurations
GLint GLsizei count
std::shared_ptr< std::function< void(rs2::frame fref)> > check_stream_sanity(const rs2::context &ctx, const rs2::sensor &sub, int num_of_frames, bool infinite=false)
static const std::map< dev_type, device_profiles > pipeline_configurations_for_extrinsic
rs2_rs400_visual_preset
For RS400 devices: provides optimized settings (presets) for specific types of usage.
Definition: rs_option.h:137
static const std::map< dev_type, device_profiles > pipeline_custom_configurations
bool operator<(const test_profile &other) const
#define SECTION_FROM_TEST_NAME
typename::boost::move_detail::remove_reference< T >::type && move(T &&t) BOOST_NOEXCEPT
Video stream intrinsics.
Definition: rs_types.h:58
void open(const stream_profile &profile) const
Definition: rs_sensor.hpp:111
void set_devices_changed_callback(T callback)
Definition: rs_context.hpp:169
std::vector< test_profile > streams
void rs2_set_devices_changed_callback(const rs2_context *context, rs2_devices_changed_callback_ptr callback, void *user, rs2_error **error)
Definition: rs.cpp:787
#define REQUIRE_FALSE(...)
Definition: catch.hpp:17399
GLsizei range
Motion device intrinsics: scale, bias, and variances.
Definition: rs_types.h:103
REQUIRE_NOTHROW(rs2_log(RS2_LOG_SEVERITY_INFO,"Log message using rs2_log()", nullptr))
#define NULL
Definition: tinycthread.c:47
void enable_device_from_file(const std::string &file_name, bool repeat_playback=true)
int i
GLuint res
Definition: glext.h:8856
void close() const
Definition: rs_sensor.hpp:173
option_range get_option_range(rs2_option option) const
Definition: rs_options.hpp:84
bool file_exists(const std::string &filename)
rs2_format format
pipeline_profile get_active_profile() const
pipeline_profile start()
device wait_for_device() const
Definition: rs_context.hpp:240
rs2_log_severity
Severity of the librealsense logger.
Definition: rs_types.h:153
std::vector< stream_profile > get_stream_profiles() const
Definition: rs_sensor.hpp:219
size_t size() const
Definition: rs_frame.hpp:1097
rs2_stream stream_type() const
Definition: rs_frame.hpp:39
void set_option(rs2_option option, float value) const
Definition: rs_options.hpp:99
GLuint64EXT * result
Definition: glext.h:10921
float get_option(rs2_option option) const
Definition: rs_options.hpp:72
void metadata_verification(const std::vector< internal_frame_additional_data > &data)
double get_time()
Definition: rs_internal.hpp:62
int fps() const
Definition: rs_frame.hpp:49
Definition: parser.hpp:150
const int BPP
void start(T callback) const
Definition: rs_sensor.hpp:185
GLint GLsizei width
void copy(void *dst, void const *src, size_t size)
Definition: types.cpp:836
void stop() const
Definition: rs_sensor.hpp:195
T as() const
Definition: rs_device.hpp:129
std::string to_string(T value)


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