deal with type conversion

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"
@@ -413,6 +414,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)
@@ -426,6 +431,17 @@ 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 spatialmath.NewPose(r3.Vector{}, spatialmath.NewOrientationVector()), "", err
+	}
+
+	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}
+	return vcUtils.CheckQuaternionFromClientAlgo(pose, cartoSvc.primarySensorName, returnedExt)
+}
+
 // GetPointCloudMap creates a request, calls the slam algorithms GetPointCloudMap endpoint and returns a callback
 // function which will return the next chunk of the current pointcloud map.
 func (cartoSvc *cartographerService) GetPointCloudMap(ctx context.Context) (func() ([]byte, error), error) {