From ad9503bdbdf831a6e168cd6c4cc323b5ae56e811 Mon Sep 17 00:00:00 2001
From: Tess Avitabile <tessavitabile@users.noreply.github.com>
Date: Wed, 12 Oct 2022 15:19:10 -0400
Subject: [PATCH] =?UTF-8?q?[DATA-536]=20Make=20Camera.fps=20in=20orbslam?=
 =?UTF-8?q?=20file=20generation=20use=20Hz=20instead=20=E2=80=A6=20(#1483)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 services/slam/builtin/orbslam_yaml.go      |  9 +++++
 services/slam/builtin/orbslam_yaml_test.go | 42 ++++++++++++++++++++++
 2 files changed, 51 insertions(+)

diff --git a/services/slam/builtin/orbslam_yaml.go b/services/slam/builtin/orbslam_yaml.go
index c4f97f21385..0c750cc0ac4 100644
--- a/services/slam/builtin/orbslam_yaml.go
+++ b/services/slam/builtin/orbslam_yaml.go
@@ -43,6 +43,15 @@ func (slamSvc *builtIn) orbCamMaker(camProperties *transform.PinholeCameraModel)
 		FPSCamera:   int16(slamSvc.dataRateMs),
 		FileVersion: fileVersion,
 	}
+	if slamSvc.dataRateMs <= 0 {
+		// dataRateMs is always expected to be positive, since 0 gets reset to the default, and all other
+		// values lower than the default are rejected
+		return nil, errors.Errorf("orbslam yaml generation expected dataRateMs greater than 0, got %d", slamSvc.dataRateMs)
+	}
+	orbslam.FPSCamera = int16(1000 / slamSvc.dataRateMs)
+	if orbslam.FPSCamera == 0 {
+		orbslam.FPSCamera = 1
+	}
 	distortion, ok := camProperties.Distortion.(*transform.BrownConrady)
 	if !ok {
 		return nil, utils.NewUnimplementedInterfaceError(distortion, camProperties.Distortion)
diff --git a/services/slam/builtin/orbslam_yaml_test.go b/services/slam/builtin/orbslam_yaml_test.go
index ea3f841ab10..0c54df1c4b4 100644
--- a/services/slam/builtin/orbslam_yaml_test.go
+++ b/services/slam/builtin/orbslam_yaml_test.go
@@ -80,6 +80,21 @@ func TestOrbslamYAMLNew(t *testing.T) {
 		DataRateMs:    dataRateMs,
 		Port:          "localhost:4445",
 	}
+	attrCfgGoodHighDataRateMs := &builtin.AttrConfig{
+		Algorithm: "fake_orbslamv3",
+		Sensors:   []string{"good_color_camera"},
+		ConfigParams: map[string]string{
+			"mode":              "mono",
+			"orb_n_features":    "1000",
+			"orb_scale_factor":  "1.2",
+			"orb_n_levels":      "8",
+			"orb_n_ini_th_fast": "20",
+			"orb_n_min_th_fast": "7",
+		},
+		DataDirectory: name,
+		DataRateMs:    10000,
+		Port:          "localhost:4445",
+	}
 	attrCfgBadCam := &builtin.AttrConfig{
 		Algorithm: "fake_orbslamv3",
 		Sensors:   []string{"bad_camera_intrinsics"},
@@ -124,6 +139,7 @@ func TestOrbslamYAMLNew(t *testing.T) {
 		test.That(t, orbslam.NLevels, test.ShouldEqual, 8)
 		test.That(t, orbslam.ScaleFactor, test.ShouldEqual, 1.2)
 		test.That(t, orbslam.LoadMapLoc, test.ShouldEqual, "")
+		test.That(t, orbslam.FPSCamera, test.ShouldEqual, 5)
 
 		//save a fake map for the next map using the previous timestamp
 		fakeMap = filepath.Join(name, "map", attrCfgGood.Sensors[0]+"_data_"+yamlFileTimeStampGood)
@@ -167,6 +183,32 @@ func TestOrbslamYAMLNew(t *testing.T) {
 		test.That(t, saveTimeStamp.After(oldTimeStamp), test.ShouldBeTrue)
 	})
 
+	t.Run("New orbslamv3 service with high dataRateMs", func(t *testing.T) {
+		// Create slam service
+		logger := golog.NewTestLogger(t)
+		grpcServer := setupTestGRPCServer(attrCfgGoodHighDataRateMs.Port)
+		svc, err := createSLAMService(t, attrCfgGoodHighDataRateMs, logger, false, true)
+		test.That(t, err, test.ShouldBeNil)
+
+		grpcServer.Stop()
+		test.That(t, utils.TryClose(context.Background(), svc), test.ShouldBeNil)
+
+		yamlFileTimeStampGood, yamlFilePathGood, err := findLastYAML(name)
+
+		fakeMapTimestamp = yamlFileTimeStampGood
+		test.That(t, err, test.ShouldBeNil)
+
+		yamlDataAll, err := os.ReadFile(yamlFilePathGood)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, yamlDataAll[:len(yamlFilePrefixBytes)], test.ShouldResemble, []byte(yamlFilePrefixBytes))
+
+		yamlData := bytes.Replace(yamlDataAll, []byte(yamlFilePrefixBytes), []byte(""), 1)
+		orbslam := builtin.ORBsettings{}
+		err = yaml.Unmarshal(yamlData, &orbslam)
+		// Even though the real fps is 0.1 Hz, we set it to 1
+		test.That(t, orbslam.FPSCamera, test.ShouldEqual, 1)
+	})
+
 	t.Run("New orbslamv3 service with camera that errors from bad intrinsics", func(t *testing.T) {
 		// Create slam service
 		logger := golog.NewTestLogger(t)