Intel® RealSense™ Cross Platform API
Intel Realsense Cross-platform API
rs_sensor.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_SENSOR_HPP
5 #define LIBREALSENSE_RS2_SENSOR_HPP
6 
7 #include "rs_types.hpp"
8 #include "rs_frame.hpp"
9 #include "rs_processing.hpp"
10 #include "rs_options.hpp"
11 namespace rs2
12 {
13 
15  {
16  public:
18  {
19  rs2_error* e = nullptr;
20  _description = rs2_get_notification_description(nt, &e);
21  error::handle(e);
22  _timestamp = rs2_get_notification_timestamp(nt, &e);
23  error::handle(e);
24  _severity = rs2_get_notification_severity(nt, &e);
25  error::handle(e);
26  _category = rs2_get_notification_category(nt, &e);
27  error::handle(e);
28  _serialized_data = rs2_get_notification_serialized_data(nt, &e);
29  error::handle(e);
30  }
31 
32  notification() = default;
33 
39  {
40  return _category;
41  }
46  std::string get_description() const
47  {
48  return _description;
49  }
50 
55  double get_timestamp() const
56  {
57  return _timestamp;
58  }
59 
65  {
66  return _severity;
67  }
68 
73  std::string get_serialized_data() const
74  {
75  return _serialized_data;
76  }
77 
78  private:
79  std::string _description;
80  double _timestamp = -1;
83  std::string _serialized_data;
84  };
85 
86  template<class T>
88  {
89  T on_notification_function;
90  public:
91  explicit notifications_callback(T on_notification) : on_notification_function(on_notification) {}
92 
93  void on_notification(rs2_notification* _notification) override
94  {
95  on_notification_function(notification{ _notification });
96  }
97 
98  void release() override { delete this; }
99  };
100 
101 
102 
103  class sensor : public options
104  {
105  public:
106 
107  using options::supports;
112  void open(const stream_profile& profile) const
113  {
114  rs2_error* e = nullptr;
115  rs2_open(_sensor.get(),
116  profile.get(),
117  &e);
118  error::handle(e);
119  }
120 
126  bool supports(rs2_camera_info info) const
127  {
128  rs2_error* e = nullptr;
129  auto is_supported = rs2_supports_sensor_info(_sensor.get(), info, &e);
130  error::handle(e);
131  return is_supported > 0;
132  }
133 
139  const char* get_info(rs2_camera_info info) const
140  {
141  rs2_error* e = nullptr;
142  auto result = rs2_get_sensor_info(_sensor.get(), info, &e);
143  error::handle(e);
144  return result;
145  }
146 
152  void open(const std::vector<stream_profile>& profiles) const
153  {
154  rs2_error* e = nullptr;
155 
156  std::vector<const rs2_stream_profile*> profs;
157  profs.reserve(profiles.size());
158  for (auto& p : profiles)
159  {
160  profs.push_back(p.get());
161  }
162 
164  profs.data(),
165  static_cast<int>(profiles.size()),
166  &e);
167  error::handle(e);
168  }
169 
174  void close() const
175  {
176  rs2_error* e = nullptr;
177  rs2_close(_sensor.get(), &e);
178  error::handle(e);
179  }
180 
185  template<class T>
186  void start(T callback) const
187  {
188  rs2_error* e = nullptr;
189  rs2_start_cpp(_sensor.get(), new frame_callback<T>(std::move(callback)), &e);
190  error::handle(e);
191  }
192 
196  void stop() const
197  {
198  rs2_error* e = nullptr;
199  rs2_stop(_sensor.get(), &e);
200  error::handle(e);
201  }
202 
207  template<class T>
208  void set_notifications_callback(T callback) const
209  {
210  rs2_error* e = nullptr;
212  new notifications_callback<T>(std::move(callback)), &e);
213  error::handle(e);
214  }
215 
216 
221  std::vector<stream_profile> get_stream_profiles() const
222  {
223  std::vector<stream_profile> results{};
224 
225  rs2_error* e = nullptr;
226  std::shared_ptr<rs2_stream_profile_list> list(
227  rs2_get_stream_profiles(_sensor.get(), &e),
229  error::handle(e);
230 
231  auto size = rs2_get_stream_profiles_count(list.get(), &e);
232  error::handle(e);
233 
234  for (auto i = 0; i < size; i++)
235  {
236  stream_profile profile(rs2_get_stream_profile(list.get(), i, &e));
237  error::handle(e);
238  results.push_back(profile);
239  }
240 
241  return results;
242  }
243 
248  std::vector<filter> get_recommended_filters() const
249  {
250  std::vector<filter> results{};
251 
252  rs2_error* e = nullptr;
253  std::shared_ptr<rs2_processing_block_list> list(
256  error::handle(e);
257 
258  auto size = rs2_get_recommended_processing_blocks_count(list.get(), &e);
259  error::handle(e);
260 
261  for (auto i = 0; i < size; i++)
262  {
263  auto f = std::shared_ptr<rs2_processing_block>(
264  rs2_get_processing_block(list.get(), i, &e),
266  error::handle(e);
267  results.push_back(f);
268  }
269 
270  return results;
271  }
272 
273  sensor& operator=(const std::shared_ptr<rs2_sensor> other)
274  {
275  options::operator=(other);
276  _sensor.reset();
277  _sensor = other;
278  return *this;
279  }
280 
281  sensor& operator=(const sensor& other)
282  {
283  *this = nullptr;
285  _sensor = other._sensor;
286  return *this;
287  }
288  sensor() : _sensor(nullptr) {}
289 
290  operator bool() const
291  {
292  return _sensor != nullptr;
293  }
294 
295  const std::shared_ptr<rs2_sensor>& get() const
296  {
297  return _sensor;
298  }
299 
300  template<class T>
301  bool is() const
302  {
303  T extension(*this);
304  return extension;
305  }
306 
307  template<class T>
308  T as() const
309  {
310  T extension(*this);
311  return extension;
312  }
313 
314  explicit sensor(std::shared_ptr<rs2_sensor> dev)
315  :options((rs2_options*)dev.get()), _sensor(dev)
316  {
317  }
318  explicit operator std::shared_ptr<rs2_sensor>() { return _sensor; }
319 
320  protected:
321  friend context;
322  friend device_list;
323  friend device;
324  friend device_base;
325  friend roi_sensor;
326 
327  std::shared_ptr<rs2_sensor> _sensor;
328 
329 
330  };
331 
332  inline std::shared_ptr<sensor> sensor_from_frame(frame f)
333  {
334  std::shared_ptr<rs2_sensor> psens(f.get_sensor(), rs2_delete_sensor);
335  return std::make_shared<sensor>(psens);
336  }
337 
338  inline bool operator==(const sensor& lhs, const sensor& rhs)
339  {
342  return false;
343 
344  return std::string(lhs.get_info(RS2_CAMERA_INFO_NAME)) == rhs.get_info(RS2_CAMERA_INFO_NAME)
346  }
347 
348  class roi_sensor : public sensor
349  {
350  public:
352  : sensor(s.get())
353  {
354  rs2_error* e = nullptr;
355  if(rs2_is_sensor_extendable_to(_sensor.get(), RS2_EXTENSION_ROI, &e) == 0 && !e)
356  {
357  _sensor.reset();
358  }
359  error::handle(e);
360  }
361 
363  {
364  rs2_error* e = nullptr;
365  rs2_set_region_of_interest(_sensor.get(), roi.min_x, roi.min_y, roi.max_x, roi.max_y, &e);
366  error::handle(e);
367  }
368 
370  {
371  region_of_interest roi {};
372  rs2_error* e = nullptr;
373  rs2_get_region_of_interest(_sensor.get(), &roi.min_x, &roi.min_y, &roi.max_x, &roi.max_y, &e);
374  error::handle(e);
375  return roi;
376  }
377 
378  operator bool() const { return _sensor.get() != nullptr; }
379  };
380 
381  class depth_sensor : public sensor
382  {
383  public:
385  : sensor(s.get())
386  {
387  rs2_error* e = nullptr;
389  {
390  _sensor.reset();
391  }
392  error::handle(e);
393  }
394 
398  float get_depth_scale() const
399  {
400  rs2_error* e = nullptr;
401  auto res = rs2_get_depth_scale(_sensor.get(), &e);
402  error::handle(e);
403  return res;
404  }
405 
406  operator bool() const { return _sensor.get() != nullptr; }
407  explicit depth_sensor(std::shared_ptr<rs2_sensor> dev) : depth_sensor(sensor(dev)) {}
408  };
409 
411  {
412  public:
414  {
415  rs2_error* e = nullptr;
417  {
418  _sensor.reset();
419  }
420  error::handle(e);
421  }
422 
426  float get_stereo_baseline() const
427  {
428  rs2_error* e = nullptr;
429  auto res = rs2_get_stereo_baseline(_sensor.get(), &e);
430  error::handle(e);
431  return res;
432  }
433 
434  operator bool() const { return _sensor.get() != nullptr; }
435  };
436 
437 
438  class pose_sensor : public sensor
439  {
440  public:
442  : sensor(s.get())
443  {
444  rs2_error* e = nullptr;
446  {
447  _sensor.reset();
448  }
449  error::handle(e);
450  }
451 
456  bool import_localization_map(const std::vector<uint8_t>& lmap_buf) const
457  {
458  rs2_error* e = nullptr;
459  auto res = rs2_import_localization_map(_sensor.get(), lmap_buf.data(), uint32_t(lmap_buf.size()), &e);
460  error::handle(e);
461  return !!res;
462  }
463 
467  std::vector<uint8_t> export_localization_map() const
468  {
469  rs2_error* e = nullptr;
470  std::shared_ptr<const rs2_raw_data_buffer> loc_map(
473  error::handle(e);
474 
475  auto start = rs2_get_raw_data(loc_map.get(), &e);
476  error::handle(e);
477 
478  std::vector<uint8_t> results;
479  if (start)
480  {
481  auto size = rs2_get_raw_data_size(loc_map.get(), &e);
482  error::handle(e);
483 
484  results = std::vector<uint8_t>(start, start + size);
485  }
486  return results;
487  }
488 
495  bool set_static_node(const std::string& guid, const rs2_vector& pos, const rs2_quaternion& orient) const
496  {
497  rs2_error* e = nullptr;
498  auto res = rs2_set_static_node(_sensor.get(), guid.c_str(), pos, orient, &e);
499  error::handle(e);
500  return !!res;
501  }
502 
503 
510  bool get_static_node(const std::string& guid, rs2_vector& pos, rs2_quaternion& orient) const
511  {
512  rs2_error* e = nullptr;
513  auto res = rs2_get_static_node(_sensor.get(), guid.c_str(), &pos, &orient, &e);
514  error::handle(e);
515  return !!res;
516  }
517 
518  operator bool() const { return _sensor.get() != nullptr; }
519  explicit pose_sensor(std::shared_ptr<rs2_sensor> dev) : pose_sensor(sensor(dev)) {}
520  };
521 
522  class wheel_odometer : public sensor
523  {
524  public:
526  : sensor(s.get())
527  {
528  rs2_error* e = nullptr;
530  {
531  _sensor.reset();
532  }
533  error::handle(e);
534  }
535 
540  bool load_wheel_odometery_config(const std::vector<uint8_t>& odometry_config_buf) const
541  {
542  rs2_error* e = nullptr;
543  auto res = rs2_load_wheel_odometry_config(_sensor.get(), odometry_config_buf.data(), uint32_t(odometry_config_buf.size()), &e);
544  error::handle(e);
545  return !!res;
546  }
547 
554  bool send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const rs2_vector& translational_velocity)
555  {
556  rs2_error* e = nullptr;
557  auto res = rs2_send_wheel_odometry(_sensor.get(), wo_sensor_id, frame_num, translational_velocity, &e);
558  error::handle(e);
559  return !!res;
560  }
561 
562  operator bool() const { return _sensor.get() != nullptr; }
563  explicit wheel_odometer(std::shared_ptr<rs2_sensor> dev) : wheel_odometer(sensor(dev)) {}
564  };
565 }
566 #endif // LIBREALSENSE_RS2_SENSOR_HPP
notification(rs2_notification *nt)
Definition: rs_sensor.hpp:17
Definition: rs_frame.hpp:22
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
pose_sensor(std::shared_ptr< rs2_sensor > dev)
Definition: rs_sensor.hpp:519
Definition: rs_sensor.hpp:103
bool operator==(const sensor &lhs, const sensor &rhs)
Definition: rs_sensor.hpp:338
Definition: rs_frame.hpp:336
Definition: rs_types.hpp:168
float rs2_get_depth_scale(rs2_sensor *sensor, rs2_error **error)
bool supports(rs2_option option) const
Definition: rs_options.hpp:19
friend device
Definition: rs_sensor.hpp:323
void stop() const
Definition: rs_sensor.hpp:196
std::vector< stream_profile > get_stream_profiles() const
Definition: rs_sensor.hpp:221
region_of_interest get_region_of_interest() const
Definition: rs_sensor.hpp:369
void on_notification(rs2_notification *_notification) override
Definition: rs_sensor.hpp:93
Definition: rs_types.hpp:39
Definition: rs_sensor.hpp:381
void start(T callback) const
Definition: rs_sensor.hpp:186
int min_x
Definition: rs_types.hpp:170
Definition: rs_sensor.h:24
int rs2_get_static_node(const rs2_sensor *sensor, const char *guid, rs2_vector *pos, rs2_quaternion *orient, rs2_error **error)
void rs2_set_region_of_interest(const rs2_sensor *sensor, int min_x, int min_y, int max_x, int max_y, rs2_error **error)
sets the active region of interest to be used by auto-exposure algorithm
depth_sensor(sensor s)
Definition: rs_sensor.hpp:384
const unsigned char * rs2_get_raw_data(const rs2_raw_data_buffer *buffer, rs2_error **error)
int rs2_import_localization_map(const rs2_sensor *sensor, const unsigned char *lmap_blob, unsigned int blob_size, rs2_error **error)
friend context
Definition: rs_sensor.hpp:321
void rs2_stop(const rs2_sensor *sensor, rs2_error **error)
std::vector< filter > get_recommended_filters() const
Definition: rs_sensor.hpp:248
depth_stereo_sensor(sensor s)
Definition: rs_sensor.hpp:413
std::vector< uint8_t > export_localization_map() const
Definition: rs_sensor.hpp:467
void release() override
Definition: rs_sensor.hpp:98
const char * rs2_get_notification_serialized_data(rs2_notification *notification, rs2_error **error)
Definition: rs_context.hpp:11
bool supports(rs2_camera_info info) const
Definition: rs_sensor.hpp:126
Definition: rs_sensor.h:23
rs2_log_severity rs2_get_notification_severity(rs2_notification *notification, rs2_error **error)
bool get_static_node(const std::string &guid, rs2_vector &pos, rs2_quaternion &orient) const
Definition: rs_sensor.hpp:510
notification()=default
void rs2_delete_raw_data(const rs2_raw_data_buffer *buffer)
Definition: rs_types.h:141
int max_y
Definition: rs_types.hpp:173
sensor(std::shared_ptr< rs2_sensor > dev)
Definition: rs_sensor.hpp:314
friend roi_sensor
Definition: rs_sensor.hpp:325
void set_region_of_interest(const region_of_interest &roi)
Definition: rs_sensor.hpp:362
int max_x
Definition: rs_types.hpp:172
rs2_notification_category get_category() const
Definition: rs_sensor.hpp:38
int rs2_get_raw_data_size(const rs2_raw_data_buffer *buffer, rs2_error **error)
void rs2_open(rs2_sensor *device, const rs2_stream_profile *profile, rs2_error **error)
notifications_callback(T on_notification)
Definition: rs_sensor.hpp:91
bool load_wheel_odometery_config(const std::vector< uint8_t > &odometry_config_buf) const
Definition: rs_sensor.hpp:540
Quaternion used to represent rotation.
Definition: rs_types.h:102
Definition: rs_frame.hpp:1123
double get_timestamp() const
Definition: rs_sensor.hpp:55
void open(const stream_profile &profile) const
Definition: rs_sensor.hpp:112
void set_notifications_callback(T callback) const
Definition: rs_sensor.hpp:208
struct rs2_notification rs2_notification
Definition: rs_types.h:236
std::shared_ptr< rs2_sensor > _sensor
Definition: rs_sensor.hpp:327
Definition: rs_types.h:140
Definition: rs_sensor.hpp:14
float rs2_get_stereo_baseline(rs2_sensor *sensor, rs2_error **error)
friend device_base
Definition: rs_sensor.hpp:324
Definition: rs_types.h:151
int rs2_get_recommended_processing_blocks_count(const rs2_processing_block_list *list, rs2_error **error)
void rs2_delete_sensor(rs2_sensor *sensor)
void rs2_get_region_of_interest(const rs2_sensor *sensor, int *min_x, int *min_y, int *max_x, int *max_y, rs2_error **error)
gets the active region of interest to be used by auto-exposure algorithm
rs2_time_t rs2_get_notification_timestamp(rs2_notification *notification, rs2_error **error)
sensor()
Definition: rs_sensor.hpp:288
depth_sensor(std::shared_ptr< rs2_sensor > dev)
Definition: rs_sensor.hpp:407
const rs2_stream_profile * rs2_get_stream_profile(const rs2_stream_profile_list *list, int index, rs2_error **error)
void rs2_set_notifications_callback_cpp(const rs2_sensor *sensor, rs2_notifications_callback *callback, rs2_error **error)
Definition: rs_options.hpp:11
T as() const
Definition: rs_sensor.hpp:308
pose_sensor(sensor s)
Definition: rs_sensor.hpp:441
void rs2_delete_processing_block(rs2_processing_block *block)
int rs2_supports_sensor_info(const rs2_sensor *sensor, rs2_camera_info info, rs2_error **error)
struct rs2_options rs2_options
Definition: rs_types.h:233
void rs2_delete_recommended_processing_blocks(rs2_processing_block_list *list)
bool import_localization_map(const std::vector< uint8_t > &lmap_buf) const
Definition: rs_sensor.hpp:456
Definition: rs_sensor.hpp:87
static void handle(rs2_error *e)
Definition: rs_types.hpp:128
rs2_sensor * get_sensor()
Definition: rs_frame.hpp:438
std::string get_serialized_data() const
Definition: rs_sensor.hpp:73
bool is() const
Definition: rs_sensor.hpp:301
void close() const
Definition: rs_sensor.hpp:174
const rs2_stream_profile * get() const
Definition: rs_frame.hpp:137
Definition: rs_types.h:127
void open(const std::vector< stream_profile > &profiles) const
Definition: rs_sensor.hpp:152
wheel_odometer(sensor s)
Definition: rs_sensor.hpp:525
float get_stereo_baseline() const
Definition: rs_sensor.hpp:426
Definition: rs_types.h:24
int rs2_is_sensor_extendable_to(const rs2_sensor *sensor, rs2_extension extension, rs2_error **error)
3D vector in Euclidean coordinate space
Definition: rs_types.h:96
Definition: rs_types.h:169
rs2_log_severity get_severity() const
Definition: rs_sensor.hpp:64
rs2_notification_category
Category of the librealsense notification.
Definition: rs_types.h:17
void rs2_delete_stream_profiles_list(rs2_stream_profile_list *list)
void rs2_close(const rs2_sensor *sensor, rs2_error **error)
bool set_static_node(const std::string &guid, const rs2_vector &pos, const rs2_quaternion &orient) const
Definition: rs_sensor.hpp:495
rs2_stream_profile_list * rs2_get_stream_profiles(rs2_sensor *device, rs2_error **error)
options & operator=(const options &other)
Definition: rs_options.hpp:135
Definition: rs_sensor.hpp:522
void rs2_open_multiple(rs2_sensor *device, const rs2_stream_profile **profiles, int count, rs2_error **error)
void rs2_start_cpp(const rs2_sensor *sensor, rs2_frame_callback *callback, rs2_error **error)
Definition: rs_sensor.hpp:348
friend device_list
Definition: rs_sensor.hpp:322
rs2_notification_category rs2_get_notification_category(rs2_notification *notification, rs2_error **error)
rs2_processing_block * rs2_get_processing_block(const rs2_processing_block_list *list, int index, rs2_error **error)
bool send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const rs2_vector &translational_velocity)
Definition: rs_sensor.hpp:554
int rs2_set_static_node(const rs2_sensor *sensor, const char *guid, const rs2_vector pos, const rs2_quaternion orient, rs2_error **error)
Definition: rs_types.h:168
const rs2_raw_data_buffer * rs2_export_localization_map(const rs2_sensor *sensor, rs2_error **error)
std::string get_description() const
Definition: rs_sensor.hpp:46
float get_depth_scale() const
Definition: rs_sensor.hpp:398
const char * rs2_get_sensor_info(const rs2_sensor *sensor, rs2_camera_info info, rs2_error **error)
sensor & operator=(const sensor &other)
Definition: rs_sensor.hpp:281
const char * rs2_get_notification_description(rs2_notification *notification, rs2_error **error)
int rs2_get_stream_profiles_count(const rs2_stream_profile_list *list, rs2_error **error)
const std::shared_ptr< rs2_sensor > & get() const
Definition: rs_sensor.hpp:295
struct rs2_error rs2_error
Definition: rs_types.h:209
int rs2_load_wheel_odometry_config(const rs2_sensor *sensor, const unsigned char *odometry_config_buf, unsigned int blob_size, rs2_error **error)
rs2_log_severity
Severity of the librealsense logger.
Definition: rs_types.h:120
Definition: rs_sensor.hpp:410
rs2_processing_block_list * rs2_get_recommended_processing_blocks(rs2_sensor *sensor, rs2_error **error)
int rs2_send_wheel_odometry(const rs2_sensor *sensor, char wo_sensor_id, unsigned int frame_num, const rs2_vector translational_velocity, rs2_error **error)
Definition: rs_sensor.hpp:438
sensor & operator=(const std::shared_ptr< rs2_sensor > other)
Definition: rs_sensor.hpp:273
const char * get_info(rs2_camera_info info) const
Definition: rs_sensor.hpp:139
wheel_odometer(std::shared_ptr< rs2_sensor > dev)
Definition: rs_sensor.hpp:563
std::shared_ptr< sensor > sensor_from_frame(frame f)
Definition: rs_sensor.hpp:332
int min_y
Definition: rs_types.hpp:171
roi_sensor(sensor s)
Definition: rs_sensor.hpp:351