The open source OpenXR runtime
t/common: Refactor Lighthouse builder [NFC]
+151
-86
+151
-86
src/xrt/targets/common/target_builder_lighthouse.c
+151
-86
src/xrt/targets/common/target_builder_lighthouse.c
···
175
175
}
176
176
177
177
static struct xrt_slam_sinks *
178
-
valve_index_slam_track(struct lighthouse_system *lhs)
178
+
valve_index_slam_track(struct vive_device *vive_head,
179
+
struct xrt_frame_context *xfctx,
180
+
struct t_slam_calibration *slam_calib)
179
181
{
180
182
struct xrt_slam_sinks *sinks = NULL;
181
183
182
184
#ifdef XRT_FEATURE_SLAM
183
-
struct vive_device *d = (struct vive_device *)lhs->devices->base.roles.head;
184
-
185
185
struct t_slam_tracker_config config = {0};
186
186
t_slam_fill_default_config(&config);
187
187
config.cam_count = 2;
188
-
config.slam_calib = &lhs->slam_calib;
188
+
config.slam_calib = slam_calib;
189
189
190
-
int create_status = t_slam_create(&lhs->devices->xfctx, &config, &d->tracking.slam, &sinks);
190
+
int create_status = t_slam_create(xfctx, &config, &vive_head->tracking.slam, &sinks);
191
191
if (create_status != 0) {
192
192
return NULL;
193
193
}
194
194
195
-
int start_status = t_slam_start(d->tracking.slam);
195
+
int start_status = t_slam_start(vive_head->tracking.slam);
196
196
if (start_status != 0) {
197
197
return NULL;
198
198
}
···
205
205
206
206
static bool
207
207
valve_index_hand_track(struct lighthouse_system *lhs,
208
-
struct xrt_prober *xp,
208
+
struct xrt_device *head,
209
+
struct xrt_frame_context *xfctx,
209
210
struct xrt_pose head_in_left_cam,
210
211
struct t_stereo_camera_calibration *stereo_calib,
211
212
struct xrt_slam_sinks **out_sinks,
···
241
242
info.views[1].boundary.circle.normalized_radius = 0.55;
242
243
243
244
struct xrt_device *ht_device = NULL;
244
-
int create_status = ht_device_create(&lhs->devices->xfctx, //
245
-
stereo_calib, //
246
-
info, //
247
-
&sinks, //
248
-
&ht_device);
245
+
int create_status = ht_device_create( //
246
+
xfctx, //
247
+
stereo_calib, //
248
+
info, //
249
+
&sinks, //
250
+
&ht_device);
249
251
if (create_status != 0) {
250
252
LH_WARN("Failed to create hand tracking device\n");
251
253
return false;
252
254
}
253
255
254
-
ht_device =
255
-
multi_create_tracking_override(XRT_TRACKING_OVERRIDE_ATTACHED, ht_device, lhs->devices->base.roles.head,
256
-
XRT_INPUT_GENERIC_HEAD_POSE, &head_in_left_cam);
256
+
ht_device = multi_create_tracking_override( //
257
+
XRT_TRACKING_OVERRIDE_ATTACHED, //
258
+
ht_device, //
259
+
head, //
260
+
XRT_INPUT_GENERIC_HEAD_POSE, //
261
+
&head_in_left_cam); //
257
262
258
-
int created_devices = cemu_devices_create(lhs->devices->base.roles.head, ht_device, two_hands);
263
+
int created_devices = cemu_devices_create( //
264
+
head, //
265
+
ht_device, //
266
+
two_hands); //
259
267
if (created_devices != 2) {
260
268
LH_WARN("Unexpected amount of hand devices created (%d)\n", create_status);
261
269
xrt_device_destroy(&ht_device);
···
397
405
// If the HMD is a Valve Index, decide if we want visual (HT/Slam) trackers, and if so set them up.
398
406
static bool
399
407
valve_index_setup_visual_trackers(struct lighthouse_system *lhs,
408
+
struct xrt_device *head,
409
+
struct vive_device *vive_head,
410
+
struct xrt_frame_context *xfctx,
400
411
struct xrt_prober *xp,
401
412
struct xrt_slam_sinks *out_sinks,
402
413
struct xrt_device **out_devices)
···
417
428
// Initialize SLAM tracker
418
429
struct xrt_slam_sinks *slam_sinks = NULL;
419
430
if (slam_enabled) {
420
-
slam_sinks = valve_index_slam_track(lhs);
431
+
LH_ASSERT_(lhs->driver == DRIVER_VIVE);
432
+
LH_ASSERT_(vive_head != NULL);
433
+
434
+
slam_sinks = valve_index_slam_track(vive_head, xfctx, &lhs->slam_calib);
421
435
if (slam_sinks == NULL) {
422
436
lhs->vive_tstatus.slam_enabled = false;
423
437
slam_enabled = false;
···
429
443
struct xrt_slam_sinks *hand_sinks = NULL;
430
444
struct xrt_device *hand_devices[2] = {NULL};
431
445
if (hand_enabled) {
432
-
bool success =
433
-
valve_index_hand_track(lhs, xp, head_in_left_cam, stereo_calib, &hand_sinks, hand_devices);
446
+
bool success = valve_index_hand_track( //
447
+
lhs, //
448
+
head, //
449
+
xfctx, //
450
+
head_in_left_cam, //
451
+
stereo_calib, //
452
+
&hand_sinks, //
453
+
hand_devices); //
434
454
if (!success) {
435
455
lhs->vive_tstatus.hand_enabled = false;
436
456
hand_enabled = false;
···
441
461
t_stereo_camera_calibration_reference(&stereo_calib, NULL);
442
462
443
463
if (lhs->driver == DRIVER_VIVE) { // Refresh trackers status in vive driver
444
-
struct vive_device *d = (struct vive_device *)lhs->devices->base.roles.head;
445
-
vive_set_trackers_status(d, lhs->vive_tstatus);
464
+
LH_ASSERT_(vive_head != NULL);
465
+
vive_set_trackers_status(vive_head, lhs->vive_tstatus);
446
466
}
447
467
448
468
// Setup frame graph
···
452
472
struct xrt_frame_sink *entry_sbs_sink = NULL;
453
473
454
474
if (slam_enabled && hand_enabled) {
455
-
u_sink_split_create(&lhs->devices->xfctx, slam_sinks->cams[0], hand_sinks->cams[0], &entry_left_sink);
456
-
u_sink_split_create(&lhs->devices->xfctx, slam_sinks->cams[1], hand_sinks->cams[1], &entry_right_sink);
457
-
u_sink_stereo_sbs_to_slam_sbs_create(&lhs->devices->xfctx, entry_left_sink, entry_right_sink,
458
-
&entry_sbs_sink);
459
-
u_sink_create_format_converter(&lhs->devices->xfctx, XRT_FORMAT_L8, entry_sbs_sink, &entry_sbs_sink);
475
+
u_sink_split_create(xfctx, slam_sinks->cams[0], hand_sinks->cams[0], &entry_left_sink);
476
+
u_sink_split_create(xfctx, slam_sinks->cams[1], hand_sinks->cams[1], &entry_right_sink);
477
+
u_sink_stereo_sbs_to_slam_sbs_create(xfctx, entry_left_sink, entry_right_sink, &entry_sbs_sink);
478
+
u_sink_create_format_converter(xfctx, XRT_FORMAT_L8, entry_sbs_sink, &entry_sbs_sink);
460
479
} else if (slam_enabled) {
461
480
entry_left_sink = slam_sinks->cams[0];
462
481
entry_right_sink = slam_sinks->cams[1];
463
-
u_sink_stereo_sbs_to_slam_sbs_create(&lhs->devices->xfctx, entry_left_sink, entry_right_sink,
464
-
&entry_sbs_sink);
465
-
u_sink_create_format_converter(&lhs->devices->xfctx, XRT_FORMAT_L8, entry_sbs_sink, &entry_sbs_sink);
482
+
u_sink_stereo_sbs_to_slam_sbs_create(xfctx, entry_left_sink, entry_right_sink, &entry_sbs_sink);
483
+
u_sink_create_format_converter(xfctx, XRT_FORMAT_L8, entry_sbs_sink, &entry_sbs_sink);
466
484
} else if (hand_enabled) {
467
485
entry_left_sink = hand_sinks->cams[0];
468
486
entry_right_sink = hand_sinks->cams[1];
469
-
u_sink_stereo_sbs_to_slam_sbs_create(&lhs->devices->xfctx, entry_left_sink, entry_right_sink,
470
-
&entry_sbs_sink);
471
-
u_sink_create_format_converter(&lhs->devices->xfctx, XRT_FORMAT_L8, entry_sbs_sink, &entry_sbs_sink);
487
+
u_sink_stereo_sbs_to_slam_sbs_create(xfctx, entry_left_sink, entry_right_sink, &entry_sbs_sink);
488
+
u_sink_create_format_converter(xfctx, XRT_FORMAT_L8, entry_sbs_sink, &entry_sbs_sink);
472
489
} else {
473
490
LH_WARN("No visual trackers were set");
474
491
return false;
475
492
}
476
493
//! @todo Using a single slot queue is wrong for SLAM
477
-
u_sink_simple_queue_create(&lhs->devices->xfctx, entry_sbs_sink, &entry_sbs_sink);
494
+
u_sink_simple_queue_create(xfctx, entry_sbs_sink, &entry_sbs_sink);
478
495
479
496
struct xrt_slam_sinks entry_sinks = {
480
497
.cam_count = 1,
···
488
505
out_devices[0] = hand_devices[0];
489
506
out_devices[1] = hand_devices[1];
490
507
}
508
+
491
509
return true;
492
510
}
493
511
494
512
495
513
496
514
static bool
497
-
stream_data_sources(struct lighthouse_system *lhs, struct xrt_prober *xp, struct xrt_slam_sinks sinks)
515
+
stream_data_sources(struct lighthouse_system *lhs,
516
+
struct vive_device *vive_head,
517
+
struct xrt_prober *xp,
518
+
struct xrt_slam_sinks sinks)
498
519
{
499
520
// Open frame server
500
521
xrt_prober_list_video_devices(xp, on_video_device, lhs);
···
503
524
return false;
504
525
}
505
526
506
-
bool success = false;
507
527
uint32_t mode = get_selected_mode(lhs->xfs);
508
528
509
529
// If SLAM is enabled (only on vive driver) we intercept the data sink
510
530
if (lhs->vive_tstatus.slam_enabled) {
511
-
struct vive_device *d = (struct vive_device *)lhs->devices->base.roles.head;
512
-
LH_ASSERT_(d != NULL && d->source != NULL);
513
-
struct vive_source *vs = d->source;
514
-
vive_source_hook_into_sinks(vs, &sinks);
531
+
LH_ASSERT_(lhs->driver == DRIVER_VIVE);
532
+
LH_ASSERT_(vive_head != NULL);
533
+
LH_ASSERT_(vive_head->source != NULL);
534
+
535
+
vive_source_hook_into_sinks(vive_head->source, &sinks);
515
536
}
516
537
517
-
success = xrt_fs_stream_start(lhs->xfs, sinks.cams[0], XRT_FS_CAPTURE_TYPE_TRACKING, mode);
518
-
519
-
if (!success) {
538
+
bool bret = xrt_fs_stream_start(lhs->xfs, sinks.cams[0], XRT_FS_CAPTURE_TYPE_TRACKING, mode);
539
+
if (!bret) {
520
540
LH_ERROR("Unable to start data streaming");
541
+
return false;
521
542
}
522
543
523
-
return success;
544
+
return true;
524
545
}
525
546
526
547
static void
···
562
583
{
563
584
struct lighthouse_system *lhs = (struct lighthouse_system *)xb;
564
585
lhs->devices = u_system_devices_allocate();
565
-
struct u_system_devices *usysd = lhs->devices;
586
+
struct xrt_system_devices *xsysd = &lhs->devices->base;
587
+
struct xrt_frame_context *xfctx = &lhs->devices->xfctx;
566
588
567
589
xrt_result_t result = XRT_SUCCESS;
568
590
569
591
if (out_xsysd == NULL || *out_xsysd != NULL) {
570
592
LH_ERROR("Invalid output system pointer");
571
593
result = XRT_ERROR_DEVICE_CREATION_FAILED;
572
-
goto end;
594
+
goto end_err;
573
595
}
574
596
575
597
// Decide whether to initialize the SLAM tracker
···
599
621
switch (lhs->driver) {
600
622
case DRIVER_STEAMVR: {
601
623
#ifdef XRT_BUILD_DRIVER_STEAMVR_LIGHTHOUSE
602
-
usysd->base.xdev_count += steamvr_lh_get_devices(&usysd->base.xdevs[usysd->base.xdev_count]);
624
+
xsysd->xdev_count += steamvr_lh_get_devices(&xsysd->xdevs[xsysd->xdev_count]);
603
625
#endif
604
626
break;
605
627
}
606
628
case DRIVER_SURVIVE: {
607
629
#ifdef XRT_BUILD_DRIVER_SURVIVE
608
-
usysd->base.xdev_count +=
609
-
survive_get_devices(&usysd->base.xdevs[usysd->base.xdev_count], &lhs->hmd_config);
630
+
xsysd->xdev_count += survive_get_devices(&xsysd->xdevs[xsysd->xdev_count], &lhs->hmd_config);
610
631
#endif
611
632
break;
612
633
}
···
618
639
result = xrt_prober_lock_list(xp, &xpdevs, &xpdev_count);
619
640
if (result != XRT_SUCCESS) {
620
641
LH_ERROR("Unable to lock the prober dev list");
621
-
goto end;
642
+
goto end_err;
622
643
}
623
644
for (size_t i = 0; i < xpdev_count; i++) {
624
645
struct xrt_prober_device *device = xpdevs[i];
···
633
654
case VIVE_PRO_MAINBOARD_PID:
634
655
case VIVE_PRO2_MAINBOARD_PID:
635
656
case VIVE_PRO_LHR_PID: {
636
-
struct vive_source *vs = vive_source_create(&usysd->xfctx);
637
-
int num_devices =
638
-
vive_found(xp, xpdevs, xpdev_count, i, NULL, lhs->vive_tstatus, vs,
639
-
&lhs->hmd_config, &usysd->base.xdevs[usysd->base.xdev_count]);
640
-
usysd->base.xdev_count += num_devices;
641
-
657
+
struct vive_source *vs = vive_source_create(xfctx);
658
+
int num_devices = vive_found( //
659
+
xp, //
660
+
xpdevs, //
661
+
xpdev_count, //
662
+
i, //
663
+
NULL, //
664
+
lhs->vive_tstatus, //
665
+
vs, //
666
+
&lhs->hmd_config, //
667
+
&xsysd->xdevs[xsysd->xdev_count]); //
668
+
xsysd->xdev_count += num_devices;
642
669
} break;
643
670
case VIVE_WATCHMAN_DONGLE:
644
671
case VIVE_WATCHMAN_DONGLE_GEN2: {
645
-
int num_devices = vive_controller_found(xp, xpdevs, xpdev_count, i, NULL,
646
-
&usysd->base.xdevs[usysd->base.xdev_count]);
647
-
usysd->base.xdev_count += num_devices;
672
+
int num_devices = vive_controller_found( //
673
+
xp, //
674
+
xpdevs, //
675
+
xpdev_count, //
676
+
i, //
677
+
NULL, //
678
+
&xsysd->xdevs[xsysd->xdev_count]); //
679
+
xsysd->xdev_count += num_devices;
648
680
} break;
649
681
}
650
682
}
···
658
690
}
659
691
}
660
692
693
+
// Device indices.
661
694
int head_idx = -1;
662
695
int left_idx = -1;
663
696
int right_idx = -1;
664
697
665
-
u_device_assign_xdev_roles(usysd->base.xdevs, usysd->base.xdev_count, &head_idx, &left_idx, &right_idx);
698
+
u_device_assign_xdev_roles(xsysd->xdevs, xsysd->xdev_count, &head_idx, &left_idx, &right_idx);
666
699
667
700
if (head_idx < 0) {
668
701
LH_ERROR("Unable to find HMD");
669
702
result = XRT_ERROR_DEVICE_CREATION_FAILED;
670
-
goto end;
703
+
goto end_err;
671
704
}
672
-
usysd->base.roles.head = usysd->base.xdevs[head_idx];
705
+
706
+
// Devices to populate.
707
+
struct xrt_device *head = NULL;
708
+
struct xrt_device *left = NULL, *right = NULL;
709
+
struct xrt_device *left_ht = NULL, *right_ht = NULL;
710
+
711
+
// Always have a head.
712
+
head = xsysd->xdevs[head_idx];
673
713
674
714
// It's okay if we didn't find controllers
675
715
if (left_idx >= 0) {
676
716
lhs->vive_tstatus.controllers_found = true;
677
-
usysd->base.roles.left = usysd->base.xdevs[left_idx];
678
-
usysd->base.roles.hand_tracking.left = u_system_devices_get_ht_device_left(&usysd->base);
717
+
left = xsysd->xdevs[left_idx];
718
+
left_ht = u_system_devices_get_ht_device_left(xsysd);
679
719
}
680
720
681
721
if (right_idx >= 0) {
682
722
lhs->vive_tstatus.controllers_found = true;
683
-
usysd->base.roles.right = usysd->base.xdevs[right_idx];
684
-
usysd->base.roles.hand_tracking.right = u_system_devices_get_ht_device_right(&usysd->base);
723
+
right = xsysd->xdevs[right_idx];
724
+
right_ht = u_system_devices_get_ht_device_right(xsysd);
685
725
}
686
726
687
727
if (lhs->is_valve_index) {
···
704
744
if (lhs->hmd_config == NULL) {
705
745
// This should NEVER happen, but we're not writing Rust.
706
746
U_LOG_E("Didn't get a vive config? Not creating visual trackers.");
707
-
goto end;
747
+
goto end_valve_index;
708
748
}
709
749
if (!lhs->hmd_config->cameras.valid) {
710
750
U_LOG_I(
711
751
"HMD didn't have cameras or didn't have a valid camera calibration. Not creating visual "
712
752
"trackers.");
713
-
goto end;
753
+
goto end_valve_index;
714
754
}
715
755
716
756
struct xrt_slam_sinks sinks = {0};
717
757
struct xrt_device *hand_devices[2] = {NULL};
718
-
success = valve_index_setup_visual_trackers(lhs, xp, &sinks, hand_devices);
758
+
struct vive_device *vive_head = NULL;
759
+
760
+
// Only do cast if we are using the vive driver.
761
+
if (lhs->driver == DRIVER_VIVE) {
762
+
vive_head = (struct vive_device *)head;
763
+
}
764
+
765
+
success = valve_index_setup_visual_trackers( //
766
+
lhs, //
767
+
head, //
768
+
vive_head, //
769
+
xfctx, //
770
+
xp, //
771
+
&sinks, //
772
+
hand_devices); //
719
773
if (!success) {
720
774
result = XRT_SUCCESS; // We won't have trackers, but creation was otherwise ok
721
-
goto end;
775
+
goto end_valve_index;
722
776
}
723
777
724
778
if (lhs->vive_tstatus.hand_enabled) {
725
779
if (hand_devices[0] != NULL) {
726
-
usysd->base.roles.left = hand_devices[0];
727
-
usysd->base.roles.hand_tracking.left = hand_devices[0];
728
-
usysd->base.xdevs[usysd->base.xdev_count++] = hand_devices[0];
780
+
xsysd->xdevs[xsysd->xdev_count++] = hand_devices[0];
781
+
left = hand_devices[0];
782
+
left_ht = hand_devices[0];
729
783
}
730
784
731
785
if (hand_devices[1] != NULL) {
732
-
usysd->base.roles.right = hand_devices[1];
733
-
usysd->base.roles.hand_tracking.right = hand_devices[1];
734
-
usysd->base.xdevs[usysd->base.xdev_count++] = hand_devices[1];
786
+
xsysd->xdevs[xsysd->xdev_count++] = hand_devices[1];
787
+
right = hand_devices[1];
788
+
right_ht = hand_devices[1];
735
789
}
736
790
}
737
791
738
-
success = stream_data_sources(lhs, xp, sinks);
792
+
success = stream_data_sources(lhs, vive_head, xp, sinks);
739
793
if (!success) {
740
794
result = XRT_SUCCESS; // We can continue after freeing trackers
741
-
goto end;
795
+
goto end_valve_index;
742
796
}
743
797
}
744
-
798
+
end_valve_index:
745
799
800
+
// Check for error just in case.
801
+
if (result != XRT_SUCCESS) {
802
+
goto end_err;
803
+
}
746
804
747
-
end:
748
805
if (!lhs->vive_tstatus.hand_enabled) {
749
806
// We only want to try to add opengloves if we aren't optically tracking hands
750
-
try_add_opengloves(usysd);
807
+
try_add_opengloves(lhs->devices);
751
808
}
752
809
753
-
if (result == XRT_SUCCESS) {
754
-
*out_xsysd = &usysd->base;
755
-
u_builder_create_space_overseer(&usysd->base, out_xso);
756
-
} else {
757
-
u_system_devices_destroy(&usysd);
758
-
}
810
+
// Assign to role(s).
811
+
xsysd->roles.head = head;
812
+
xsysd->roles.left = left;
813
+
xsysd->roles.hand_tracking.left = left_ht;
814
+
xsysd->roles.right = right;
815
+
xsysd->roles.hand_tracking.right = right_ht;
816
+
817
+
*out_xsysd = xsysd;
818
+
u_builder_create_space_overseer(xsysd, out_xso);
819
+
820
+
return XRT_SUCCESS;
821
+
822
+
end_err:
823
+
xrt_system_devices_destroy(&xsysd);
759
824
760
825
return result;
761
826
}