The open source OpenXR runtime
d/rs: Split SLAM source out of rs_hdev
(This temporarily disables the RealSense SLAM stream as there is
nothing starting it until the next commit)
authored by
Mateo de Mayo
and committed by
Jakob Bornecrantz
f84629cc
0202cb92
+246
-239
+232
-237
src/xrt/drivers/realsense/rs_hdev.c
+232
-237
src/xrt/drivers/realsense/rs_hdev.c
···
51
51
#define DEFAULT_STREAM1_INDEX 1
52
52
#define DEFAULT_STREAM2_INDEX 2
53
53
54
-
#define DEVICE_STRING "Intel RealSense Host-SLAM"
54
+
#define RS_DEVICE_STR "Intel RealSense Host-SLAM"
55
+
#define RS_SOURCE_STR "RealSense Source"
56
+
#define RS_HOST_SLAM_TRACKER_STR "Host SLAM Tracker for RealSense"
55
57
56
-
#define RS_TRACE(...) U_LOG_XDEV_IFL_T(&rs->xdev, rs->ll, __VA_ARGS__)
57
-
#define RS_DEBUG(...) U_LOG_XDEV_IFL_D(&rs->xdev, rs->ll, __VA_ARGS__)
58
-
#define RS_INFO(...) U_LOG_XDEV_IFL_I(&rs->xdev, rs->ll, __VA_ARGS__)
59
-
#define RS_WARN(...) U_LOG_XDEV_IFL_W(&rs->xdev, rs->ll, __VA_ARGS__)
60
-
#define RS_ERROR(...) U_LOG_XDEV_IFL_E(&rs->xdev, rs->ll, __VA_ARGS__)
58
+
#define RS_TRACE(r, ...) U_LOG_IFL_T(r->ll, __VA_ARGS__)
59
+
#define RS_DEBUG(r, ...) U_LOG_IFL_D(r->ll, __VA_ARGS__)
60
+
#define RS_INFO(r, ...) U_LOG_IFL_I(r->ll, __VA_ARGS__)
61
+
#define RS_WARN(r, ...) U_LOG_IFL_W(r->ll, __VA_ARGS__)
62
+
#define RS_ERROR(r, ...) U_LOG_IFL_E(r->ll, __VA_ARGS__)
61
63
#define RS_ASSERT(predicate, ...) \
62
64
do { \
63
65
bool p = predicate; \
···
99
101
static void
100
102
receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *);
101
103
static void
102
-
rs_hdev_node_break_apart(struct xrt_frame_node *);
104
+
rs_source_node_break_apart(struct xrt_frame_node *);
103
105
static void
104
-
rs_hdev_node_destroy(struct xrt_frame_node *);
106
+
rs_source_node_destroy(struct xrt_frame_node *);
105
107
106
108
/*!
107
109
* Host-SLAM tracked RealSense device (any RealSense device with camera and IMU streams).
108
110
*
109
111
* @implements xrt_device
110
-
* @implements xrt_fs
111
-
* @implements xrt_frame_node
112
112
*/
113
113
struct rs_hdev
114
114
{
115
115
struct xrt_device xdev;
116
-
struct xrt_frame_context xfctx;
117
-
struct xrt_frame_node node;
118
-
struct xrt_fs xfs;
119
116
struct xrt_tracked_slam *slam;
120
117
struct xrt_pose pose; //!< Device pose
121
118
struct xrt_pose offset; //!< Additional offset to apply to `pose`
122
119
enum u_logging_level ll; //!< Log level
120
+
};
121
+
122
+
/*!
123
+
* RealSense source of camera and IMU data.
124
+
*
125
+
* @implements xrt_fs
126
+
* @implements xrt_frame_node
127
+
*/
128
+
struct rs_source
129
+
{
130
+
struct xrt_fs xfs;
131
+
struct xrt_frame_node node;
132
+
enum u_logging_level ll; //!< Log level
123
133
124
134
// Sinks
125
135
struct xrt_frame_sink left_sink; //!< Intermediate sink for left camera frames
···
163
173
164
174
//! @todo Unify check_error() and DO() usage thorough the driver.
165
175
static bool
166
-
check_error(struct rs_hdev *rs, rs2_error *e, const char *file, int line)
176
+
check_error(struct rs_source *rs, rs2_error *e, const char *file, int line)
167
177
{
168
178
if (e == NULL) {
169
179
return false; // No errors
170
180
}
171
181
172
-
RS_ERROR("rs_error was raised when calling %s(%s):", rs2_get_failed_function(e), rs2_get_failed_args(e));
173
-
RS_ERROR("%s:%d: %s", file, line, rs2_get_error_message(e));
182
+
RS_ERROR(rs, "rs_error was raised when calling %s(%s):", rs2_get_failed_function(e), rs2_get_failed_args(e));
183
+
RS_ERROR(rs, "%s:%d: %s", file, line, rs2_get_error_message(e));
174
184
exit(EXIT_FAILURE);
175
185
}
176
186
···
184
194
static inline struct rs_hdev *
185
195
rs_hdev_from_xdev(struct xrt_device *xdev)
186
196
{
187
-
struct rs_hdev *rs = container_of(xdev, struct rs_hdev, xdev);
188
-
return rs;
197
+
struct rs_hdev *rh = container_of(xdev, struct rs_hdev, xdev);
198
+
return rh;
189
199
}
190
200
191
201
static void
···
255
265
uint64_t at_timestamp_ns,
256
266
struct xrt_space_relation *out_relation)
257
267
{
258
-
struct rs_hdev *rs = rs_hdev_from_xdev(xdev);
259
-
RS_ASSERT_(rs->slam != NULL);
268
+
struct rs_hdev *rh = rs_hdev_from_xdev(xdev);
269
+
RS_ASSERT_(rh->slam != NULL);
260
270
RS_ASSERT_(at_timestamp_ns < INT64_MAX);
261
271
262
-
xrt_tracked_slam_get_tracked_pose(rs->slam, at_timestamp_ns, out_relation);
272
+
xrt_tracked_slam_get_tracked_pose(rh->slam, at_timestamp_ns, out_relation);
263
273
264
274
int pose_bits = XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT;
265
275
bool pose_tracked = out_relation->relation_flags & pose_bits;
266
276
267
277
if (pose_tracked) {
268
278
#if defined(XRT_HAVE_KIMERA_SLAM)
269
-
rs->pose = rs_hdev_correct_pose_from_kimera(out_relation->pose);
279
+
rh->pose = rs_hdev_correct_pose_from_kimera(out_relation->pose);
270
280
#elif defined(XRT_HAVE_BASALT_SLAM)
271
-
rs->pose = rs_hdev_correct_pose_from_basalt(out_relation->pose);
281
+
rh->pose = rs_hdev_correct_pose_from_basalt(out_relation->pose);
272
282
#else
273
-
rs->pose = out_relation->pose;
283
+
rh->pose = out_relation->pose;
274
284
#endif
275
285
}
276
286
277
287
struct xrt_space_graph space_graph = {0};
278
-
m_space_graph_add_pose(&space_graph, &rs->pose);
279
-
m_space_graph_add_pose(&space_graph, &rs->offset);
288
+
m_space_graph_add_pose(&space_graph, &rh->pose);
289
+
m_space_graph_add_pose(&space_graph, &rh->offset);
280
290
m_space_graph_resolve(&space_graph, out_relation);
281
291
out_relation->relation_flags = (enum xrt_space_relation_flags)(
282
292
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT |
···
286
296
static void
287
297
rs_hdev_destroy(struct xrt_device *xdev)
288
298
{
289
-
struct rs_hdev *rs = rs_hdev_from_xdev(xdev);
290
-
RS_INFO("Destroying rs_hdev");
291
-
xrt_frame_context_destroy_nodes(&rs->xfctx);
292
-
RS_ASSERT(!rs->is_running, "Trying to destroy device while it is still streaming");
293
-
u_device_free(&rs->xdev);
299
+
struct rs_hdev *rh = rs_hdev_from_xdev(xdev);
300
+
RS_INFO(rh, "Destroying rs_hdev");
301
+
u_var_remove_root(rh);
302
+
u_device_free(&rh->xdev);
294
303
}
295
304
296
305
···
305
314
//! Helper function for loading an int field from a json container and printing useful
306
315
//! messages along it. *out is expected to come preloaded with a default value.
307
316
static void
308
-
json_int(struct rs_hdev *rs, const cJSON *json, const char *field, int *out)
317
+
json_int(struct rs_source *rs, const cJSON *json, const char *field, int *out)
309
318
{
310
319
if (!u_json_get_int(u_json_get(json, field), out)) {
311
320
// This is a warning because we want the user to set these config fields
312
-
RS_WARN("Using default %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, *out);
321
+
RS_WARN(rs, "Using default %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, *out);
313
322
} else {
314
-
RS_DEBUG("Using %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, *out);
323
+
RS_DEBUG(rs, "Using %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, *out);
315
324
}
316
325
}
317
326
318
327
//! Similar to @ref json_int but for bools.
319
328
static void
320
-
json_bool(struct rs_hdev *rs, const cJSON *json, const char *field, bool *out)
329
+
json_bool(struct rs_source *rs, const cJSON *json, const char *field, bool *out)
321
330
{
322
331
if (!u_json_get_bool(u_json_get(json, field), out)) {
323
332
// This is a warning because we want the user to set these config fields
324
-
RS_WARN("Using default %s.%s=%s", JSON_CONFIG_FIELD_NAME, field, *out ? "true" : "false");
333
+
RS_WARN(rs, "Using default %s.%s=%s", JSON_CONFIG_FIELD_NAME, field, *out ? "true" : "false");
325
334
} else {
326
-
RS_DEBUG("Using %s.%s=%s", JSON_CONFIG_FIELD_NAME, field, *out ? "true" : "false");
335
+
RS_DEBUG(rs, "Using %s.%s=%s", JSON_CONFIG_FIELD_NAME, field, *out ? "true" : "false");
327
336
}
328
337
}
329
338
···
331
340
//! equivalent xrt_format if any.
332
341
static void
333
342
json_rs2_format(
334
-
struct rs_hdev *rs, const cJSON *json, const char *field, rs2_format *out_rformat, enum xrt_format *out_xformat)
343
+
struct rs_source *rs, const cJSON *json, const char *field, rs2_format *out_rformat, enum xrt_format *out_xformat)
335
344
{
336
345
int format_int = (int)*out_rformat;
337
346
bool valid_field = u_json_get_int(u_json_get(json, field), &format_int);
338
347
if (!valid_field) {
339
-
RS_WARN("Using default %s.%s=%d (%d)", JSON_CONFIG_FIELD_NAME, field, *out_rformat, *out_xformat);
348
+
RS_WARN(rs, "Using default %s.%s=%d (%d)", JSON_CONFIG_FIELD_NAME, field, *out_rformat, *out_xformat);
340
349
return;
341
350
}
342
351
···
347
356
} else if (rformat == RS2_FORMAT_RGB8 || rformat == RS2_FORMAT_BGR8) {
348
357
xformat = XRT_FORMAT_R8G8B8;
349
358
} else {
350
-
RS_ERROR("Invalid %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, format_int);
351
-
RS_ERROR("Valid values: %d, %d, %d", RS2_FORMAT_Y8, RS2_FORMAT_RGB8, RS2_FORMAT_BGR8);
352
-
RS_ERROR("Using default %s.%s=%d (%d)", JSON_CONFIG_FIELD_NAME, field, *out_rformat, *out_xformat);
359
+
RS_ERROR(rs, "Invalid %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, format_int);
360
+
RS_ERROR(rs, "Valid values: %d, %d, %d", RS2_FORMAT_Y8, RS2_FORMAT_RGB8, RS2_FORMAT_BGR8);
361
+
RS_ERROR(rs, "Using default %s.%s=%d (%d)", JSON_CONFIG_FIELD_NAME, field, *out_rformat, *out_xformat);
353
362
354
363
// Reaching this doesn't mean that a matching xrt_format doesn't exist, just
355
364
// that I didn't need it. Feel free to add it.
···
359
368
360
369
*out_rformat = rformat;
361
370
*out_xformat = xformat;
362
-
RS_DEBUG("Using %s.%s=%d (xrt_format=%d)", JSON_CONFIG_FIELD_NAME, field, *out_rformat, *out_xformat);
371
+
RS_DEBUG(rs, "Using %s.%s=%d (xrt_format=%d)", JSON_CONFIG_FIELD_NAME, field, *out_rformat, *out_xformat);
363
372
}
364
373
365
374
//! Similar to @ref json_int but for a rs2_stream type.
366
375
static void
367
-
json_rs2_stream(struct rs_hdev *rs, const cJSON *json, const char *field, rs2_stream *out_stream)
376
+
json_rs2_stream(struct rs_source *rs, const cJSON *json, const char *field, rs2_stream *out_stream)
368
377
{
369
378
int stream_int = (int)*out_stream;
370
379
bool valid_field = u_json_get_int(u_json_get(json, field), &stream_int);
371
380
if (!valid_field) {
372
-
RS_WARN("Using default %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, *out_stream);
381
+
RS_WARN(rs, "Using default %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, *out_stream);
373
382
return;
374
383
}
375
384
376
385
rs2_stream rstream = (rs2_stream)stream_int;
377
386
if (rstream != RS2_STREAM_COLOR && rstream != RS2_STREAM_INFRARED && rstream != RS2_STREAM_FISHEYE) {
378
-
RS_ERROR("Invalid %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, stream_int);
379
-
RS_ERROR("Valid values: %d, %d, %d", RS2_STREAM_COLOR, RS2_STREAM_INFRARED, RS2_STREAM_FISHEYE);
380
-
RS_ERROR("Using default %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, *out_stream);
387
+
RS_ERROR(rs, "Invalid %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, stream_int);
388
+
RS_ERROR(rs, "Valid values: %d, %d, %d", RS2_STREAM_COLOR, RS2_STREAM_INFRARED, RS2_STREAM_FISHEYE);
389
+
RS_ERROR(rs, "Using default %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, *out_stream);
381
390
return;
382
391
}
383
392
384
393
*out_stream = rstream;
385
-
RS_DEBUG("Using %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, *out_stream);
394
+
RS_DEBUG(rs, "Using %s.%s=%d", JSON_CONFIG_FIELD_NAME, field, *out_stream);
386
395
}
387
396
388
397
static void
389
-
rs_hdev_load_stream_options_from_json(struct rs_hdev *rs)
398
+
rs_source_load_stream_options_from_json(struct rs_source *rs)
390
399
{
391
400
// Set default values
392
401
rs->stereo = DEFAULT_STEREO;
···
404
413
struct u_config_json config = {0};
405
414
u_config_json_open_or_create_main_file(&config);
406
415
if (!config.file_loaded) {
407
-
RS_WARN("Unable to load config file, will use default stream values");
416
+
RS_WARN(rs, "Unable to load config file, will use default stream values");
408
417
cJSON_Delete(config.root);
409
418
return;
410
419
}
411
420
412
421
const cJSON *hdev_config = u_json_get(config.root, JSON_CONFIG_FIELD_NAME);
413
422
if (hdev_config == NULL) {
414
-
RS_WARN("Field '%s' was not present in the json file, will use default values", JSON_CONFIG_FIELD_NAME);
423
+
RS_WARN(rs, "Field '%s' not present in json file, will use defaults", JSON_CONFIG_FIELD_NAME);
415
424
}
416
425
417
426
json_bool(rs, hdev_config, "stereo", &rs->stereo);
···
437
446
438
447
//! Disable any laser emitters because they confuse SLAM feature detection
439
448
static void
440
-
disable_all_laser_emitters(struct rs_hdev *rs)
449
+
disable_all_laser_emitters(struct rs_source *rs)
441
450
{
442
451
struct rs_container *rsc = &rs->rsc;
443
452
rs2_sensor_list *sensors = DO(rs2_query_sensors, rsc->device);
···
462
471
*/
463
472
464
473
static void
465
-
rs_hdev_frame_destroy(struct xrt_frame *xf)
474
+
rs_source_frame_destroy(struct xrt_frame *xf)
466
475
{
467
476
rs2_frame *rframe = (rs2_frame *)xf->owner;
468
477
rs2_release_frame(rframe);
···
470
479
}
471
480
472
481
static void
473
-
rs2xrt_frame(struct rs_hdev *rs, rs2_frame *rframe, struct xrt_frame **out_xframe)
482
+
rs2xrt_frame(struct rs_source *rs, rs2_frame *rframe, struct xrt_frame **out_xframe)
474
483
{
475
484
RS_ASSERT_(*out_xframe == NULL);
476
485
···
495
504
496
505
struct xrt_frame *xf = U_TYPED_CALLOC(struct xrt_frame);
497
506
xf->reference.count = 1;
498
-
xf->destroy = rs_hdev_frame_destroy;
507
+
xf->destroy = rs_source_frame_destroy;
499
508
xf->owner = rframe;
500
509
xf->width = rs->video_width;
501
510
xf->height = rs->video_height;
···
516
525
}
517
526
518
527
static void
519
-
handle_frameset(struct rs_hdev *rs, rs2_frame *frames)
528
+
handle_frameset(struct rs_source *rs, rs2_frame *frames)
520
529
{
521
530
522
531
// Check number of frames on debug builds
···
544
553
xrt_sink_push_frame(rs->in_sinks.right, xf_right);
545
554
} else {
546
555
// This usually happens only once at start and never again
547
-
RS_WARN("Realsense device sent left and right frames with different timestamps %ld != %ld",
556
+
RS_WARN(rs, "Realsense device sent left and right frames with different timestamps %ld != %ld",
548
557
xf_left->timestamp, xf_right->timestamp);
549
558
}
550
559
···
562
571
//! Decides when to submit the full IMU sample out of separate
563
572
//! gyroscope/accelerometer samples.
564
573
static void
565
-
partial_imu_sample_push(struct rs_hdev *rs, timepoint_ns ts, struct xrt_vec3 vals, bool is_gyro)
574
+
partial_imu_sample_push(struct rs_source *rs, timepoint_ns ts, struct xrt_vec3 vals, bool is_gyro)
566
575
{
567
576
os_mutex_lock(&rs->partial_imu_sample.mutex);
568
577
···
585
594
}
586
595
587
596
static void
588
-
handle_gyro_frame(struct rs_hdev *rs, rs2_frame *frame)
597
+
handle_gyro_frame(struct rs_source *rs, rs2_frame *frame)
589
598
{
590
599
const float *data = DO(rs2_get_frame_data, frame);
591
600
···
598
607
double timestamp_ms = DO(rs2_get_frame_timestamp, frame);
599
608
timepoint_ns timestamp_ns = timestamp_ms * 1000 * 1000;
600
609
struct xrt_vec3 gyro = {data[0], data[1], data[2]};
601
-
RS_TRACE("gyro t=%ld x=%f y=%f z=%f", timestamp_ns, gyro.x, gyro.y, gyro.z);
610
+
RS_TRACE(rs, "gyro t=%ld x=%f y=%f z=%f", timestamp_ns, gyro.x, gyro.y, gyro.z);
602
611
partial_imu_sample_push(rs, timestamp_ns, gyro, true);
603
612
rs2_release_frame(frame);
604
613
}
605
614
606
615
static void
607
-
handle_accel_frame(struct rs_hdev *rs, rs2_frame *frame)
616
+
handle_accel_frame(struct rs_source *rs, rs2_frame *frame)
608
617
{
609
618
const float *data = DO(rs2_get_frame_data, frame);
610
619
···
619
628
double timestamp_ms = DO(rs2_get_frame_timestamp, frame);
620
629
timepoint_ns timestamp_ns = timestamp_ms * 1000 * 1000;
621
630
struct xrt_vec3 accel = {data[0], data[1], data[2]};
622
-
RS_TRACE("accel t=%ld x=%f y=%f z=%f", timestamp_ns, accel.x, accel.y, accel.z);
631
+
RS_TRACE(rs, "accel t=%ld x=%f y=%f z=%f", timestamp_ns, accel.x, accel.y, accel.z);
623
632
partial_imu_sample_push(rs, timestamp_ns, accel, false);
624
633
rs2_release_frame(frame);
625
634
}
···
627
636
//! Checks that the timestamp domain of the realsense sample (the frame) is in
628
637
//! global time or, at the very least, in another domain that we support.
629
638
static inline void
630
-
check_global_time(struct rs_hdev *rs, rs2_frame *frame, rs2_stream stream_type)
639
+
check_global_time(struct rs_source *rs, rs2_frame *frame, rs2_stream stream_type)
631
640
{
632
641
633
642
#ifndef NDEBUG // Check valid timestamp domains only on debug builds
···
656
665
}
657
666
658
667
if (!acceptable_timestamp_domain) {
659
-
RS_ERROR("Invalid ts_domain=%s", rs2_timestamp_domain_to_string(ts_domain));
660
-
RS_ERROR("One of your RealSense sensors is not using GLOBAL_TIME domain for its timestamps.");
661
-
RS_ERROR("This should be solved by applying the kernel patch required by the RealSense SDK.");
668
+
RS_ERROR(rs, "Invalid ts_domain=%s", rs2_timestamp_domain_to_string(ts_domain));
669
+
RS_ERROR(rs, "One of your RealSense sensors is not using GLOBAL_TIME domain for its timestamps.");
670
+
RS_ERROR(rs, "This should be solved by applying the kernel patch required by the RealSense SDK.");
662
671
if (is_motion_sensor) {
663
672
const char *a = is_accel ? "accelerometer" : "gyroscope";
664
673
const char *b = is_accel ? "gyroscope" : "accelerometer";
665
-
RS_ERROR("As an alternative, set %s frequency to be greater than %s frequency.", b, a);
674
+
RS_ERROR(rs, "As an alternative, set %s frequency to be greater than %s frequency.", b, a);
666
675
}
667
676
RS_DASSERT(false, "Unacceptable timestamp domain %s", rs2_timestamp_domain_to_string(ts_domain));
668
677
}
···
672
681
static void
673
682
on_frame(rs2_frame *frame, void *ptr)
674
683
{
675
-
struct rs_hdev *rs = (struct rs_hdev *)ptr;
684
+
struct rs_source *rs = (struct rs_source *)ptr;
676
685
677
686
const rs2_stream_profile *stream = DO(rs2_get_frame_stream_profile, frame);
678
687
rs2_stream stream_type;
···
706
715
*
707
716
*/
708
717
709
-
static inline struct rs_hdev *
710
-
rs_hdev_from_xfs(struct xrt_fs *xfs)
718
+
static inline struct rs_source *
719
+
rs_source_from_xfs(struct xrt_fs *xfs)
711
720
{
712
-
struct rs_hdev *rs = container_of(xfs, struct rs_hdev, xfs);
721
+
struct rs_source *rs = container_of(xfs, struct rs_source, xfs);
713
722
return rs;
714
723
}
715
724
716
725
static bool
717
-
rs_hdev_enumerate_modes(struct xrt_fs *xfs, struct xrt_fs_mode **out_modes, uint32_t *out_count)
726
+
rs_source_enumerate_modes(struct xrt_fs *xfs, struct xrt_fs_mode **out_modes, uint32_t *out_count)
718
727
{
719
-
//! @todo implement
720
-
RS_ASSERT(false, "Not Implemented");
721
-
return false;
728
+
struct rs_source *rs = container_of(xfs, struct rs_source, xfs);
729
+
struct xrt_fs_mode *modes = U_TYPED_ARRAY_CALLOC(struct xrt_fs_mode, 1);
730
+
RS_ASSERT(modes != NULL, "Unable to calloc rs_source playback modes");
731
+
732
+
//! @todo only exposing the one stream configuration the user provided through
733
+
//! the json configuration but we could show all possible stream setups.
734
+
modes[0] = (struct xrt_fs_mode){.width = rs->video_width,
735
+
.height = rs->video_height,
736
+
.format = rs->xrt_video_format,
737
+
//! @todo The stereo_format being NONE is incorrect but one that supports
738
+
//! frames in different memory regions does not exist yet.
739
+
.stereo_format = XRT_STEREO_FORMAT_NONE};
740
+
741
+
*out_modes = modes;
742
+
*out_count = 1;
743
+
744
+
return true;
722
745
}
723
746
724
747
static bool
725
-
rs_hdev_configure_capture(struct xrt_fs *xfs, struct xrt_fs_capture_parameters *cp)
748
+
rs_source_configure_capture(struct xrt_fs *xfs, struct xrt_fs_capture_parameters *cp)
726
749
{
727
750
//! @todo implement
728
751
RS_ASSERT(false, "Not Implemented");
···
730
753
}
731
754
732
755
static bool
733
-
rs_hdev_stream_stop(struct xrt_fs *xfs)
756
+
rs_source_stream_stop(struct xrt_fs *xfs)
734
757
{
735
-
struct rs_hdev *rs = rs_hdev_from_xfs(xfs);
758
+
struct rs_source *rs = rs_source_from_xfs(xfs);
736
759
if (rs->is_running) {
737
760
DO(rs2_pipeline_stop, rs->rsc.pipeline);
738
761
rs->is_running = false;
···
741
764
}
742
765
743
766
static bool
744
-
rs_hdev_is_running(struct xrt_fs *xfs)
767
+
rs_source_is_running(struct xrt_fs *xfs)
745
768
{
746
-
struct rs_hdev *rs = rs_hdev_from_xfs(xfs);
769
+
struct rs_source *rs = rs_source_from_xfs(xfs);
747
770
return rs->is_running;
748
771
}
749
772
750
773
static bool
751
-
rs_hdev_stream_start(struct xrt_fs *xfs,
752
-
struct xrt_frame_sink *xs,
753
-
enum xrt_fs_capture_type capture_type,
754
-
uint32_t descriptor_index)
774
+
rs_source_stream_start(struct xrt_fs *xfs,
775
+
struct xrt_frame_sink *xs,
776
+
enum xrt_fs_capture_type capture_type,
777
+
uint32_t descriptor_index)
755
778
{
756
-
struct rs_hdev *rs = rs_hdev_from_xfs(xfs);
779
+
struct rs_source *rs = rs_source_from_xfs(xfs);
757
780
if (xs == NULL && capture_type == XRT_FS_CAPTURE_TYPE_TRACKING) {
758
781
RS_ASSERT(rs->out_sinks.left != NULL, "No left sink provided");
759
-
RS_INFO("Starting RealSense stream in tracking mode");
782
+
RS_INFO(rs, "Starting RealSense stream in tracking mode");
760
783
} else if (xs != NULL && capture_type == XRT_FS_CAPTURE_TYPE_CALIBRATION) {
761
-
RS_INFO("Starting RealSense stream in calibration mode, will stream only left frames");
784
+
RS_INFO(rs, "Starting RealSense stream in calibration mode, will stream only left frames");
762
785
rs->out_sinks.left = xs;
763
786
} else {
764
787
RS_ASSERT(false, "Unsupported stream configuration xs=%p capture_type=%d", (void *)xs, capture_type);
···
775
798
}
776
799
777
800
static bool
778
-
rs_hdev_slam_stream_start(struct xrt_fs *xfs, struct xrt_slam_sinks *sinks)
801
+
rs_source_slam_stream_start(struct xrt_fs *xfs, struct xrt_slam_sinks *sinks)
779
802
{
780
-
struct rs_hdev *rs = rs_hdev_from_xfs(xfs);
803
+
struct rs_source *rs = rs_source_from_xfs(xfs);
781
804
rs->out_sinks = *sinks;
782
-
return rs_hdev_stream_start(xfs, NULL, XRT_FS_CAPTURE_TYPE_TRACKING, 0);
783
-
}
784
-
785
-
//! Create and open the frame server for IMU/camera streaming.
786
-
static struct xrt_fs *
787
-
rs_hdev_fs_create(struct rs_hdev *rs, int device_idx)
788
-
{
789
-
// Setup xrt_fs
790
-
struct xrt_fs *xfs = &rs->xfs;
791
-
xfs->enumerate_modes = rs_hdev_enumerate_modes;
792
-
xfs->configure_capture = rs_hdev_configure_capture;
793
-
xfs->stream_start = rs_hdev_stream_start;
794
-
xfs->slam_stream_start = rs_hdev_slam_stream_start;
795
-
xfs->stream_stop = rs_hdev_stream_stop;
796
-
xfs->is_running = rs_hdev_is_running;
797
-
snprintf(xfs->name, sizeof(xfs->name), DEVICE_STRING " X");
798
-
snprintf(xfs->product, sizeof(xfs->product), DEVICE_STRING " P");
799
-
snprintf(xfs->manufacturer, sizeof(xfs->manufacturer), DEVICE_STRING " M");
800
-
snprintf(xfs->serial, sizeof(xfs->serial), DEVICE_STRING " S");
801
-
xfs->source_id = 0x2EA15E115E;
802
-
803
-
// Setup realsense pipeline
804
-
struct rs_container *rsc = &rs->rsc;
805
-
rsc->error_status = NULL;
806
-
rsc->context = DO(rs2_create_context, RS2_API_VERSION);
807
-
rsc->device_list = DO(rs2_query_devices, rsc->context);
808
-
rsc->device_count = DO(rs2_get_device_count, rsc->device_list);
809
-
rsc->device_idx = device_idx;
810
-
rsc->device = DO(rs2_create_device, rsc->device_list, rsc->device_idx);
811
-
rsc->pipeline = DO(rs2_create_pipeline, rsc->context);
812
-
rsc->config = DO_(rs2_create_config);
813
-
814
-
// Set the pipeline to start specifically on the realsense device the prober selected
815
-
bool hdev_has_serial = DO(rs2_supports_device_info, rsc->device, RS2_CAMERA_INFO_SERIAL_NUMBER);
816
-
if (hdev_has_serial) {
817
-
const char *hdev_serial = DO(rs2_get_device_info, rsc->device, RS2_CAMERA_INFO_SERIAL_NUMBER);
818
-
DO(rs2_config_enable_device, rsc->config, hdev_serial);
819
-
} else {
820
-
RS_WARN("Unexpected, the realsense device in use does not provide a serial number.");
821
-
}
822
-
823
-
rs_hdev_load_stream_options_from_json(rs);
824
-
825
-
rs2_stream stream_type = rs->stream_type;
826
-
int width = rs->video_width;
827
-
int height = rs->video_height;
828
-
int fps = rs->video_fps;
829
-
rs2_format format = rs->video_format;
830
-
DO(rs2_config_enable_stream, rsc->config, RS2_STREAM_GYRO, 0, 0, 0, RS2_FORMAT_MOTION_XYZ32F, rs->gyro_fps);
831
-
DO(rs2_config_enable_stream, rsc->config, RS2_STREAM_ACCEL, 0, 0, 0, RS2_FORMAT_MOTION_XYZ32F, rs->accel_fps);
832
-
DO(rs2_config_enable_stream, rsc->config, stream_type, rs->stream1_index, width, height, format, fps);
833
-
if (rs->stereo) {
834
-
DO(rs2_config_enable_stream, rsc->config, stream_type, rs->stream2_index, width, height, format, fps);
835
-
}
836
-
837
-
// Setup sinks
838
-
rs->left_sink.push_frame = receive_left_frame;
839
-
rs->right_sink.push_frame = receive_right_frame;
840
-
rs->imu_sink.push_imu = receive_imu_sample;
841
-
rs->in_sinks.left = &rs->left_sink;
842
-
rs->in_sinks.right = &rs->right_sink;
843
-
rs->in_sinks.imu = &rs->imu_sink;
844
-
845
-
// Setup UI
846
-
u_sink_debug_init(&rs->ui_left_sink);
847
-
u_sink_debug_init(&rs->ui_right_sink);
848
-
m_ff_vec3_f32_alloc(&rs->gyro_ff, 1000);
849
-
m_ff_vec3_f32_alloc(&rs->accel_ff, 1000);
850
-
u_var_add_root(rs, "RealSense Device", false);
851
-
u_var_add_ro_text(rs, "Host SLAM", "Tracked by");
852
-
u_var_add_log_level(rs, &rs->ll, "Log Level");
853
-
u_var_add_pose(rs, &rs->pose, "SLAM Pose");
854
-
u_var_add_pose(rs, &rs->offset, "Offset Pose");
855
-
u_var_add_ro_ff_vec3_f32(rs, rs->gyro_ff, "Gyroscope");
856
-
u_var_add_ro_ff_vec3_f32(rs, rs->accel_ff, "Accelerometer");
857
-
u_var_add_sink_debug(rs, &rs->ui_left_sink, "Left Camera");
858
-
u_var_add_sink_debug(rs, &rs->ui_right_sink, "Right Camera");
859
-
860
-
// Setup node
861
-
struct xrt_frame_node *xfn = &rs->node;
862
-
xfn->break_apart = rs_hdev_node_break_apart;
863
-
xfn->destroy = rs_hdev_node_destroy;
864
-
xrt_frame_context_add(&rs->xfctx, &rs->node);
865
-
866
-
// Setup IMU synchronizer lock
867
-
os_mutex_init(&rs->partial_imu_sample.mutex);
868
-
869
-
return xfs;
870
-
}
871
-
872
-
873
-
/*
874
-
*
875
-
* SLAM tracker functionality
876
-
*
877
-
*/
878
-
879
-
//! @todo The slam tracker should be started from the tracking factory, not from here
880
-
static bool
881
-
rs_hdev_slam_track(struct rs_hdev *rs, struct xrt_prober *xp)
882
-
{
883
-
#ifdef XRT_HAVE_SLAM
884
-
struct xrt_slam_sinks *sinks = NULL;
885
-
886
-
int create_status = t_slam_create(&rs->xfctx, &rs->slam, &sinks);
887
-
if (create_status != 0) {
888
-
return false;
889
-
}
890
-
891
-
bool stream_started = xrt_fs_slam_stream_start(&rs->xfs, sinks);
892
-
if (!stream_started) {
893
-
return false;
894
-
}
895
-
896
-
int start_status = t_slam_start(rs->slam);
897
-
if (start_status != 0) {
898
-
return false;
899
-
}
900
-
return true;
901
-
#else
902
-
return false;
903
-
#endif
805
+
return rs_source_stream_start(xfs, NULL, XRT_FS_CAPTURE_TYPE_TRACKING, 0);
904
806
}
905
807
906
808
···
913
815
static void
914
816
receive_left_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
915
817
{
916
-
struct rs_hdev *rs = container_of(sink, struct rs_hdev, left_sink);
917
-
RS_TRACE("left img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
818
+
struct rs_source *rs = container_of(sink, struct rs_source, left_sink);
819
+
RS_TRACE(rs, "left img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
918
820
u_sink_debug_push_frame(&rs->ui_left_sink, xf);
919
821
if (rs->out_sinks.left) {
920
822
xrt_sink_push_frame(rs->out_sinks.left, xf);
···
924
826
static void
925
827
receive_right_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
926
828
{
927
-
struct rs_hdev *rs = container_of(sink, struct rs_hdev, right_sink);
928
-
RS_TRACE("right img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
829
+
struct rs_source *rs = container_of(sink, struct rs_source, right_sink);
830
+
RS_TRACE(rs, "right img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
929
831
u_sink_debug_push_frame(&rs->ui_right_sink, xf);
930
832
if (rs->out_sinks.right) {
931
833
xrt_sink_push_frame(rs->out_sinks.right, xf);
···
935
837
static void
936
838
receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
937
839
{
938
-
struct rs_hdev *rs = container_of(sink, struct rs_hdev, imu_sink);
840
+
struct rs_source *rs = container_of(sink, struct rs_source, imu_sink);
939
841
940
842
timepoint_ns ts = s->timestamp_ns;
941
843
struct xrt_vec3_f64 a = s->accel_m_s2;
942
844
struct xrt_vec3_f64 w = s->gyro_rad_secs;
943
-
RS_TRACE("imu t=%ld a=(%f %f %f) w=(%f %f %f)", ts, a.x, a.y, a.z, w.x, w.y, w.z);
845
+
RS_TRACE(rs, "imu t=%ld a=(%f %f %f) w=(%f %f %f)", ts, a.x, a.y, a.z, w.x, w.y, w.z);
944
846
945
847
// Push to debug UI by adjusting the timestamp to monotonic time
946
848
···
970
872
*/
971
873
972
874
static void
973
-
rs_hdev_node_break_apart(struct xrt_frame_node *node)
875
+
rs_source_node_break_apart(struct xrt_frame_node *node)
974
876
{
975
-
struct rs_hdev *rs = container_of(node, struct rs_hdev, node);
976
-
rs_hdev_stream_stop(&rs->xfs);
877
+
struct rs_source *rs = container_of(node, struct rs_source, node);
878
+
rs_source_stream_stop(&rs->xfs);
977
879
}
978
880
979
881
static void
980
-
rs_hdev_node_destroy(struct xrt_frame_node *node)
882
+
rs_source_node_destroy(struct xrt_frame_node *node)
981
883
{
982
-
struct rs_hdev *rs = container_of(node, struct rs_hdev, node);
884
+
struct rs_source *rs = container_of(node, struct rs_source, node);
885
+
RS_INFO(rs, "Destroying RealSense source");
983
886
os_mutex_destroy(&rs->partial_imu_sample.mutex);
984
887
u_var_remove_root(rs);
985
888
u_sink_debug_destroy(&rs->ui_left_sink);
986
889
u_sink_debug_destroy(&rs->ui_right_sink);
987
890
m_ff_vec3_f32_free(&rs->gyro_ff);
988
891
m_ff_vec3_f32_free(&rs->accel_ff);
989
-
990
892
rs_container_cleanup(&rs->rsc);
991
-
992
-
RS_INFO("Frame node destroyed");
893
+
free(rs);
993
894
}
994
895
995
896
···
1002
903
struct xrt_device *
1003
904
rs_hdev_create(struct xrt_prober *xp, int device_idx)
1004
905
{
1005
-
struct rs_hdev *rs = U_DEVICE_ALLOCATE(struct rs_hdev, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0);
1006
-
rs->ll = debug_get_log_option_rs_log();
1007
-
rs->pose = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}};
1008
-
rs->offset = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}};
906
+
struct rs_hdev *rh = U_DEVICE_ALLOCATE(struct rs_hdev, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0);
907
+
rh->ll = debug_get_log_option_rs_log();
908
+
rh->pose = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}};
909
+
rh->offset = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}};
1009
910
1010
-
struct xrt_device *xd = &rs->xdev;
911
+
struct xrt_device *xd = &rh->xdev;
1011
912
xd->name = XRT_DEVICE_REALSENSE;
1012
913
xd->device_type = XRT_DEVICE_TYPE_GENERIC_TRACKER;
1013
914
1014
-
snprintf(xd->str, XRT_DEVICE_NAME_LEN, "%s", DEVICE_STRING);
1015
-
snprintf(xd->serial, XRT_DEVICE_NAME_LEN, "%s", DEVICE_STRING);
915
+
snprintf(xd->str, XRT_DEVICE_NAME_LEN, "%s", RS_DEVICE_STR);
916
+
snprintf(xd->serial, XRT_DEVICE_NAME_LEN, "%s", RS_DEVICE_STR);
1016
917
1017
918
snprintf(xd->tracking_origin->name, XRT_TRACKING_NAME_LEN, "%s", RS_HOST_SLAM_TRACKER_STR);
1018
919
xd->tracking_origin->type = XRT_TRACKING_TYPE_EXTERNAL_SLAM;
···
1026
927
xd->get_tracked_pose = rs_hdev_get_tracked_pose;
1027
928
xd->destroy = rs_hdev_destroy;
1028
929
1029
-
rs_hdev_fs_create(rs, device_idx);
930
+
// Setup UI
931
+
u_var_add_root(rh, "RealSense Device", false);
932
+
u_var_add_ro_text(rh, "Host SLAM", "Tracked by");
933
+
u_var_add_log_level(rh, &rh->ll, "Log Level");
934
+
u_var_add_pose(rh, &rh->pose, "SLAM Pose");
935
+
u_var_add_pose(rh, &rh->offset, "Offset Pose");
1030
936
1031
-
bool tracked = rs_hdev_slam_track(rs, xp);
937
+
bool tracked = xp->tracking->create_tracked_slam(xp->tracking, xd, &rh->slam) >= 0;
1032
938
if (!tracked) {
1033
-
RS_WARN("Unable to setup the SLAM tracker");
939
+
RS_WARN(rh, "Unable to setup the SLAM tracker");
1034
940
rs_hdev_destroy(xd);
1035
941
return NULL;
1036
942
}
1037
943
1038
-
RS_DEBUG("Host-SLAM RealSense device created");
944
+
RS_DEBUG(rh, "Host-SLAM RealSense device created");
1039
945
1040
946
return xd;
1041
947
}
948
+
949
+
//! Create and open the frame server for IMU/camera streaming.
950
+
struct xrt_fs *
951
+
rs_source_create(struct xrt_frame_context *xfctx, int device_idx)
952
+
{
953
+
struct rs_source *rs = U_TYPED_CALLOC(struct rs_source);
954
+
rs->ll = debug_get_log_option_rs_log();
955
+
956
+
// Setup xrt_fs
957
+
struct xrt_fs *xfs = &rs->xfs;
958
+
xfs->enumerate_modes = rs_source_enumerate_modes;
959
+
xfs->configure_capture = rs_source_configure_capture;
960
+
xfs->stream_start = rs_source_stream_start;
961
+
xfs->slam_stream_start = rs_source_slam_stream_start;
962
+
xfs->stream_stop = rs_source_stream_stop;
963
+
xfs->is_running = rs_source_is_running;
964
+
snprintf(xfs->name, sizeof(xfs->name), RS_SOURCE_STR);
965
+
snprintf(xfs->product, sizeof(xfs->product), RS_SOURCE_STR " Product");
966
+
snprintf(xfs->manufacturer, sizeof(xfs->manufacturer), RS_SOURCE_STR " Manufacturer");
967
+
snprintf(xfs->serial, sizeof(xfs->serial), RS_SOURCE_STR " Serial");
968
+
xfs->source_id = 0x2EA15E115E;
969
+
970
+
// Setup realsense pipeline data
971
+
struct rs_container *rsc = &rs->rsc;
972
+
rsc->error_status = NULL;
973
+
rsc->context = DO(rs2_create_context, RS2_API_VERSION);
974
+
rsc->device_list = DO(rs2_query_devices, rsc->context);
975
+
rsc->device_count = DO(rs2_get_device_count, rsc->device_list);
976
+
rsc->device_idx = device_idx;
977
+
rsc->device = DO(rs2_create_device, rsc->device_list, rsc->device_idx);
978
+
rsc->pipeline = DO(rs2_create_pipeline, rsc->context);
979
+
rsc->config = DO_(rs2_create_config);
980
+
981
+
// Set the pipeline to start specifically on the realsense device the prober selected
982
+
bool hdev_has_serial = DO(rs2_supports_device_info, rsc->device, RS2_CAMERA_INFO_SERIAL_NUMBER);
983
+
if (hdev_has_serial) {
984
+
const char *hdev_serial = DO(rs2_get_device_info, rsc->device, RS2_CAMERA_INFO_SERIAL_NUMBER);
985
+
DO(rs2_config_enable_device, rsc->config, hdev_serial);
986
+
} else {
987
+
RS_WARN(rs, "Unexpected, the realsense device in use does not provide a serial number.");
988
+
}
989
+
990
+
// Load RealSense pipeline options from json
991
+
rs_source_load_stream_options_from_json(rs);
992
+
993
+
// Enable RealSense pipeline streams
994
+
rs2_stream stream_type = rs->stream_type;
995
+
int width = rs->video_width;
996
+
int height = rs->video_height;
997
+
int fps = rs->video_fps;
998
+
rs2_format format = rs->video_format;
999
+
DO(rs2_config_enable_stream, rsc->config, RS2_STREAM_GYRO, 0, 0, 0, RS2_FORMAT_MOTION_XYZ32F, rs->gyro_fps);
1000
+
DO(rs2_config_enable_stream, rsc->config, RS2_STREAM_ACCEL, 0, 0, 0, RS2_FORMAT_MOTION_XYZ32F, rs->accel_fps);
1001
+
DO(rs2_config_enable_stream, rsc->config, stream_type, rs->stream1_index, width, height, format, fps);
1002
+
if (rs->stereo) {
1003
+
DO(rs2_config_enable_stream, rsc->config, stream_type, rs->stream2_index, width, height, format, fps);
1004
+
}
1005
+
1006
+
// Setup sinks
1007
+
rs->left_sink.push_frame = receive_left_frame;
1008
+
rs->right_sink.push_frame = receive_right_frame;
1009
+
rs->imu_sink.push_imu = receive_imu_sample;
1010
+
rs->in_sinks.left = &rs->left_sink;
1011
+
rs->in_sinks.right = &rs->right_sink;
1012
+
rs->in_sinks.imu = &rs->imu_sink;
1013
+
1014
+
// Setup UI
1015
+
u_sink_debug_init(&rs->ui_left_sink);
1016
+
u_sink_debug_init(&rs->ui_right_sink);
1017
+
m_ff_vec3_f32_alloc(&rs->gyro_ff, 1000);
1018
+
m_ff_vec3_f32_alloc(&rs->accel_ff, 1000);
1019
+
u_var_add_root(rs, "RealSense Source", false);
1020
+
u_var_add_log_level(rs, &rs->ll, "Log Level");
1021
+
u_var_add_ro_ff_vec3_f32(rs, rs->gyro_ff, "Gyroscope");
1022
+
u_var_add_ro_ff_vec3_f32(rs, rs->accel_ff, "Accelerometer");
1023
+
u_var_add_sink_debug(rs, &rs->ui_left_sink, "Left Camera");
1024
+
u_var_add_sink_debug(rs, &rs->ui_right_sink, "Right Camera");
1025
+
1026
+
// Setup node
1027
+
struct xrt_frame_node *xfn = &rs->node;
1028
+
xfn->break_apart = rs_source_node_break_apart;
1029
+
xfn->destroy = rs_source_node_destroy;
1030
+
xrt_frame_context_add(xfctx, &rs->node);
1031
+
1032
+
// Setup IMU synchronizer lock
1033
+
os_mutex_init(&rs->partial_imu_sample.mutex);
1034
+
1035
+
return xfs;
1036
+
}
+14
-2
src/xrt/drivers/realsense/rs_interface.h
+14
-2
src/xrt/drivers/realsense/rs_interface.h
···
13
13
extern "C" {
14
14
#endif
15
15
16
+
struct xrt_frame_context;
17
+
16
18
/*!
17
19
* @defgroup drv_rs Intel RealSense driver
18
20
* @ingroup drv
19
21
*
20
22
* @brief Driver for the SLAM-capable Intel Realsense devices.
21
23
*/
22
-
23
-
#define RS_HOST_SLAM_TRACKER_STR "Host SLAM Tracker for RealSense"
24
24
25
25
#define RS_TRACKING_DISABLED -1
26
26
#define RS_TRACKING_UNSPECIFIED 0
···
34
34
*/
35
35
struct xrt_auto_prober *
36
36
rs_create_auto_prober(void);
37
+
38
+
/*!
39
+
* Creates a RealSense SLAM source from the appropriate @p device_idx.
40
+
* The streaming configuration is loaded from the global config file.
41
+
*
42
+
* @param xfctx Frame context this frameserver lifetime is tied to.
43
+
* @param device_idx Index of the realsense device to use. Usually 0 if you only
44
+
* have one RealSense device.
45
+
* @return Frameserver with SLAM streaming capabilities.
46
+
*/
47
+
struct xrt_fs *
48
+
rs_source_create(struct xrt_frame_context *xfctx, int device_idx);
37
49
38
50
/*!
39
51
* @dir drivers/realsense