t/euroc: Fix crash when recording euroc dataset by providing UI button

Previously, a crash occurred in some situations when the second sample was
written before the first sample was able to create the necessary CSV files.

src/xrt/auxiliary/tracking/t_euroc_recorder.cpp

@@ -12,6 +12,7 @@
 #include "os/os_time.h"
 #include "util/u_frame.h"
 #include "util/u_sink.h"
+#include "util/u_var.h"
 
 #include <cassert>
 #include <ctime>
@@ -33,8 +34,11 @@ using std::filesystem::create_directories;
 struct euroc_recorder
 {
 	struct xrt_frame_node node;
-	string path;         //!< Destination path for the dataset
-	bool first_received; //!< Whether we have received the first sample
+	string path; //!< Destination path for the dataset
+
+	bool recording;                    //!< Whether samples are being recorded
+	bool files_created;                //!< Whether the dataset directory structure has been created
+	struct u_var_button recording_btn; //!< UI button to start/stop `recording`
 
 	// Cloner sinks: copy frame to heap for quick release of the original
 	struct xrt_slam_sinks cloner_queues; //!< Queue sinks that write into cloner sinks
@@ -67,11 +71,11 @@ struct euroc_recorder
 static void
 euroc_recorder_try_mkfiles(struct euroc_recorder *er)
 {
-	// Create directory structure and files only on first received frame
-	if (er->first_received) {
+	// Create directory structure and files only once
+	if (er->files_created) {
 		return;
 	}
-	er->first_received = true;
+	er->files_created = true;
 
 	string path = er->path;
 
@@ -110,7 +114,6 @@ extern "C" void
 euroc_recorder_save_imu(xrt_imu_sink *sink, struct xrt_imu_sample *sample)
 {
 	euroc_recorder *er = container_of(sink, euroc_recorder, writer_imu_sink);
-	euroc_recorder_try_mkfiles(er);
 
 	timepoint_ns ts = sample->timestamp_ns;
 	xrt_vec3_f64 a = sample->accel_m_s2;
@@ -124,8 +127,6 @@ euroc_recorder_save_imu(xrt_imu_sink *sink, struct xrt_imu_sample *sample)
 static void
 euroc_recorder_save_frame(euroc_recorder *er, struct xrt_frame *frame, bool is_left)
 {
-	euroc_recorder_try_mkfiles(er);
-
 	string cam_name = is_left ? "cam0" : "cam1";
 	uint64_t ts = frame->timestamp;
 
@@ -169,6 +170,11 @@ euroc_recorder_receive_imu(xrt_imu_sink *sink, struct xrt_imu_sample *sample)
 	// sinks so we use an std::queue to temporarily store IMU samples, later we
 	// write them to disk when writing left frames.
 	euroc_recorder *er = container_of(sink, euroc_recorder, cloner_imu_sink);
+
+	if (!er->recording) {
+		return;
+	}
+
 	er->imu_queue.push(*sample);
 }
 
@@ -176,6 +182,10 @@ euroc_recorder_receive_imu(xrt_imu_sink *sink, struct xrt_imu_sample *sample)
 static void
 euroc_recorder_receive_frame(euroc_recorder *er, struct xrt_frame *src_frame, bool is_left)
 {
+	if (!er->recording) {
+		return;
+	}
+
 	// Let's clone the frame so that we can release the src_frame quickly
 	xrt_frame *copy = nullptr;
 	u_frame_clone(src_frame, &copy);
@@ -228,10 +238,15 @@ euroc_recorder_node_destroy(struct xrt_frame_node *node)
  */
 
 extern "C" xrt_slam_sinks *
-euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path)
+euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path, bool record_from_start)
 {
 	struct euroc_recorder *er = new euroc_recorder{};
 
+	er->recording = record_from_start;
+	if (record_from_start) {
+		euroc_recorder_try_mkfiles(er);
+	}
+
 	struct xrt_frame_node *xfn = &er->node;
 	xfn->break_apart = euroc_recorder_node_break_apart;
 	xfn->destroy = euroc_recorder_node_destroy;
@@ -276,3 +291,22 @@ euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path)
 	xrt_slam_sinks *public_sinks = &er->cloner_queues;
 	return public_sinks;
 }
+
+static void
+euroc_recorder_btn_cb(void *ptr)
+{
+	euroc_recorder *er = (euroc_recorder *)ptr;
+	euroc_recorder_try_mkfiles(er);
+	er->recording = !er->recording;
+	snprintf(er->recording_btn.label, sizeof(er->recording_btn.label),
+	         er->recording ? "Stop recording" : "Record EuRoC dataset");
+}
+
+extern "C" void
+euroc_recorder_add_ui(struct xrt_slam_sinks *public_sinks, void *root)
+{
+	euroc_recorder *er = container_of(public_sinks, euroc_recorder, cloner_queues);
+	er->recording_btn.cb = euroc_recorder_btn_cb;
+	er->recording_btn.ptr = er;
+	u_var_add_button(root, &er->recording_btn, er->recording ? "Stop recording" : "Record EuRoC dataset");
+}

src/xrt/auxiliary/tracking/t_euroc_recorder.h

@@ -24,12 +24,22 @@ extern "C" {
  *
  * @param xfctx Frame context for the sinks.
  * @param record_path Directory name to save the dataset or NULL for a default based on the current datetime.
+ * @param record_from_start Whether to start recording immediately on creation.
  * @return struct xrt_slam_sinks* Sinks to push samples to for recording.
  *
  * @ingroup aux_tracking
  */
 struct xrt_slam_sinks *
-euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path);
+euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path, bool record_from_start);
+
+/*!
+ * Add EuRoC recorder UI button to start recording after creation.
+ *
+ * @param er The sinks returned by @ref euroc_recorder_create
+ * @param root The pointer to add UI button to
+ */
+void
+euroc_recorder_add_ui(struct xrt_slam_sinks *er, void *root);
 
 #ifdef __cplusplus
 }

