unit test

viam-cartographer.go

@@ -121,7 +121,7 @@ func TerminateCartoLib() error {
 
 func initSensorProcess(cancelCtx context.Context, cartoSvc *cartographerService) {
 	spConfig := sensorprocess.Config{
-		Cartofacade:      &cartoSvc.cartofacade,
+		Cartofacade:      cartoSvc.cartofacade,
 		Lidar:            cartoSvc.lidar,
 		LidarName:        cartoSvc.primarySensorName,
 		DataRateMs:       cartoSvc.dataRateMs,
@@ -334,7 +334,7 @@ func initCartoFacade(cancelCtx context.Context, cartoSvc *cartographerService) (
 	if err != nil {
 		return nil, err
 	}
-	cartoSvc.cartofacade = cf
+	cartoSvc.cartofacade = &cf
 
 	return cartoSvc, nil
 }
@@ -387,7 +387,7 @@ type cartographerService struct {
 	useLiveData         bool
 
 	modularizationV2Enabled bool
-	cartofacade             cartofacade.CartoFacade
+	cartofacade             cartofacade.Interface
 	cartoFacadeTimeout      time.Duration
 
 	port       string
@@ -438,7 +438,7 @@ func (cartoSvc *cartographerService) getPositionModularizationV2(ctx context.Con
 	}
 
 	pose := spatialmath.NewPoseFromPoint(r3.Vector{X: pos.X, Y: pos.Y, Z: pos.Z})
-	returnedExt := map[string]interface{}{"real": pos.Real, "imag": pos.Imag, "jmag": pos.Jmag, "kmag": pos.Kmag}
+	returnedExt := map[string]interface{}{"quat": map[string]interface{}{"real": pos.Real, "imag": pos.Imag, "jmag": pos.Jmag, "kmag": pos.Kmag}}
 	return vcUtils.CheckQuaternionFromClientAlgo(pose, cartoSvc.primarySensorName, returnedExt)
 }
 

viam-cartographer_internal_test.go

@@ -4,6 +4,7 @@ import (
 	"bytes"
 	"context"
 	"testing"
+	"time"
 
 	"github.com/golang/geo/r3"
 	"github.com/pkg/errors"
@@ -16,6 +17,7 @@ import (
 	"google.golang.org/grpc"
 	"google.golang.org/protobuf/types/known/structpb"
 
+	"github.com/viamrobotics/viam-cartographer/cartofacade"
 	inject "github.com/viamrobotics/viam-cartographer/internal/inject"
 )
 
@@ -241,6 +243,57 @@ func TestGetPositionEndpoint(t *testing.T) {
 	})
 }
 
+func TestGetPositionModularizationV2Endpoint(t *testing.T) {
+	svc := &cartographerService{Named: resource.NewName(slam.API, "test").AsNamed()}
+	mockCartoFacade := &cartofacade.Mock{}
+	svc.cartofacade = mockCartoFacade
+	svc.modularizationV2Enabled = true
+
+	var inputPose commonv1.Pose
+	var inputQuat map[string]interface{}
+
+	t.Run("successful client", func(t *testing.T) {
+
+		t.Run("origin pose success", func(t *testing.T) {
+			mockCartoFacade.GetPositionFunc = func(
+				ctx context.Context,
+				timeout time.Duration,
+			) (cartofacade.GetPosition, error) {
+				return cartofacade.GetPosition{X: 0, Y: 0, Z: 0, Real: 1.0, Imag: 0.0, Jmag: 0.0, Kmag: 0.0}, nil
+			}
+
+			inputPose = commonv1.Pose{X: 0, Y: 0, Z: 0, OX: 0, OY: 0, OZ: 1, Theta: 0}
+			inputQuat = map[string]interface{}{"real": 1.0, "imag": 0.0, "jmag": 0.0, "kmag": 0.0}
+			pose, _, err := svc.GetPosition(context.Background())
+			test.That(t, err, test.ShouldBeNil)
+			expectedPose := spatialmath.NewPose(
+				r3.Vector{X: inputPose.X, Y: inputPose.Y, Z: inputPose.Z},
+				makeQuaternionFromGenericMap(inputQuat),
+			)
+			test.That(t, pose, test.ShouldResemble, expectedPose)
+		})
+
+		t.Run("non origin pose success", func(t *testing.T) {
+			mockCartoFacade.GetPositionFunc = func(
+				ctx context.Context,
+				timeout time.Duration,
+			) (cartofacade.GetPosition, error) {
+				return cartofacade.GetPosition{X: 5, Y: 5, Z: 5, Real: 1.0, Imag: 1.0, Jmag: 0.0, Kmag: 0.0}, nil
+			}
+
+			inputPose = commonv1.Pose{X: 5, Y: 5, Z: 5, OX: 0, OY: 0, OZ: 1, Theta: 0}
+			inputQuat = map[string]interface{}{"real": 1.0, "imag": 1.0, "jmag": 0.0, "kmag": 0.0}
+			pose, _, err := svc.GetPosition(context.Background())
+			test.That(t, err, test.ShouldBeNil)
+			expectedPose := spatialmath.NewPose(
+				r3.Vector{X: inputPose.X, Y: inputPose.Y, Z: inputPose.Z},
+				makeQuaternionFromGenericMap(inputQuat),
+			)
+			test.That(t, pose, test.ShouldResemble, expectedPose)
+		})
+	})
+}
+
 //nolint:dupl
 func TestGetPointCloudMapEndpoint(t *testing.T) {
 	svc := &cartographerService{Named: resource.NewName(slam.API, "test").AsNamed()}