Intel® RealSense™ Cross Platform API
Intel Realsense Cross-platform API
config.h
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 
4 #ifndef CONFIG_HPP
5 #define CONFIG_HPP
6 
7 #include <map>
8 #include <mutex>
9 #include <condition_variable>
10 #include <algorithm>
11 #include <cmath>
12 #include <set>
13 #include <iostream>
14 #include "sensor.h"
15 #include "types.h"
16 
17 namespace librealsense
18 {
19 
20  namespace util
21  {
23  {
27  };
28 
29  class config
30  {
31  public:
32  struct request_type
33  {
36  int width, height;
38  int fps;
39  };
40 
41  struct index_type
42  {
44  int index;
45 
46  bool operator<(const index_type& other) const
47  {
48  return std::make_pair(stream, index) <
49  std::make_pair(other.stream, other.index);
50  }
51  };
52 
53 
54 
55  static bool match_stream(const index_type& a, const index_type& b)
56  {
57  if (a.stream != RS2_STREAM_ANY && b.stream != RS2_STREAM_ANY && (a.stream != b.stream))
58  return false;
59  if (a.index != -1 && b.index != -1 && (a.index != b.index))
60  return false;
61 
62  return true;
63  }
64 
65  template<class Stream_Profile>
66  static bool match(const Stream_Profile& a, const Stream_Profile& b)
67  {
68  if (a.stream_type() != RS2_STREAM_ANY && b.stream_type() != RS2_STREAM_ANY && (a.stream_type() != b.stream_type()))
69  return false;
70  if (a.stream_index() != -1 && b.stream_index() != -1 && (a.stream_index() != b.stream_index()))
71  return false;
72  if (a.format() != RS2_FORMAT_ANY && b.format() != RS2_FORMAT_ANY && (a.format() != b.format()))
73  return false;
74  if (a.fps() != 0 && b.fps() != 0 && (a.fps() != b.fps()))
75  return false;
76 
77 
78  if (auto vid_a = dynamic_cast<video_stream_profile_interface*>(a))
79  {
80  if (auto vid_b = dynamic_cast<video_stream_profile_interface*>(b))
81  {
82  if (vid_a->get_width() != 0 && vid_b->get_width() != 0 && (vid_a->get_width() != vid_b->get_width()))
83  return false;
84  if (vid_a->get_height() != 0 && vid_b->get_height() != 0 && (vid_a->get_height() != vid_b->get_height()))
85  return false;
86  }
87  else return false;
88  }
89 
90  return true;
91  }
92 
93  static bool match(stream_profile_interface* a, const request_type& b)
94  {
95  if (a->get_stream_type() != RS2_STREAM_ANY && b.stream != RS2_STREAM_ANY && (a->get_stream_type() != b.stream))
96  return false;
97  if (a->get_stream_index() != -1 && b.stream_index != -1 && (a->get_stream_index() != b.stream_index))
98  return false;
99  if (a->get_format() != RS2_FORMAT_ANY && b.format != RS2_FORMAT_ANY && (a->get_format() != b.format))
100  return false;
101  if (a->get_framerate() != 0 && b.fps != 0 && (a->get_framerate() != b.fps))
102  return false;
103 
104  if (auto vid_a = dynamic_cast<video_stream_profile_interface*>(a))
105  {
106  if (vid_a->get_width() != 0 && b.width != 0 && (vid_a->get_width() != b.width))
107  return false;
108  if (vid_a->get_height() != 0 && b.height != 0 && (vid_a->get_height() != b.height))
109  return false;
110  }
111 
112  return true;
113  }
114 
115 
117  {
118  for (auto request : others)
119  {
120  if (a->get_framerate()!= 0 && request->get_framerate() != 0 && (a->get_framerate() != request->get_framerate()))
121  return true;
122  }
123 
124  if (auto vid_a = dynamic_cast<video_stream_profile_interface*>(a))
125  {
126  for (auto request : others)
127  {
128  if (auto vid_b = dynamic_cast<video_stream_profile_interface*>(request.get()))
129  {
130  if (vid_a->get_width() != 0 && vid_b->get_width() != 0 && (vid_a->get_width() != vid_b->get_width()))
131  return true;
132  if (vid_a->get_framerate() != 0 && vid_b->get_framerate() != 0 && (vid_a->get_framerate() != vid_b->get_framerate()))
133  return true;
134  }
135  }
136  }
137 
138  return false;
139  }
140 
141  static bool contradicts(stream_profile_interface* a, const std::vector<request_type>& others)
142  {
143  for (auto request : others)
144  {
145  if (a->get_framerate() != 0 && request.fps != 0 && (a->get_framerate() != request.fps))
146  return true;
147  }
148 
149  if (auto vid_a = dynamic_cast<video_stream_profile_interface*>(a))
150  {
151  for (auto request : others)
152  {
153  // Patch for DS5U_S that allows different resolutions on multi-pin device
154  if ((vid_a->get_height() == vid_a->get_width()) && (request.height == request.width))
155  return false;
156  if (vid_a->get_width() != 0 && request.width != 0 && (vid_a->get_width() != request.width))
157  return true;
158  if (vid_a->get_height() != 0 && request.height != 0 && (vid_a->get_height() != request.height))
159  return true;
160  }
161  }
162 
163  return false;
164  }
165 
166 
168  {
169  if (a->get_framerate() == 0 || a->get_stream_type() == RS2_STREAM_ANY || a->get_stream_index() == -1 || a->get_format() == RS2_FORMAT_ANY) return true;
170  if (auto vid_a = dynamic_cast<video_stream_profile_interface*>(a))
171  {
172  if (vid_a->get_width() == 0 || vid_a->get_height() == 0) return true;
173  }
174  return false;
175  }
176 
177  static bool has_wildcards(const request_type& a)
178  {
179  if (a.fps == 0 || a.stream == RS2_STREAM_ANY || a.format == RS2_FORMAT_ANY) return true;
180  if (a.width == 0 || a.height == 0) return true;
181  if (a.stream_index == -1) return true;
182  return false;
183  }
184  std::map<index_type, request_type> get_requests()
185  {
186  return _requests;
187  }
188 
189  std::map<index_type, config_preset> get_presets()
190  {
191  return _presets;
192  }
193 
195  {
196  public:
198 
199  explicit multistream(std::map<int, sensor_interface*> results,
200  std::map<index_type, std::shared_ptr<stream_profile_interface>> profiles,
201  std::map<int, stream_profiles> dev_to_profiles)
202  : _profiles(std::move(profiles)),
203  _dev_to_profiles(std::move(dev_to_profiles)),
204  _results(std::move(results))
205  {}
206 
207 
208  void open()
209  {
210  for (auto && kvp : _dev_to_profiles) {
211  auto&& sub = _results.at(kvp.first);
212  sub->open(kvp.second);
213  }
214  }
215 
216  template<class T>
217  void start(T callback)
218  {
219  for (auto&& sensor : _results)
220  sensor.second->start(callback);
221  }
222 
223  void stop()
224  {
225  for (auto&& sensor : _results)
226  sensor.second->stop();
227  }
228 
229  void close()
230  {
231  for (auto&& sensor : _results)
232  sensor.second->close();
233  }
234  std::map<index_type, std::shared_ptr<stream_profile_interface>> get_profiles() const
235  {
236  return _profiles;
237  }
238 
239  std::map<int, stream_profiles> get_profiles_per_sensor() const
240  {
241  return _dev_to_profiles;
242  }
243  private:
244  friend class config;
245 
246  std::map<index_type, std::shared_ptr<stream_profile_interface>> _profiles;
247  std::map<index_type, sensor_interface*> _devices;
248  std::map<int, sensor_interface*> _results;
249  std::map<int, stream_profiles> _dev_to_profiles;
250  };
251 
252  config() : require_all(true) {}
253 
254 
255  void enable_stream(rs2_stream stream, int index, int width, int height, rs2_format format, int fps)
256  {
257  _presets.erase({ stream, index });
258  _requests[{stream, index}] = request_type{ stream, index, width, height, format, fps };
259  require_all = true;
260  }
261 
263  {
264  // disable_stream doesn't change require_all because we want both
265  // enable_stream(COLOR); disable_stream(COLOR);
266  // and enable_all(best_quality); disable_stream(MOTION_DATA);
267  // to work as expected.
268 
269  auto itr = _requests.begin();
270  while( itr != _requests.end())
271  {
272  if(itr->first.stream == stream)
273  {
274  _requests.erase(itr++);
275  }
276  else
277  {
278  itr++;
279  }
280  }
281 
282  }
284  {
285  _requests.erase({ stream, -1 });
286  _presets[{ stream, -1 }] = preset;
287  require_all = true;
288  }
289 
291  {
292  _requests.erase({stream, index});
293  _presets[{stream, index}] = preset;
294  require_all = true;
295  }
296 
298  {
299  for (int i = RS2_STREAM_DEPTH; i < RS2_STREAM_COUNT; i++)
300  {
301  enable_stream(static_cast<rs2_stream>(i), -1, p);
302  }
303  require_all = false;
304  }
305 
306  void disable_all()
307  {
308  _presets.clear();
309  _requests.clear();
310  }
311 
313  {
314  for (auto&& sensor : stream._results)
315  sensor.second->close();
316  }
317 
318  /* template <typename... Args>
319  bool can_enable_stream(device_interface* dev, rs2_stream stream, Args... args) {
320  config c(*this);
321  c.enable_stream(stream, args...);
322  for (auto && kvp : c.map_streams(dev))
323  if (kvp.second->get_stream_type() == stream)
324  return true;
325  return false;
326  }*/
327 
329  {
330  config c(*this);
331  c.enable_stream(stream, index, width, height, format, fps);
332  for (auto && kvp : c.map_streams(dev))
333 
334  if (kvp.second->get_stream_type() == stream && kvp.second->get_stream_index() == index)
335  return true;
336 
337  auto it = _requests.erase({stream, index});
338  return false;
339  }
340 
342  {
343  auto mapping = map_streams(dev);
344 
345  // If required, make sure we've succeeded at opening
346  // all the requested streams
347  if (require_all)
348  {
349  std::set<index_type> all_streams;
350  for (auto && kvp : mapping)
351  all_streams.insert({ kvp.second->get_stream_type(), kvp.second->get_stream_index() });
352 
353 
354  for (auto && kvp : _presets)
355  {
356  auto it = std::find_if(std::begin(all_streams), std::end(all_streams), [&](const index_type& i)
357  {
358  return match_stream(kvp.first, i);
359  });
360  if (it == std::end(all_streams))
361  throw std::runtime_error("Config couldn't configure all streams");
362  }
363  for (auto && kvp : _requests)
364  {
365  auto it = std::find_if(std::begin(all_streams), std::end(all_streams), [&](const index_type& i)
366  {
367  return match_stream(kvp.first, i);
368  });
369  if (it == std::end(all_streams))
370  throw std::runtime_error("Config couldn't configure all streams");
371  }
372  }
373 
374  // Unpack the data returned by assign
375  std::map<int, stream_profiles> dev_to_profiles;
376  std::map<index_type, sensor_interface*> stream_to_dev;
377  std::map<index_type, std::shared_ptr<stream_profile_interface>> stream_to_profile;
378 
379  std::map<int, sensor_interface*> sensors_map;
380  for(auto i = 0; i< dev->get_sensors_count(); i++)
381  {
382  if (mapping.find(i) != mapping.end())
383  {
384  sensors_map[i] = &dev->get_sensor(i);
385  }
386  }
387 
388  for (auto && kvp : mapping) {
389  dev_to_profiles[kvp.first].push_back(kvp.second);
390  index_type idx{ kvp.second->get_stream_type(), kvp.second->get_stream_index() };
391  stream_to_dev.emplace(idx, sensors_map.at(kvp.first));
392  stream_to_profile[idx] = kvp.second;
393  }
394 
395  // TODO: make sure it works
396  return multistream(std::move(sensors_map), std::move(stream_to_profile), std::move(dev_to_profiles));
397  }
398 
399  private:
400  static bool sort_highest_framerate(const std::shared_ptr<stream_profile_interface> lhs, const std::shared_ptr<stream_profile_interface> rhs) {
401  return lhs->get_framerate() < rhs->get_framerate();
402  }
403 
404  static bool sort_largest_image(std::shared_ptr<stream_profile_interface> lhs, std::shared_ptr<stream_profile_interface> rhs) {
405  if (auto a = dynamic_cast<video_stream_profile_interface*>(lhs.get()))
406  if (auto b = dynamic_cast<video_stream_profile_interface*>(rhs.get()))
407  return a->get_width()*a->get_height() < b->get_width()*b->get_height();
408  return sort_highest_framerate(lhs, rhs);
409  }
410 
411  static bool sort_best_quality( std::shared_ptr<stream_profile_interface> lhs, std::shared_ptr<stream_profile_interface> rhs) {
412  if (auto a = dynamic_cast<video_stream_profile_interface*>(lhs.get()))
413  {
414  if (auto b = dynamic_cast<video_stream_profile_interface*>(rhs.get()))
415  {
416  return std::make_tuple((a->get_height() == 640 && a->get_height() == 480), (lhs->get_framerate() == 30), (lhs->get_format() == RS2_FORMAT_Z16), (lhs->get_format() == RS2_FORMAT_Y8), (lhs->get_format() == RS2_FORMAT_RGB8), int(lhs->get_format()))
417  < std::make_tuple((b->get_width() == 640 && b->get_height() == 480), (rhs->get_framerate() == 30), (rhs->get_format() == RS2_FORMAT_Z16), (rhs->get_format() == RS2_FORMAT_Y8), (rhs->get_format() == RS2_FORMAT_RGB8), int(rhs->get_format()));
418 
419  }
420  }
421  return sort_highest_framerate(lhs, rhs);
422  }
423 
424  static void auto_complete(std::vector<request_type> &requests, sensor_interface &target)
425  {
426  auto candidates = target.get_stream_profiles();
427  for (auto & request : requests)
428  {
429  if (!has_wildcards(request)) continue;
430  for (auto candidate : candidates)
431  {
432  if (match(candidate.get(), request) && !contradicts(candidate.get(), requests))
433  {
434  request = to_request(candidate.get());
435  break;
436  }
437  }
438  if (has_wildcards(request))
439  throw std::runtime_error(std::string("Couldn't autocomplete request for subdevice ") + target.get_info(RS2_CAMERA_INFO_NAME));
440  }
441  }
442 
443  static request_type to_request(stream_profile_interface* profile)
444  {
445  request_type r;
446  r.fps = profile->get_framerate();
447  r.stream = profile->get_stream_type();
448  r.format = profile->get_format();
449  r.stream_index = profile->get_stream_index();
450  if (auto vid = dynamic_cast<video_stream_profile_interface*>(profile))
451  {
452  r.width = vid->get_width();
453  r.height = vid->get_height();
454  }
455  else
456  {
457  r.width = 1;
458  r.height = 1;
459  }
460  return r;
461  }
462 
463  std::multimap<int, std::shared_ptr<stream_profile_interface>> map_streams(device_interface* dev) const
464  {
465  std::multimap<int, std::shared_ptr<stream_profile_interface>> out;
466  std::set<index_type> satisfied_streams;
467 
468  // Algorithm assumes get_adjacent_devices always
469  // returns the devices in the same order
470  for (size_t i = 0; i < dev->get_sensors_count(); ++i)
471  {
472  auto&& sub = dev->get_sensor(i);
473  std::vector<request_type> targets;
474  auto profiles = sub.get_stream_profiles();
475 
476  // deal with explicit requests
477  for (auto && kvp : _requests)
478  {
479  if (satisfied_streams.count(kvp.first)) continue; // skip satisfied requests
480 
481  // if any profile on the subdevice can supply this request, consider it satisfiable
482  auto it = std::find_if(begin(profiles), end(profiles),
483  [&kvp](const std::shared_ptr<stream_profile_interface>& profile)
484  {
485  return match(profile.get(), kvp.second);
486  });
487  if (it != end(profiles))
488  {
489  targets.push_back(kvp.second); // store that this request is going to this subdevice
490  satisfied_streams.insert(kvp.first); // mark stream as satisfied
491  }
492  }
493 
494  // deal with preset streams
495  std::vector<rs2_format> prefered_formats = { RS2_FORMAT_Z16, RS2_FORMAT_Y8, RS2_FORMAT_RGB8 };
496  for (auto && kvp : _presets)
497  {
498  if (satisfied_streams.count(kvp.first)) continue; // skip satisfied streams
499 
500  auto result = [&]() -> request_type
501  {
502  switch (kvp.second)
503  {
505  std::sort(begin(profiles), end(profiles), sort_best_quality);
506  break;
508  std::sort(begin(profiles), end(profiles), sort_largest_image);
509  break;
511  std::sort(begin(profiles), end(profiles), sort_highest_framerate);
512  break;
513  default: throw std::runtime_error("Unknown preset selected");
514  }
515 
516  for (auto itr: profiles)
517  {
518  auto stream = index_type{ itr->get_stream_type() ,itr->get_stream_index() };
519  if (match_stream(stream, kvp.first))
520  {
521  return to_request(itr.get());
522  }
523  }
524 
525  return { RS2_STREAM_ANY, -1, 0, 0, RS2_FORMAT_ANY, 0 };
526  }();
527 
528  // RS2_STREAM_COUNT signals subdevice can't handle this stream
529  if (result.stream != RS2_STREAM_ANY)
530  {
531  targets.push_back(result);
532  satisfied_streams.insert({ result.stream, result.stream_index });
533  }
534  }
535 
536  if (targets.size() > 0) // if subdevice is handling any streams
537  {
538  auto_complete(targets, sub);
539 
540  for (auto && t : targets)
541  {
542  for (auto && p : profiles)
543  {
544  if (match(p.get(), t))
545  {
546  out.emplace((int)i, p);
547  break;
548  }
549  }
550  }
551 
552  }
553  }
554 
555  return out;
556 
557  }
558 
559  std::map<index_type, request_type> _requests;
560  std::map<index_type, config_preset> _presets;
561  bool require_all;
562  };
563 
564  }
565 }
566 
567 #endif
virtual rs2_format get_format() const =0
static bool match(stream_profile_interface *a, const request_type &b)
Definition: config.h:93
virtual rs2_stream get_stream_type() const =0
Definition: config.h:24
void close(multistream stream)
Definition: config.h:312
int height
Definition: config.h:36
void stop()
Definition: config.h:223
std::map< index_type, request_type > get_requests()
Definition: config.h:184
void disable_stream(rs2_stream stream)
Definition: config.h:262
config_preset
Definition: config.h:22
virtual size_t get_sensors_count() const =0
void close()
Definition: config.h:229
int index
Definition: config.h:44
void start(T callback)
Definition: config.h:217
multistream(std::map< int, sensor_interface *> results, std::map< index_type, std::shared_ptr< stream_profile_interface >> profiles, std::map< int, stream_profiles > dev_to_profiles)
Definition: config.h:199
Definition: rs_sensor.h:58
sql::statement::iterator end(sql::statement &stmt)
Definition: stream.h:188
Definition: rs_sensor.h:23
Definition: streaming.h:131
void disable_all()
Definition: config.h:306
static bool has_wildcards(const request_type &a)
Definition: config.h:177
static bool has_wildcards(stream_profile_interface *a)
Definition: config.h:167
Definition: rs_sensor.h:62
Definition: algo.h:16
void open()
Definition: config.h:208
static bool match(const Stream_Profile &a, const Stream_Profile &b)
Definition: config.h:66
rs2_stream stream
Definition: config.h:43
config()
Definition: config.h:252
Definition: streaming.h:46
sql::statement::iterator begin(sql::statement &stmt)
virtual sensor_interface & get_sensor(size_t i)=0
Definition: rs_sensor.h:50
virtual stream_profiles get_stream_profiles() const =0
Definition: config.h:25
multistream resolve(device_interface *dev)
Definition: config.h:341
static bool contradicts(stream_profile_interface *a, stream_profiles others)
Definition: config.h:116
Definition: rs_sensor.h:57
Definition: config.h:29
rs2_format
Format identifies how binary data is encoded within a frame.
Definition: rs_sensor.h:55
virtual const std::string & get_info(rs2_camera_info info) const =0
std::vector< std::shared_ptr< stream_profile_interface >> stream_profiles
Definition: streaming.h:104
Definition: rs_sensor.h:41
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:38
std::map< int, stream_profiles > get_profiles_per_sensor() const
Definition: config.h:239
int stream_index
Definition: config.h:35
rs2_stream stream
Definition: config.h:34
static bool contradicts(stream_profile_interface *a, const std::vector< request_type > &others)
Definition: config.h:141
multistream()
Definition: config.h:197
bool can_enable_stream(device_interface *dev, rs2_stream stream, int index, int width, int height, rs2_format format, int fps)
Definition: config.h:328
int width
Definition: config.h:36
Definition: presets.h:99
Definition: rs_sensor.h:40
int fps
Definition: config.h:38
Definition: stream.h:14
static bool match_stream(const index_type &a, const index_type &b)
Definition: config.h:55
virtual int get_stream_index() const =0
std::map< index_type, std::shared_ptr< stream_profile_interface > > get_profiles() const
Definition: config.h:234
void enable_stream(rs2_stream stream, int index, config_preset preset)
Definition: config.h:290
Definition: streaming.h:106
virtual uint32_t get_framerate() const =0
std::map< index_type, config_preset > get_presets()
Definition: config.h:189
bool operator<(const index_type &other) const
Definition: config.h:46
void enable_stream(rs2_stream stream, config_preset preset)
Definition: config.h:283
void enable_all(config_preset p)
Definition: config.h:297
rs2_format format
Definition: config.h:37
void enable_stream(rs2_stream stream, int index, int width, int height, rs2_format format, int fps)
Definition: config.h:255
Definition: rs_sensor.h:66