device.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 
4 #include "environment.h"
5 #include "core/video.h"
6 #include "core/motion.h"
7 #include "device.h"
8 
9 using namespace librealsense;
10 
11 std::shared_ptr<matcher> matcher_factory::create(rs2_matchers matcher, std::vector<stream_interface*> profiles)
12 {
13  switch (matcher)
14  {
15  case RS2_MATCHER_DI:
16  return create_DI_matcher(profiles);
17  case RS2_MATCHER_DI_C:
18  return create_DI_C_matcher(profiles);
19  case RS2_MATCHER_DLR_C:
20  return create_DLR_C_matcher(profiles);
21  case RS2_MATCHER_DLR:
22  return create_DLR_matcher(profiles);
23  case RS2_MATCHER_DEFAULT:default:
24  LOG_DEBUG("Created default matcher");
25  return create_timestamp_matcher(profiles);
26  break;
27  }
28 }
30 {
31  auto prof = std::find_if(profiles.begin(), profiles.end(), [&](stream_interface* profile)
32  {
33  return profile->get_stream_type() == stream && profile->get_stream_index() == index;
34  });
35 
36  if (prof != profiles.end())
37  return *prof;
38  else
39  return nullptr;
40 }
41 
42 std::shared_ptr<matcher> matcher_factory::create_DLR_C_matcher(std::vector<stream_interface*> profiles)
43 {
44  auto color = find_profile(RS2_STREAM_COLOR, 0, profiles);
45  if (!color)
46  {
47  LOG_DEBUG("Created default matcher");
48  return create_timestamp_matcher(profiles);
49  }
50 
53 }
54 
55 std::shared_ptr<matcher> matcher_factory::create_DI_C_matcher(std::vector<stream_interface*> profiles)
56 {
57  auto color = find_profile(RS2_STREAM_COLOR, 0, profiles);
58  if (!color)
59  {
60  LOG_DEBUG("Created default matcher");
61  return create_timestamp_matcher(profiles);
62  }
63 
65  create_identity_matcher(profiles[2]) });
66 }
67 
68 std::shared_ptr<matcher> matcher_factory::create_DLR_matcher(std::vector<stream_interface*> profiles)
69 {
70  auto depth = find_profile(RS2_STREAM_DEPTH, 0, profiles);
71  auto left = find_profile(RS2_STREAM_INFRARED, 1, profiles);
72  auto right = find_profile(RS2_STREAM_INFRARED, 2, profiles);
73 
74  if (!depth || !left || !right)
75  {
76  LOG_DEBUG("Created default matcher");
77  return create_timestamp_matcher(profiles);
78  }
80 }
81 
82 std::shared_ptr<matcher> matcher_factory::create_DI_matcher(std::vector<stream_interface*> profiles)
83 {
84  auto depth = find_profile(RS2_STREAM_DEPTH, 0, profiles);
85  auto ir = find_profile(RS2_STREAM_INFRARED, 1, profiles);
86 
87  if (!depth || !ir)
88  {
89  LOG_DEBUG("Created default matcher");
90  return create_timestamp_matcher(profiles);
91  }
92  return create_frame_number_matcher({ depth , ir });
93 }
94 
95 std::shared_ptr<matcher> matcher_factory::create_frame_number_matcher(std::vector<stream_interface*> profiles)
96 {
97  std::vector<std::shared_ptr<matcher>> matchers;
98  for (auto& p : profiles)
99  matchers.push_back(std::make_shared<identity_matcher>(p->get_unique_id(), p->get_stream_type()));
100 
101  return create_frame_number_composite_matcher(matchers);
102 }
103 std::shared_ptr<matcher> matcher_factory::create_timestamp_matcher(std::vector<stream_interface*> profiles)
104 {
105  std::vector<std::shared_ptr<matcher>> matchers;
106  for (auto& p : profiles)
107  matchers.push_back(std::make_shared<identity_matcher>(p->get_unique_id(), p->get_stream_type()));
108 
109  return create_timestamp_composite_matcher(matchers);
110 }
111 
113 {
114  return std::make_shared<identity_matcher>(profile->get_unique_id(), profile->get_stream_type());
115 }
116 
117 std::shared_ptr<matcher> matcher_factory::create_frame_number_composite_matcher(std::vector<std::shared_ptr<matcher>> matchers)
118 {
119  return std::make_shared<frame_number_composite_matcher>(matchers);
120 }
121 std::shared_ptr<matcher> matcher_factory::create_timestamp_composite_matcher(std::vector<std::shared_ptr<matcher>> matchers)
122 {
123  return std::make_shared<timestamp_composite_matcher>(matchers);
124 }
125 
126 device::device(std::shared_ptr<context> ctx,
128  bool device_changed_notifications)
129  : _context(ctx), _group(group), _is_valid(true),
130  _device_changed_notifications(device_changed_notifications)
131 {
133 
135  {
136  auto cb = new devices_changed_callback_internal([this](rs2_device_list* removed, rs2_device_list* added)
137  {
138  // Update is_valid variable when device is invalid
139  std::lock_guard<std::mutex> lock(_device_changed_mtx);
140  for (auto& dev_info : removed->list)
141  {
142  if (dev_info.info->get_device_data() == _group)
143  {
144  _is_valid = false;
145  return;
146  }
147  }
148  });
149 
150  _callback_id = _context->register_internal_device_callback({ cb, [](rs2_devices_changed_callback* p) { p->release(); } });
151  }
152 }
153 
155 {
157  {
158  _context->unregister_internal_device_callback(_callback_id);
159  }
160  _sensors.clear();
161 }
162 
163 int device::add_sensor(const std::shared_ptr<sensor_interface>& sensor_base)
164 {
165  _sensors.push_back(sensor_base);
166  return (int)_sensors.size() - 1;
167 }
168 
169 int device::assign_sensor(const std::shared_ptr<sensor_interface>& sensor_base, uint8_t idx)
170 {
171  try
172  {
173  _sensors[idx] = sensor_base;
174  return (int)_sensors.size() - 1;
175  }
176  catch (std::out_of_range)
177  {
178  throw invalid_value_exception(to_string() << "Cannot assign sensor - invalid subdevice value" << idx);
179  }
180 }
181 
183 {
184  return static_cast<unsigned int>(_sensors.size());
185 }
186 
188 {
189  try
190  {
191  return *(_sensors.at(subdevice));
192  }
193  catch (std::out_of_range)
194  {
195  throw invalid_value_exception("invalid subdevice value");
196  }
197 }
198 
200 {
201  int idx = 0;
202  for (auto&& sensor : _sensors)
203  {
204  if (&s == sensor.get()) return idx;
205  idx++;
206  }
207  throw std::runtime_error("Sensor not found!");
208 }
209 
210 const sensor_interface& device::get_sensor(size_t subdevice) const
211 {
212  try
213  {
214  return *(_sensors.at(subdevice));
215  }
216  catch (std::out_of_range)
217  {
218  throw invalid_value_exception("invalid subdevice value");
219  }
220 }
221 
223 {
224  throw not_implemented_exception(to_string() << __FUNCTION__ << " is not implemented for this device!");
225 }
226 
227 std::shared_ptr<matcher> device::create_matcher(const frame_holder& frame) const
228 {
229 
230  return std::make_shared<identity_matcher>( frame.frame->get_stream()->get_unique_id(), frame.frame->get_stream()->get_stream_type());
231 }
232 
233 std::pair<uint32_t, rs2_extrinsics> device::get_extrinsics(const stream_interface& stream) const
234 {
235  auto stream_index = stream.get_unique_id();
236  auto pair = _extrinsics.at(stream_index);
237  auto pin_stream = pair.second;
238  rs2_extrinsics ext{};
239  if (environment::get_instance().get_extrinsics_graph().try_fetch_extrinsics(*pin_stream, stream, &ext) == false)
240  {
241  throw std::runtime_error(to_string() << "Failed to fetch extrinsics between pin stream (" << pin_stream->get_unique_id() << ") to given stream (" << stream.get_unique_id() << ")");
242  }
243  return std::make_pair(pair.first, ext);
244 }
245 
247 {
248  auto iter = std::find_if(_extrinsics.begin(),
249  _extrinsics.end(),
250  [group_index](const std::pair<int, std::pair<uint32_t, std::shared_ptr<const stream_interface>>>& p) { return p.second.first == group_index; });
251  if (iter == _extrinsics.end())
252  {
253  //First stream to register for this group
254  _extrinsics[stream.get_unique_id()] = std::make_pair(group_index, stream.shared_from_this());
255  }
256  else
257  {
258  //iter->second holds the group_id and the key stream
259  _extrinsics[stream.get_unique_id()] = iter->second;
260  }
261 }
262 
263 std::vector<rs2_format> device::map_supported_color_formats(rs2_format source_format)
264 {
265  // Mapping from source color format to all of the compatible target color formats.
266 
267  std::vector<rs2_format> target_formats = { RS2_FORMAT_RGB8, RS2_FORMAT_RGBA8, RS2_FORMAT_BGR8, RS2_FORMAT_BGRA8 };
268  switch (source_format)
269  {
270  case RS2_FORMAT_YUYV:
271  target_formats.push_back(RS2_FORMAT_YUYV);
272  target_formats.push_back(RS2_FORMAT_Y16);
273  break;
274  case RS2_FORMAT_UYVY:
275  target_formats.push_back(RS2_FORMAT_UYVY);
276  break;
277  default:
278  LOG_ERROR("Format is not supported for mapping");
279  }
280  return target_formats;
281 }
282 
284 {
285  for (auto profile : profiles)
286  {
287  for (auto tag : *_profiles_tags)
288  {
289  if (auto vp = dynamic_cast<video_stream_profile_interface*>(profile.get()))
290  {
291  if ((tag.stream == RS2_STREAM_ANY || vp->get_stream_type() == tag.stream) &&
292  (tag.format == RS2_FORMAT_ANY || vp->get_format() == tag.format) &&
293  (tag.width == -1 || vp->get_width() == tag.width) &&
294  (tag.height == -1 || vp->get_height() == tag.height) &&
295  (tag.fps == -1 || vp->get_framerate() == tag.fps) &&
296  (tag.stream_index == -1 || vp->get_stream_index() == tag.stream_index))
297  profile->tag_profile(tag.tag);
298  }
299  else
300  if (auto mp = dynamic_cast<motion_stream_profile_interface*>(profile.get()))
301  {
302  if ((tag.stream == RS2_STREAM_ANY || mp->get_stream_type() == tag.stream) &&
303  (tag.format == RS2_FORMAT_ANY || mp->get_format() == tag.format) &&
304  (tag.fps == -1 || mp->get_framerate() == tag.fps) &&
305  (tag.stream_index == -1 || mp->get_stream_index() == tag.stream_index))
306  profile->tag_profile(tag.tag);
307  }
308  }
309  }
310 }
311 
312 bool device::contradicts(const stream_profile_interface* a, const std::vector<stream_profile>& others) const
313 {
314  if (auto vid_a = dynamic_cast<const video_stream_profile_interface*>(a))
315  {
316  for (auto request : others)
317  {
318  if (a->get_framerate() != 0 && request.fps != 0 && (a->get_framerate() != request.fps))
319  return true;
320  if (vid_a->get_width() != 0 && request.width != 0 && (vid_a->get_width() != request.width))
321  return true;
322  if (vid_a->get_height() != 0 && request.height != 0 && (vid_a->get_height() != request.height))
323  return true;
324  }
325  }
326  return false;
327 }
328 
330 {
331  for (auto& sensor : _sensors)
332  {
333  auto snr_name = (sensor->supports_info(RS2_CAMERA_INFO_NAME)) ? sensor->get_info(RS2_CAMERA_INFO_NAME) : "";
334 
335  // Disable asynchronous services
336  for (auto& opt : sensor->get_supported_options())
337  {
339  {
340  try
341  {
342  // enumerated options use zero or positive values
343  if (sensor->get_option(opt).query() > 0.f)
344  sensor->get_option(opt).set(false);
345  }
346  catch (...)
347  {
348  LOG_ERROR("Failed to toggle off " << opt << " [" << snr_name << "]");
349  }
350  }
351  }
352 
353  // Stop UVC/HID streaming
354  try
355  {
356  if (sensor->is_streaming())
357  {
358  sensor->stop();
359  sensor->close();
360  }
361  }
362  catch (const wrong_api_call_sequence_exception& exc)
363  {
364  LOG_WARNING("Out of order stop/close invocation for " << snr_name << ": " << exc.what());
365  }
366  catch (...)
367  {
368  auto snr_name = (sensor->supports_info(RS2_CAMERA_INFO_NAME)) ? sensor->get_info(RS2_CAMERA_INFO_NAME) : "";
369  LOG_ERROR("Failed to deactivate " << snr_name);
370  }
371  }
372 }
static const textual_icon lock
Definition: model-views.h:218
virtual rs2_stream get_stream_type() const =0
static std::shared_ptr< matcher > create_timestamp_composite_matcher(std::vector< std::shared_ptr< matcher >> matchers)
Definition: device.cpp:121
std::mutex _device_changed_mtx
Definition: device.h:100
size_t find_sensor_idx(const sensor_interface &s) const
Definition: device.cpp:199
boost_foreach_argument_dependent_lookup_hack tag
Definition: foreach_fwd.hpp:31
virtual bool contradicts(const stream_profile_interface *a, const std::vector< stream_profile > &others) const override
Definition: device.cpp:312
void tag_profiles(stream_profiles profiles) const override
Definition: device.cpp:283
static std::shared_ptr< matcher > create_DLR_matcher(std::vector< stream_interface * > profiles)
Definition: device.cpp:68
GLdouble s
GLfloat GLfloat p
Definition: glext.h:12687
#define LOG_WARNING(...)
Definition: src/types.h:241
bool val_in_range(const T &val, const std::initializer_list< T > &list)
Definition: src/types.h:174
GLuint color
uint64_t _callback_id
Definition: device.h:101
GLint GLint GLsizei GLsizei GLsizei depth
std::vector< rs2_device_info > list
Definition: context.h:29
unsigned char uint8_t
Definition: stdint.h:78
size_t get_sensors_count() const override
Definition: device.cpp:182
GLuint index
GLboolean GLboolean GLboolean GLboolean a
static std::shared_ptr< matcher > create_DLR_C_matcher(std::vector< stream_interface * > profiles)
Definition: device.cpp:42
virtual ~device()
Definition: device.cpp:154
bool _device_changed_notifications
Definition: device.h:99
stream_interface * find_profile(rs2_stream stream, int index, std::vector< stream_interface * > profiles)
Definition: device.cpp:29
static std::shared_ptr< matcher > create_frame_number_matcher(std::vector< stream_interface * > profiles)
Definition: device.cpp:95
unsigned int uint32_t
Definition: stdint.h:80
rs2_matchers
Specifies types of different matchers.
Definition: rs_types.h:230
GLboolean GLuint group
Definition: glext.h:5688
int assign_sensor(const std::shared_ptr< sensor_interface > &sensor_base, uint8_t idx)
Definition: device.cpp:169
std::map< int, std::pair< uint32_t, std::shared_ptr< const stream_interface > > > _extrinsics
Definition: device.h:93
void hardware_reset() override
Definition: device.cpp:222
GLint left
Definition: glext.h:1963
rs2_format
A stream&#39;s format identifies how binary data is encoded within a frame.
Definition: rs_sensor.h:59
#define LOG_ERROR(...)
Definition: src/types.h:242
virtual std::vector< tagged_profile > get_profiles_tags() const =0
std::vector< std::shared_ptr< stream_profile_interface >> stream_profiles
Definition: streaming.h:165
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:42
virtual std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
Definition: device.cpp:227
static environment & get_instance()
int add_sensor(const std::shared_ptr< sensor_interface > &sensor_base)
Definition: device.cpp:163
sensor_interface & get_sensor(size_t subdevice) override
Definition: device.cpp:187
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented...
Definition: rs_sensor.h:96
virtual std::shared_ptr< stream_profile_interface > get_stream() const =0
frame_interface * frame
Definition: streaming.h:126
static std::shared_ptr< matcher > create_DI_C_matcher(std::vector< stream_interface * > profiles)
Definition: device.cpp:55
static std::shared_ptr< matcher > create_frame_number_composite_matcher(std::vector< std::shared_ptr< matcher >> matchers)
Definition: device.cpp:117
lazy< std::vector< tagged_profile > > _profiles_tags
Definition: device.h:102
GLdouble right
const char * what() const noexceptoverride
Definition: src/types.h:284
std::shared_ptr< context > _context
Definition: device.h:97
std::vector< rs2_format > map_supported_color_formats(rs2_format source_format)
Definition: device.cpp:263
static std::shared_ptr< matcher > create_identity_matcher(stream_interface *profiles)
Definition: device.cpp:112
void register_stream_to_extrinsic_group(const stream_interface &stream, uint32_t groupd_index)
Definition: device.cpp:246
virtual uint32_t get_framerate() const =0
std::vector< std::shared_ptr< sensor_interface > > _sensors
Definition: device.h:96
device(std::shared_ptr< context > ctx, const platform::backend_device_group group, bool device_changed_notifications=false)
Definition: device.cpp:126
std::pair< uint32_t, rs2_extrinsics > get_extrinsics(const stream_interface &stream) const override
Definition: device.cpp:233
#define LOG_DEBUG(...)
Definition: src/types.h:239
const platform::backend_device_group _group
Definition: device.h:98
virtual int get_unique_id() const =0
virtual void stop_activity() const
Definition: device.cpp:329
static std::shared_ptr< matcher > create_timestamp_matcher(std::vector< stream_interface * > profiles)
Definition: device.cpp:103
std::string to_string(T value)
static std::shared_ptr< matcher > create(rs2_matchers matcher, std::vector< stream_interface * > profiles)
Definition: device.cpp:11
static std::shared_ptr< matcher > create_DI_matcher(std::vector< stream_interface * > profiles)
Definition: device.cpp:82


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