Intel® RealSense™ Cross Platform API
Intel Realsense Cross-platform API
rs_device.hpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2017 Intel Corporation. All Rights Reserved.
3 
4 #ifndef LIBREALSENSE_RS2_DEVICE_HPP
5 #define LIBREALSENSE_RS2_DEVICE_HPP
6 
7 #include "rs_types.hpp"
8 #include "rs_sensor.hpp"
9 #include <array>
10 
11 namespace rs2
12 {
13  class context;
14  class device_list;
15  class pipeline_profile;
16  class device_hub;
17 
18  class device
19  {
20  public:
25  std::vector<sensor> query_sensors() const
26  {
27  rs2_error* e = nullptr;
28  std::shared_ptr<rs2_sensor_list> list(
29  rs2_query_sensors(_dev.get(), &e),
31  error::handle(e);
32 
33  auto size = rs2_get_sensors_count(list.get(), &e);
34  error::handle(e);
35 
36  std::vector<sensor> results;
37  for (auto i = 0; i < size; i++)
38  {
39  std::shared_ptr<rs2_sensor> dev(
40  rs2_create_sensor(list.get(), i, &e),
42  error::handle(e);
43 
44  sensor rs2_dev(dev);
45  results.push_back(rs2_dev);
46  }
47 
48  return results;
49  }
50 
51  template<class T>
52  T first() const
53  {
54  for (auto&& s : query_sensors())
55  {
56  if (auto t = s.as<T>()) return t;
57  }
58  throw rs2::error("Could not find requested sensor type!");
59  }
60 
66  bool supports(rs2_camera_info info) const
67  {
68  rs2_error* e = nullptr;
69  auto is_supported = rs2_supports_device_info(_dev.get(), info, &e);
70  error::handle(e);
71  return is_supported > 0;
72  }
73 
79  const char* get_info(rs2_camera_info info) const
80  {
81  rs2_error* e = nullptr;
82  auto result = rs2_get_device_info(_dev.get(), info, &e);
83  error::handle(e);
84  return result;
85  }
86 
91  {
92  rs2_error* e = nullptr;
93 
94  rs2_hardware_reset(_dev.get(), &e);
95  error::handle(e);
96  }
97 
98  device& operator=(const std::shared_ptr<rs2_device> dev)
99  {
100  _dev.reset();
101  _dev = dev;
102  return *this;
103  }
104  device& operator=(const device& dev)
105  {
106  *this = nullptr;
107  _dev = dev._dev;
108  return *this;
109  }
110  device() : _dev(nullptr) {}
111 
112  operator bool() const
113  {
114  return _dev != nullptr;
115  }
116  const std::shared_ptr<rs2_device>& get() const
117  {
118  return _dev;
119  }
120 
121  template<class T>
122  bool is() const
123  {
124  T extension(*this);
125  return extension;
126  }
127 
128  template<class T>
129  T as() const
130  {
131  T extension(*this);
132  return extension;
133  }
134  virtual ~device()
135  {
136  }
137 
138  explicit operator std::shared_ptr<rs2_device>() { return _dev; };
139  explicit device(std::shared_ptr<rs2_device> dev) : _dev(dev) {}
140  protected:
141  friend class rs2::context;
142  friend class rs2::device_list;
143  friend class rs2::pipeline_profile;
144  friend class rs2::device_hub;
145 
146  std::shared_ptr<rs2_device> _dev;
147 
148  };
149 
150  template<class T>
152  {
153  T _callback;
154 
155  public:
156  explicit update_progress_callback(T callback) : _callback(callback) {}
157 
158  void on_update_progress(const float progress) override
159  {
160  _callback(progress);
161  }
162 
163  void release() override { delete this; }
164  };
165 
166  class updatable : public device
167  {
168  public:
169  updatable() : device() {}
171  : device(d.get())
172  {
173  rs2_error* e = nullptr;
174  if (rs2_is_device_extendable_to(_dev.get(), RS2_EXTENSION_UPDATABLE, &e) == 0 && !e)
175  {
176  _dev.reset();
177  }
178  error::handle(e);
179  }
180 
181  // Move the device to update state, this will cause the updatable device to disconnect and reconnect as an update device.
182  void enter_update_state() const
183  {
184  rs2_error* e = nullptr;
185  rs2_enter_update_state(_dev.get(), &e);
186  error::handle(e);
187  }
188 
189  // Create backup of camera flash memory. Such backup does not constitute valid firmware image, and cannot be
190  // loaded back to the device, but it does contain all calibration and device information."
191  std::vector<uint8_t> create_flash_backup() const
192  {
193  std::vector<uint8_t> results;
194 
195  rs2_error* e = nullptr;
196  std::shared_ptr<const rs2_raw_data_buffer> list(
197  rs2_create_flash_backup_cpp(_dev.get(), nullptr, &e),
199  error::handle(e);
200 
201  auto size = rs2_get_raw_data_size(list.get(), &e);
202  error::handle(e);
203 
204  auto start = rs2_get_raw_data(list.get(), &e);
205 
206  results.insert(results.begin(), start, start + size);
207 
208  return results;
209  }
210 
211  template<class T>
212  std::vector<uint8_t> create_flash_backup(T callback) const
213  {
214  std::vector<uint8_t> results;
215 
216  rs2_error* e = nullptr;
217  std::shared_ptr<const rs2_raw_data_buffer> list(
218  rs2_create_flash_backup_cpp(_dev.get(), new update_progress_callback<T>(std::move(callback)), &e),
220  error::handle(e);
221 
222  auto size = rs2_get_raw_data_size(list.get(), &e);
223  error::handle(e);
224 
225  auto start = rs2_get_raw_data(list.get(), &e);
226 
227  results.insert(results.begin(), start, start + size);
228 
229  return results;
230  }
231 
232  // Update an updatable device to the provided unsigned firmware. This call is executed on the caller's thread.
233  void update_unsigned(const std::vector<uint8_t>& image, int update_mode = RS2_UNSIGNED_UPDATE_MODE_UPDATE) const
234  {
235  rs2_error* e = nullptr;
236  rs2_update_firmware_unsigned_cpp(_dev.get(), image.data(), (int)image.size(), nullptr, update_mode, &e);
237  error::handle(e);
238  }
239 
240  // Update an updatable device to the provided unsigned firmware. This call is executed on the caller's thread and it supports progress notifications via the callback.
241  template<class T>
242  void update_unsigned(const std::vector<uint8_t>& image, T callback, int update_mode = RS2_UNSIGNED_UPDATE_MODE_UPDATE) const
243  {
244  rs2_error* e = nullptr;
245  rs2_update_firmware_unsigned_cpp(_dev.get(), image.data(), image.size(), new update_progress_callback<T>(std::move(callback)), update_mode, &e);
246  error::handle(e);
247  }
248  };
249 
250  class update_device : public device
251  {
252  public:
255  : device(d.get())
256  {
257  rs2_error* e = nullptr;
259  {
260  _dev.reset();
261  }
262  error::handle(e);
263  }
264 
265  // Update an updatable device to the provided firmware.
266  // This call is executed on the caller's thread.
267  void update(const std::vector<uint8_t>& fw_image) const
268  {
269  rs2_error* e = nullptr;
270  rs2_update_firmware_cpp(_dev.get(), fw_image.data(), (int)fw_image.size(), NULL, &e);
271  error::handle(e);
272  }
273 
274  // Update an updatable device to the provided firmware.
275  // This call is executed on the caller's thread and it supports progress notifications via the callback.
276  template<class T>
277  void update(const std::vector<uint8_t>& fw_image, T callback) const
278  {
279  rs2_error* e = nullptr;
280  rs2_update_firmware_cpp(_dev.get(), fw_image.data(), fw_image.size(), new update_progress_callback<T>(std::move(callback)), &e);
281  error::handle(e);
282  }
283  };
284 
285  class debug_protocol : public device
286  {
287  public:
289  : device(d.get())
290  {
291  rs2_error* e = nullptr;
292  if(rs2_is_device_extendable_to(_dev.get(), RS2_EXTENSION_DEBUG, &e) == 0 && !e)
293  {
294  _dev.reset();
295  }
296  error::handle(e);
297  }
298 
299  std::vector<uint8_t> send_and_receive_raw_data(const std::vector<uint8_t>& input) const
300  {
301  std::vector<uint8_t> results;
302 
303  rs2_error* e = nullptr;
304  std::shared_ptr<const rs2_raw_data_buffer> list(
305  rs2_send_and_receive_raw_data(_dev.get(), (void*)input.data(), (uint32_t)input.size(), &e),
307  error::handle(e);
308 
309  auto size = rs2_get_raw_data_size(list.get(), &e);
310  error::handle(e);
311 
312  auto start = rs2_get_raw_data(list.get(), &e);
313 
314  results.insert(results.begin(), start, start + size);
315 
316  return results;
317  }
318  };
319 
321  {
322  public:
323  explicit device_list(std::shared_ptr<rs2_device_list> list)
324  : _list(move(list)) {}
325 
327  : _list(nullptr) {}
328 
329  operator std::vector<device>() const
330  {
331  std::vector<device> res;
332  for (auto&& dev : *this) res.push_back(dev);
333  return res;
334  }
335 
336  bool contains(const device& dev) const
337  {
338  rs2_error* e = nullptr;
339  auto res = !!(rs2_device_list_contains(_list.get(), dev.get().get(), &e));
340  error::handle(e);
341  return res;
342  }
343 
344  device_list& operator=(std::shared_ptr<rs2_device_list> list)
345  {
346  _list = move(list);
347  return *this;
348  }
349 
350  device operator[](uint32_t index) const
351  {
352  rs2_error* e = nullptr;
353  std::shared_ptr<rs2_device> dev(
354  rs2_create_device(_list.get(), index, &e),
356  error::handle(e);
357 
358  return device(dev);
359  }
360 
361  uint32_t size() const
362  {
363  rs2_error* e = nullptr;
364  auto size = rs2_get_device_count(_list.get(), &e);
365  error::handle(e);
366  return size;
367  }
368 
369  device front() const { return std::move((*this)[0]); }
370  device back() const
371  {
372  return std::move((*this)[size() - 1]);
373  }
374 
376  {
378  const device_list& device_list,
379  uint32_t uint32_t)
380  : _list(device_list),
381  _index(uint32_t)
382  {
383  }
384 
385  public:
387  {
388  return _list[_index];
389  }
390  bool operator!=(const device_list_iterator& other) const
391  {
392  return other._index != _index || &other._list != &_list;
393  }
394  bool operator==(const device_list_iterator& other) const
395  {
396  return !(*this != other);
397  }
399  {
400  _index++;
401  return *this;
402  }
403  private:
404  friend device_list;
405  const device_list& _list;
406  uint32_t _index;
407  };
408 
410  {
411  return device_list_iterator(*this, 0);
412  }
414  {
415  return device_list_iterator(*this, size());
416  }
417  const rs2_device_list* get_list() const
418  {
419  return _list.get();
420  }
421 
422  operator std::shared_ptr<rs2_device_list>() { return _list; };
423 
424  private:
425  std::shared_ptr<rs2_device_list> _list;
426  };
427 
437  class tm2 : public device //TODO: add to wrappers
438  {
439  public:
441  : device(d.get())
442  {
443  rs2_error* e = nullptr;
444  if (rs2_is_device_extendable_to(_dev.get(), RS2_EXTENSION_TM2, &e) == 0 && !e)
445  {
446  _dev.reset();
447  }
448  error::handle(e);
449  }
450 
455  void enable_loopback(const std::string& from_file)
456  {
457  rs2_error* e = nullptr;
458  rs2_loopback_enable(_dev.get(), from_file.c_str(), &e);
459  error::handle(e);
460  }
461 
466  {
467  rs2_error* e = nullptr;
468  rs2_loopback_disable(_dev.get(), &e);
469  error::handle(e);
470  }
471 
476  bool is_loopback_enabled() const
477  {
478  rs2_error* e = nullptr;
479  int is_enabled = rs2_loopback_is_enabled(_dev.get(), &e);
480  error::handle(e);
481  return is_enabled != 0;
482  }
483 
488  void connect_controller(const std::array<uint8_t, 6>& mac_addr)
489  {
490  rs2_error* e = nullptr;
491  rs2_connect_tm2_controller(_dev.get(), mac_addr.data(), &e);
492  error::handle(e);
493  }
494 
500  {
501  rs2_error* e = nullptr;
502  rs2_disconnect_tm2_controller(_dev.get(), id, &e);
503  error::handle(e);
504  }
505 
511  void set_intrinsics(int fisheye_sensor_id, const rs2_intrinsics& intrinsics)
512  {
513  rs2_error* e = nullptr;
514  auto fisheye_sensor = get_sensor_profile(RS2_STREAM_FISHEYE, fisheye_sensor_id);
515  rs2_set_intrinsics(fisheye_sensor.first.get().get(), fisheye_sensor.second.get(), &intrinsics, &e);
516  error::handle(e);
517  }
518 
527  void set_extrinsics(rs2_stream from_stream, int from_id, rs2_stream to_stream, int to_id, rs2_extrinsics& extrinsics)
528  {
529  rs2_error* e = nullptr;
530  auto from_sensor = get_sensor_profile(from_stream, from_id);
531  auto to_sensor = get_sensor_profile(to_stream, to_id);
532  rs2_set_extrinsics(from_sensor.first.get().get(), from_sensor.second.get(), to_sensor.first.get().get(), to_sensor.second.get(), &extrinsics, &e);
533  error::handle(e);
534  }
535 
541  void set_motion_device_intrinsics(rs2_stream stream_type, const rs2_motion_device_intrinsic& motion_intriniscs)
542  {
543  rs2_error* e = nullptr;
544  auto motion_sensor = get_sensor_profile(stream_type, 0);
545  rs2_set_motion_device_intrinsics(motion_sensor.first.get().get(), motion_sensor.second.get(), &motion_intriniscs, &e);
546  error::handle(e);
547  }
548 
555  {
556  rs2_error* e = nullptr;
558  error::handle(e);
559  }
560 
567  {
568  rs2_error* e = nullptr;
569  rs2_write_calibration(_dev.get(), &e);
570  error::handle(e);
571  }
572 
573  private:
574 
575  std::pair<sensor, stream_profile> get_sensor_profile(rs2_stream stream_type, int stream_index) {
576  for (auto s : query_sensors()) {
577  for (auto p : s.get_stream_profiles()) {
578  if (p.stream_type() == stream_type && p.stream_index() == stream_index)
579  return std::pair<sensor, stream_profile>(s, p);
580  }
581  }
582  return std::pair<sensor, stream_profile>();
583  }
584  };
585 }
586 #endif // LIBREALSENSE_RS2_DEVICE_HPP
Definition: rs_types.hpp:76
device operator*() const
Definition: rs_device.hpp:386
device back() const
Definition: rs_device.hpp:370
int rs2_get_sensors_count(const rs2_sensor_list *info_list, rs2_error **error)
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
Definition: rs_sensor.hpp:103
update_device()
Definition: rs_device.hpp:253
bool is() const
Definition: rs_device.hpp:122
int rs2_device_list_contains(const rs2_device_list *info_list, const rs2_device *device, rs2_error **error)
int rs2_loopback_is_enabled(const rs2_device *device, rs2_error **error)
device_list(std::shared_ptr< rs2_device_list > list)
Definition: rs_device.hpp:323
std::vector< uint8_t > send_and_receive_raw_data(const std::vector< uint8_t > &input) const
Definition: rs_device.hpp:299
std::vector< uint8_t > create_flash_backup() const
Definition: rs_device.hpp:191
void release() override
Definition: rs_device.hpp:163
std::vector< sensor > query_sensors() const
Definition: rs_device.hpp:25
device()
Definition: rs_device.hpp:110
const char * get_info(rs2_camera_info info) const
Definition: rs_device.hpp:79
device & operator=(const device &dev)
Definition: rs_device.hpp:104
Definition: rs_pipeline.hpp:18
bool operator==(const device_list_iterator &other) const
Definition: rs_device.hpp:394
void update_unsigned(const std::vector< uint8_t > &image, int update_mode=RS2_UNSIGNED_UPDATE_MODE_UPDATE) const
Definition: rs_device.hpp:233
void update(const std::vector< uint8_t > &fw_image) const
Definition: rs_device.hpp:267
void rs2_set_intrinsics(const rs2_sensor *sensor, const rs2_stream_profile *profile, const rs2_intrinsics *intrinsics, rs2_error **error)
rs2_sensor * rs2_create_sensor(const rs2_sensor_list *list, int index, rs2_error **error)
void set_intrinsics(int fisheye_sensor_id, const rs2_intrinsics &intrinsics)
Definition: rs_device.hpp:511
void update_unsigned(const std::vector< uint8_t > &image, T callback, int update_mode=RS2_UNSIGNED_UPDATE_MODE_UPDATE) const
Definition: rs_device.hpp:242
void rs2_delete_device(rs2_device *device)
void connect_controller(const std::array< uint8_t, 6 > &mac_addr)
Definition: rs_device.hpp:488
Definition: rs_sensor.h:47
const unsigned char * rs2_get_raw_data(const rs2_raw_data_buffer *buffer, rs2_error **error)
void rs2_write_calibration(const rs2_device *device, rs2_error **e)
void disconnect_controller(int id)
Definition: rs_device.hpp:499
rs2_device * rs2_create_device(const rs2_device_list *info_list, int index, rs2_error **error)
device_list & operator=(std::shared_ptr< rs2_device_list > list)
Definition: rs_device.hpp:344
Definition: rs_device.hpp:151
Definition: rs_context.hpp:11
void rs2_delete_raw_data(const rs2_raw_data_buffer *buffer)
Definition: rs_context.hpp:96
int rs2_get_raw_data_size(const rs2_raw_data_buffer *buffer, rs2_error **error)
Definition: rs_device.hpp:166
T as() const
Definition: rs_device.hpp:129
void enter_update_state() const
Definition: rs_device.hpp:182
void rs2_connect_tm2_controller(const rs2_device *device, const unsigned char *mac_addr, rs2_error **error)
device_list_iterator begin() const
Definition: rs_device.hpp:409
void rs2_loopback_enable(const rs2_device *device, const char *from_file, rs2_error **error)
void rs2_disconnect_tm2_controller(const rs2_device *device, int id, rs2_error **error)
Definition: rs_device.hpp:285
void set_motion_device_intrinsics(rs2_stream stream_type, const rs2_motion_device_intrinsic &motion_intriniscs)
Definition: rs_device.hpp:541
bool is_loopback_enabled() const
Definition: rs_device.hpp:476
device operator[](uint32_t index) const
Definition: rs_device.hpp:350
int rs2_supports_device_info(const rs2_device *device, rs2_camera_info info, rs2_error **error)
void reset_to_factory_calibration()
Definition: rs_device.hpp:554
#define RS2_UNSIGNED_UPDATE_MODE_UPDATE
Definition: rs_device.h:206
void rs2_delete_sensor(rs2_sensor *sensor)
int rs2_is_device_extendable_to(const rs2_device *device, rs2_extension extension, rs2_error **error)
device front() const
Definition: rs_device.hpp:369
uint32_t size() const
Definition: rs_device.hpp:361
void hardware_reset()
Definition: rs_device.hpp:90
void rs2_set_extrinsics(const rs2_sensor *from_sensor, const rs2_stream_profile *from_profile, rs2_sensor *to_sensor, const rs2_stream_profile *to_profile, const rs2_extrinsics *extrinsics, rs2_error **error)
const std::shared_ptr< rs2_device > & get() const
Definition: rs_device.hpp:116
void update(const std::vector< uint8_t > &fw_image, T callback) const
Definition: rs_device.hpp:277
std::shared_ptr< rs2_device > _dev
Definition: rs_device.hpp:146
std::vector< uint8_t > create_flash_backup(T callback) const
Definition: rs_device.hpp:212
void rs2_hardware_reset(const rs2_device *device, rs2_error **error)
Definition: rs_types.h:172
rs2_sensor_list * rs2_query_sensors(const rs2_device *device, rs2_error **error)
void rs2_update_firmware_cpp(const rs2_device *device, const void *fw_image, int fw_image_size, rs2_update_progress_callback *callback, rs2_error **error)
update_device(device d)
Definition: rs_device.hpp:254
void set_extrinsics(rs2_stream from_stream, int from_id, rs2_stream to_stream, int to_id, rs2_extrinsics &extrinsics)
Definition: rs_device.hpp:527
void write_calibration()
Definition: rs_device.hpp:566
static void handle(rs2_error *e)
Definition: rs_types.hpp:128
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:41
void rs2_reset_to_factory_calibration(const rs2_device *device, rs2_error **e)
void rs2_set_motion_device_intrinsics(const rs2_sensor *sensor, const rs2_stream_profile *profile, const rs2_motion_device_intrinsic *intrinsics, rs2_error **error)
Definition: rs_types.h:157
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented.
Definition: rs_sensor.h:88
const rs2_raw_data_buffer * rs2_create_flash_backup_cpp(const rs2_device *device, rs2_update_progress_callback *callback, rs2_error **error)
bool contains(const device &dev) const
Definition: rs_device.hpp:336
const rs2_raw_data_buffer * rs2_send_and_receive_raw_data(rs2_device *device, void *raw_data_to_send, unsigned size_of_raw_data_to_send, rs2_error **error)
Definition: rs_types.hpp:67
Definition: rs_types.h:136
bool supports(rs2_camera_info info) const
Definition: rs_device.hpp:66
update_progress_callback(T callback)
Definition: rs_device.hpp:156
device & operator=(const std::shared_ptr< rs2_device > dev)
Definition: rs_device.hpp:98
device_list_iterator & operator++()
Definition: rs_device.hpp:398
device_list()
Definition: rs_device.hpp:326
updatable()
Definition: rs_device.hpp:169
Definition: rs_device.hpp:320
updatable(device d)
Definition: rs_device.hpp:170
void rs2_enter_update_state(const rs2_device *device, rs2_error **error)
Definition: rs_context.hpp:224
bool operator!=(const device_list_iterator &other) const
Definition: rs_device.hpp:390
Video stream intrinsics.
Definition: rs_types.h:58
Definition: rs_types.h:173
void rs2_update_firmware_unsigned_cpp(const rs2_device *device, const void *fw_image, int fw_image_size, rs2_update_progress_callback *callback, int update_mode, rs2_error **error)
Motion device intrinsics: scale, bias, and variances.
Definition: rs_types.h:71
void rs2_delete_sensor_list(rs2_sensor_list *info_list)
void enable_loopback(const std::string &from_file)
Definition: rs_device.hpp:455
void on_update_progress(const float progress) override
Definition: rs_device.hpp:158
struct rs2_device_list rs2_device_list
Definition: rs_types.h:217
debug_protocol(device d)
Definition: rs_device.hpp:288
void disable_loopback()
Definition: rs_device.hpp:465
tm2(device d)
Definition: rs_device.hpp:440
device_list_iterator end() const
Definition: rs_device.hpp:413
Definition: rs_device.hpp:437
struct rs2_error rs2_error
Definition: rs_types.h:210
Definition: rs_device.hpp:375
void rs2_loopback_disable(const rs2_device *device, rs2_error **error)
Definition: rs_device.hpp:18
int rs2_get_device_count(const rs2_device_list *info_list, rs2_error **error)
Definition: rs_device.hpp:250
const rs2_device_list * get_list() const
Definition: rs_device.hpp:417
T first() const
Definition: rs_device.hpp:52
virtual ~device()
Definition: rs_device.hpp:134
device(std::shared_ptr< rs2_device > dev)
Definition: rs_device.hpp:139
const char * rs2_get_device_info(const rs2_device *device, rs2_camera_info info, rs2_error **error)