RSDK-3950 - call into cgo for getPosition

viam-cartographer.go

@@ -13,6 +13,7 @@ import (
 	"time"
 
 	"github.com/edaniels/golog"
+	"github.com/golang/geo/r3"
 	"github.com/pkg/errors"
 	"go.opencensus.io/trace"
 	"go.uber.org/zap/zapcore"
@@ -455,7 +456,7 @@ type cartographerService struct {
 	useLiveData bool
 
 	modularizationV2Enabled bool
-	cartofacade             *cartofacade.CartoFacade
+	cartofacade             cartofacade.Interface
 	cartoFacadeTimeout      time.Duration
 
 	// deprecated
@@ -496,6 +497,10 @@ func (cartoSvc *cartographerService) GetPosition(ctx context.Context) (spatialma
 	ctx, span := trace.StartSpan(ctx, "viamcartographer::cartographerService::GetPosition")
 	defer span.End()
 
+	if cartoSvc.modularizationV2Enabled {
+		return cartoSvc.getPositionModularizationV2(ctx)
+	}
+
 	req := &pb.GetPositionRequest{Name: cartoSvc.Name().ShortName()}
 
 	resp, err := cartoSvc.clientAlgo.GetPosition(ctx, req)
@@ -509,6 +514,24 @@ func (cartoSvc *cartographerService) GetPosition(ctx context.Context) (spatialma
 	return vcUtils.CheckQuaternionFromClientAlgo(pose, componentReference, returnedExt)
 }
 
+func (cartoSvc *cartographerService) getPositionModularizationV2(ctx context.Context) (spatialmath.Pose, string, error) {
+	pos, err := cartoSvc.cartofacade.GetPosition(ctx, cartoSvc.cartoFacadeTimeout)
+	if err != nil {
+		return nil, "", err
+	}
+
+	pose := spatialmath.NewPoseFromPoint(r3.Vector{X: pos.X, Y: pos.Y, Z: pos.Z})
+	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)
+}
+
 // GetPointCloudMap creates a request, recording the time, calls the slam algorithms GetPointCloudMap endpoint and returns a callback
 // function which will return the next chunk of the current pointcloud map.
 // If startup is in localization mode, the timestamp is NOT updated.

viam-cartographer_internal_test.go

@@ -4,6 +4,7 @@ import (
 	"bytes"
 	"context"
 	"testing"
+	"time"
 
 	"github.com/edaniels/golog"
 	"github.com/golang/geo/r3"
@@ -243,6 +244,112 @@ func TestGetPositionEndpoint(t *testing.T) {
 	})
 }
 
+func setMockGetPositionFunc(
+	mockCartoFacade *cartofacade.Mock,
+	pos cartofacade.GetPosition,
+) {
+	mockCartoFacade.GetPositionFunc = func(
+		ctx context.Context,
+		timeout time.Duration,
+	) (cartofacade.GetPosition, error) {
+		return pos,
+			nil
+	}
+}
+
+func checkGetPositionModularizationV2Outputs(
+	t *testing.T,
+	mockCartoFacade *cartofacade.Mock,
+	inputPose commonv1.Pose,
+	inputQuat map[string]interface{},
+	inputComponentRef string,
+	svc *cartographerService,
+	pos cartofacade.GetPosition,
+) {
+	setMockGetPositionFunc(mockCartoFacade, pos)
+
+	pose, componentReference, 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)
+	test.That(t, componentReference, test.ShouldEqual, inputComponentRef)
+}
+
+func TestGetPositionModularizationV2Endpoint(t *testing.T) {
+	svc := &cartographerService{Named: resource.NewName(slam.API, "test").AsNamed()}
+	mockCartoFacade := &cartofacade.Mock{}
+	svc.cartofacade = mockCartoFacade
+	svc.modularizationV2Enabled = true
+
+	originPos := cartofacade.GetPosition{
+		X:    0,
+		Y:    0,
+		Z:    0,
+		Real: 1.0,
+		Imag: 0.0,
+		Jmag: 0.0,
+		Kmag: 0.0,
+	}
+
+	nonOriginPos := cartofacade.GetPosition{
+		X:    5,
+		Y:    5,
+		Z:    5,
+		Real: 1.0,
+		Imag: 1.0,
+		Jmag: 0.0,
+		Kmag: 0.0,
+	}
+
+	var inputPose commonv1.Pose
+	var inputQuat map[string]interface{}
+
+	t.Run("empty component reference success", func(t *testing.T) {
+		svc.primarySensorName = ""
+		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}
+
+		checkGetPositionModularizationV2Outputs(t, mockCartoFacade, inputPose, inputQuat, "", svc, originPos)
+	})
+
+	t.Run("origin pose success", func(t *testing.T) {
+		svc.primarySensorName = "primarySensor1"
+		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}
+
+		checkGetPositionModularizationV2Outputs(t, mockCartoFacade, inputPose, inputQuat, "primarySensor1", svc, originPos)
+	})
+
+	t.Run("non origin pose success", func(t *testing.T) {
+		svc.primarySensorName = "primarySensor2"
+		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}
+
+		checkGetPositionModularizationV2Outputs(t, mockCartoFacade, inputPose, inputQuat, "primarySensor2", svc, nonOriginPos)
+	})
+
+	t.Run("error case", func(t *testing.T) {
+		svc.primarySensorName = "primarySensor3"
+		mockCartoFacade.GetPositionFunc = func(
+			ctx context.Context,
+			timeout time.Duration,
+		) (cartofacade.GetPosition, error) {
+			return cartofacade.GetPosition{}, errors.New("testError")
+		}
+
+		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, componentReference, err := svc.GetPosition(context.Background())
+
+		test.That(t, err, test.ShouldBeError, errors.New("testError"))
+		test.That(t, pose, test.ShouldBeNil)
+		test.That(t, componentReference, test.ShouldEqual, "")
+	})
+}
+
 //nolint:dupl
 func TestGetPointCloudMapEndpoint(t *testing.T) {
 	svc := &cartographerService{Named: resource.NewName(slam.API, "test").AsNamed()}