src/xrt/auxiliary/tracking/t_tracker_slam.cpp

@@ -302,7 +302,6 @@ struct TrackerSlam
 	MatFrame *cv_wrapper;           //!< Wraps a xrt_frame in a cv::Mat to send to the SLAM system
 
 	struct xrt_slam_sinks *euroc_recorder; //!< EuRoC dataset recording sinks
-	bool euroc_record;                     //!< When true, samples will be saved to disk in EuRoC format
 
 	// Used mainly for checking that the timestamps come in order
 	timepoint_ns last_imu_ts = INT64_MIN;   //!< Last received IMU sample timestamp
@@ -748,8 +747,8 @@ setup_ui(TrackerSlam &t)
 	u_var_add_root(&t, "SLAM Tracker", true);
 	u_var_add_log_level(&t, &t.log_level, "Log Level");
 	u_var_add_bool(&t, &t.submit, "Submit data to SLAM");
-	u_var_add_bool(&t, &t.euroc_record, "Record as EuRoC");
 	u_var_add_bool(&t, &t.gt.override_tracking, "Track with ground truth (if available)");
+	euroc_recorder_add_ui(t.euroc_recorder, &t);
 
 	u_var_add_gui_header(&t, NULL, "Trajectory Filter");
 	u_var_add_bool(&t, &t.filter.use_moving_average_filter, "Enable moving average filter");
@@ -850,9 +849,7 @@ t_slam_imu_sink_push(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
 	}
 	SLAM_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);
 
-	if (t.euroc_record) {
-		xrt_sink_push_imu(t.euroc_recorder->imu, s);
-	}
+	xrt_sink_push_imu(t.euroc_recorder->imu, s);
 
 	struct xrt_vec3 gyro = {(float)w.x, (float)w.y, (float)w.z};
 	struct xrt_vec3 accel = {(float)a.x, (float)a.y, (float)a.z};
@@ -890,10 +887,7 @@ t_slam_frame_sink_push_left(struct xrt_frame_sink *sink, struct xrt_frame *frame
 {
 	auto &t = *container_of(sink, TrackerSlam, left_sink);
 	push_frame(t, frame, true);
-
-	if (t.euroc_record) {
-		xrt_sink_push_frame(t.euroc_recorder->left, frame);
-	}
+	xrt_sink_push_frame(t.euroc_recorder->left, frame);
 }
 
 extern "C" void
@@ -901,10 +895,7 @@ t_slam_frame_sink_push_right(struct xrt_frame_sink *sink, struct xrt_frame *fram
 {
 	auto &t = *container_of(sink, TrackerSlam, right_sink);
 	push_frame(t, frame, false);
-
-	if (t.euroc_record) {
-		xrt_sink_push_frame(t.euroc_recorder->right, frame);
-	}
+	xrt_sink_push_frame(t.euroc_recorder->right, frame);
 }
 
 extern "C" void
@@ -1039,7 +1030,7 @@ t_slam_create(struct xrt_frame_context *xfctx,
 
 	xrt_frame_context_add(xfctx, &t.node);
 
-	t.euroc_recorder = euroc_recorder_create(xfctx, NULL);
+	t.euroc_recorder = euroc_recorder_create(xfctx, NULL, false);
 
 	t.pred_type = config->prediction;