RSDK-2336 - [rdk -> slam] Move builtin_helpers into utils (#179)

go.mod

@@ -6,6 +6,7 @@ require (
 	github.com/edaniels/golinters v0.0.5-0.20220906153528-641155550742
 	github.com/edaniels/golog v0.0.0-20230215213219-28954395e8d0
 	github.com/edaniels/gostream v0.0.0-20230217173133-d1a1fe96076e
+	github.com/golang/geo v0.0.0-20210211234256-740aa86cb551
 	github.com/golangci/golangci-lint v1.50.1
 	github.com/pkg/errors v0.9.1
 	github.com/rhysd/actionlint v1.6.23
@@ -95,7 +96,6 @@ require (
 	github.com/gofrs/flock v0.8.1 // indirect
 	github.com/golang-jwt/jwt/v4 v4.4.3 // indirect
 	github.com/golang/freetype v0.0.0-20170609003504-e2365dfdc4a0 // indirect
-	github.com/golang/geo v0.0.0-20210211234256-740aa86cb551 // indirect
 	github.com/golang/groupcache v0.0.0-20210331224755-41bb18bfe9da // indirect
 	github.com/golang/protobuf v1.5.2 // indirect
 	github.com/golang/snappy v0.0.4 // indirect

utils/utils.go

@@ -1,7 +1,12 @@
 // Package utils contains helper functions for the slam library implementations
 package utils
 
-import "strings"
+import (
+	"strings"
+
+	"github.com/pkg/errors"
+	"go.viam.com/rdk/spatialmath"
+)
 
 // DictToString converts a dictionary to a string so
 // it can be loaded into an arg for the slam process.
@@ -16,3 +21,26 @@ func DictToString(m map[string]string) string {
 
 	return "{" + stringMap + "}"
 }
+
+// CheckQuaternionFromClientAlgo checks to see if the internal SLAM algorithm sent a quaternion. If it did, return the updated pose.
+func CheckQuaternionFromClientAlgo(pose spatialmath.Pose, componentReference string,
+	returnedExt map[string]interface{},
+) (spatialmath.Pose, string, error) {
+	// check if extra contains a quaternion. If no quaternion is found, throw an error
+	if val, ok := returnedExt["quat"]; ok {
+		q := val.(map[string]interface{})
+
+		valReal, ok1 := q["real"].(float64)
+		valIMag, ok2 := q["imag"].(float64)
+		valJMag, ok3 := q["jmag"].(float64)
+		valKMag, ok4 := q["kmag"].(float64)
+
+		if !ok1 || !ok2 || !ok3 || !ok4 {
+			return nil, "", errors.Errorf("error getting SLAM position: quaternion given, but invalid format detected, %v", q)
+		}
+		actualPose := spatialmath.NewPose(pose.Point(),
+			&spatialmath.Quaternion{Real: valReal, Imag: valIMag, Jmag: valJMag, Kmag: valKMag})
+		return actualPose, componentReference, nil
+	}
+	return nil, "", errors.Errorf("error getting SLAM position: quaternion not given, %v", returnedExt)
+}

utils/utils_test.go

@@ -1,10 +1,13 @@
 package utils
 
 import (
+	"math"
 	"sort"
 	"strings"
 	"testing"
 
+	"github.com/golang/geo/r3"
+	"go.viam.com/rdk/spatialmath"
 	"go.viam.com/test"
 )
 
@@ -32,3 +35,55 @@ func TestDictToString(t *testing.T) {
 		test.That(t, actualContents, test.ShouldResemble, expectedContents)
 	})
 }
+
+func TestBuiltinQuaternion(t *testing.T) {
+	poseSucc := spatialmath.NewPose(r3.Vector{X: 1, Y: 2, Z: 3}, &spatialmath.OrientationVector{Theta: math.Pi / 2, OX: 0, OY: 0, OZ: -1})
+	componentRefSucc := "cam"
+	t.Run("test successful quaternion from internal server", func(t *testing.T) {
+		returnedExtSucc := map[string]interface{}{
+			"quat": map[string]interface{}{
+				"real": poseSucc.Orientation().Quaternion().Real,
+				"imag": poseSucc.Orientation().Quaternion().Imag,
+				"jmag": poseSucc.Orientation().Quaternion().Jmag,
+				"kmag": poseSucc.Orientation().Quaternion().Kmag,
+			},
+		}
+
+		pose, componentRef, err := CheckQuaternionFromClientAlgo(poseSucc, componentRefSucc, returnedExtSucc)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, spatialmath.PoseAlmostEqual(poseSucc, pose), test.ShouldBeTrue)
+		test.That(t, componentRef, test.ShouldEqual, componentRefSucc)
+	})
+
+	t.Run("test failure due to quaternion not being given", func(t *testing.T) {
+		returnedExtFail := map[string]interface{}{
+			"badquat": map[string]interface{}{
+				"real": poseSucc.Orientation().Quaternion().Real,
+				"imag": poseSucc.Orientation().Quaternion().Imag,
+				"jmag": poseSucc.Orientation().Quaternion().Jmag,
+				"kmag": poseSucc.Orientation().Quaternion().Kmag,
+			},
+		}
+
+		pose, componentRef, err := CheckQuaternionFromClientAlgo(poseSucc, componentRefSucc, returnedExtFail)
+		test.That(t, err.Error(), test.ShouldContainSubstring, "error getting SLAM position: quaternion not given")
+		test.That(t, pose, test.ShouldBeNil)
+		test.That(t, componentRef, test.ShouldBeEmpty)
+	})
+
+	t.Run("test failure due to invalid quaternion format", func(t *testing.T) {
+		returnedExtFail := map[string]interface{}{
+			"quat": map[string]interface{}{
+				"realbad": poseSucc.Orientation().Quaternion().Real,
+				"imagbad": poseSucc.Orientation().Quaternion().Imag,
+				"jmagbad": poseSucc.Orientation().Quaternion().Jmag,
+				"kmagbad": poseSucc.Orientation().Quaternion().Kmag,
+			},
+		}
+
+		pose, componentRef, err := CheckQuaternionFromClientAlgo(poseSucc, componentRefSucc, returnedExtFail)
+		test.That(t, err.Error(), test.ShouldContainSubstring, "error getting SLAM position: quaternion given, but invalid format detected")
+		test.That(t, pose, test.ShouldBeNil)
+		test.That(t, componentRef, test.ShouldBeEmpty)
+	})
+}