st/prober: Hook the realsense SLAM source into the prober

This commit is contained in:
Mateo de Mayo 2021-10-23 23:03:48 -03:00 committed by Jakob Bornecrantz
parent f84629ccbc
commit f407536279
2 changed files with 51 additions and 0 deletions
src/xrt/state_trackers/prober

View file

@ -31,6 +31,10 @@
#include "euroc/euroc_interface.h"
#endif
#ifdef XRT_BUILD_DRIVER_RS
#include "realsense/rs_interface.h"
#endif
#ifdef XRT_BUILD_DRIVER_REMOTE
#include "remote/r_interface.h"
#endif
@ -53,6 +57,7 @@ DEBUG_GET_ONCE_BOOL_OPTION(qwerty_enable, "QWERTY_ENABLE", false)
DEBUG_GET_ONCE_BOOL_OPTION(qwerty_combine, "QWERTY_COMBINE", false)
DEBUG_GET_ONCE_OPTION(vf_path, "VF_PATH", NULL)
DEBUG_GET_ONCE_OPTION(euroc_path, "EUROC_PATH", NULL)
DEBUG_GET_ONCE_NUM_OPTION(rs_source_index, "RS_SOURCE_INDEX", -1)
/*
@ -996,6 +1001,14 @@ p_open_video_device(struct xrt_prober *xp,
}
#endif
#if defined(XRT_BUILD_DRIVER_RS)
int rs_source_index = debug_get_num_option_rs_source_index();
if (rs_source_index != -1) {
*out_xfs = rs_source_create(xfctx, rs_source_index);
return 0;
}
#endif
#if defined(XRT_HAVE_V4L2)
if (pdev->num_v4ls == 0) {
return -1;
@ -1022,11 +1035,14 @@ p_list_video_devices(struct xrt_prober *xp, xrt_prober_list_video_cb cb, void *p
// Video sources from drivers (at most one will be listed)
const char *vf_path = debug_get_option_vf_path();
const char *euroc_path = debug_get_option_euroc_path();
int rs_source_index = debug_get_num_option_rs_source_index();
if (vf_path != NULL) {
cb(xp, NULL, "Video File", "Collabora", vf_path, ptr);
} else if (euroc_path != NULL) {
cb(xp, NULL, "Euroc Dataset", "Collabora", euroc_path, ptr);
} else if (rs_source_index != -1) {
cb(xp, NULL, "RealSense Source", "Collabora", "", ptr);
}
// Video sources from video devices

View file

@ -31,6 +31,11 @@
DEBUG_GET_ONCE_OPTION(euroc_path, "EUROC_PATH", NULL)
#endif
#ifdef XRT_BUILD_DRIVER_RS
#include "util/u_debug.h"
DEBUG_GET_ONCE_NUM_OPTION(rs_source_index, "RS_SOURCE_INDEX", -1)
#endif
/*
*
* Structs and defines.
@ -241,6 +246,11 @@ p_factory_ensure_frameserver(struct p_factory *fact)
static bool
p_factory_ensure_slam_frameserver(struct p_factory *fact)
{
//! @todo The check for (XRT_HAVE_SLAM && XRT_BUILD_DRIVER_* &&
//! debug_flag_is_correct) is getting duplicated in: p_open_video_device,
//! p_list_video_devices, and p_factory_ensure_slam_frameserver (here) with
//! small differences. Incorrectly modifying one will mess the others.
// Factory frameserver is already in use
if (fact->xfs != NULL) {
return false;
@ -271,6 +281,31 @@ p_factory_ensure_slam_frameserver(struct p_factory *fact)
}
#endif
// SLAM tracker with RealSense frameserver
#ifdef XRT_BUILD_DRIVER_RS
if (debug_get_num_option_rs_source_index() != -1) {
struct xrt_slam_sinks empty_sinks = {0};
struct xrt_slam_sinks *sinks = &empty_sinks;
xrt_prober_open_video_device(&fact->p->base, NULL, &fact->xfctx, &fact->xfs);
assert(fact->xfs->source_id == 0x2EA15E115E && "xfs is not RealSense, unsynced open_video_device?");
#ifdef XRT_HAVE_SLAM
int ret = t_slam_create(&fact->xfctx, &fact->xts, &sinks);
if (ret != 0) {
U_LOG_W("Unable to initialize SLAM tracking, the RealSense driver will not be tracked");
}
#else
U_LOG_W("SLAM tracking support is disabled, the RealSense driver will not be tracked by host SLAM");
#endif
xrt_fs_slam_stream_start(fact->xfs, sinks);
return true;
}
#endif
// No SLAM sources were started
return false;
}