RSDK-3965 - call into cgo for getPointCloudMap (#193)

viam-modules · Jul 12, 2023 · 575bc4d · 575bc4d
1 parent 7afd4a1
commit 575bc4d
Show file tree

Hide file tree

Showing 3 changed files with 130 additions and 9 deletions.
diff --git a/viam-cartographer.go b/viam-cartographer.go
@@ -4,6 +4,7 @@ package viamcartographer
 
 import (
 	"bufio"
+	"bytes"
 	"context"
 	"io"
 	"os"
@@ -54,6 +55,7 @@ const (
 	parsePortMaxTimeoutSec               = 60
 	localhost0                           = "localhost:0"
 	defaultCartoFacadeTimeout            = 5 * time.Second
+	chunkSizeBytes                       = 1 * 1024 * 1024
 )
 
 var defaultCartoAlgoCfg = cartofacade.CartoAlgoConfig{
@@ -558,9 +560,32 @@ func (cartoSvc *cartographerService) GetPointCloudMap(ctx context.Context) (func
 	if !cartoSvc.localizationMode {
 		cartoSvc.mapTimestamp = time.Now().UTC()
 	}
+
+	if cartoSvc.modularizationV2Enabled {
+		return cartoSvc.getPointCloudMapModularizationV2(ctx)
+	}
 	return grpchelper.GetPointCloudMapCallback(ctx, cartoSvc.Name().ShortName(), cartoSvc.clientAlgo)
 }
 
+func (cartoSvc *cartographerService) getPointCloudMapModularizationV2(ctx context.Context) (func() ([]byte, error), error) {
+	chunk := make([]byte, chunkSizeBytes)
+	pc, err := cartoSvc.cartofacade.GetPointCloudMap(ctx, cartoSvc.cartoFacadeTimeout)
+	if err != nil {
+		return nil, err
+	}
+
+	pointcloudReader := bytes.NewReader(pc)
+
+	f := func() ([]byte, error) {
+		bytesRead, err := pointcloudReader.Read(chunk)
+		if err != nil {
+			return nil, err
+		}
+		return chunk[:bytesRead], err
+	}
+	return f, nil
+}
+
 // GetInternalState creates a request, calls the slam algorithms GetInternalState endpoint and returns a callback
 // function which will return the next chunk of the current internal state of the slam algo.
 func (cartoSvc *cartographerService) GetInternalState(ctx context.Context) (func() ([]byte, error), error) {

diff --git a/viam-cartographer_internal_test.go b/viam-cartographer_internal_test.go
@@ -3,6 +3,7 @@ package viamcartographer
 import (
 	"bytes"
 	"context"
+	"os"
 	"testing"
 	"time"
 
@@ -15,6 +16,7 @@ import (
 	"go.viam.com/rdk/services/slam"
 	"go.viam.com/rdk/spatialmath"
 	"go.viam.com/test"
+	"go.viam.com/utils/artifact"
 	"google.golang.org/grpc"
 	"google.golang.org/protobuf/types/known/structpb"
 
@@ -428,6 +430,79 @@ func TestGetPointCloudMapEndpoint(t *testing.T) {
 	})
 }
 
+func setMockGetPointCloudFunc(
+	mock *cartofacade.Mock,
+	pc []byte,
+) {
+	mock.GetPointCloudMapFunc = func(
+		ctx context.Context,
+		timeout time.Duration,
+	) ([]byte, error) {
+		return pc, nil
+	}
+}
+
+func TestGetPointCloudMapEndpointModularizationV2Endpoint(t *testing.T) {
+	svc := &cartographerService{Named: resource.NewName(slam.API, "test").AsNamed()}
+	mockCartoFacade := &cartofacade.Mock{}
+
+	svc.cartofacade = mockCartoFacade
+	svc.modularizationV2Enabled = true
+
+	t.Run("pointcloud smaller than 1 mb limit - success", func(t *testing.T) {
+		file := "viam-cartographer/outputs/viam-office-02-22-3/pointcloud/pointcloud_0.pcd"
+		inputPointCloudMapBytes, err := os.ReadFile(artifact.MustPath(file))
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, len(inputPointCloudMapBytes), test.ShouldBeLessThan, 1024*1024)
+
+		setMockGetPointCloudFunc(mockCartoFacade, inputPointCloudMapBytes)
+		callback, err := svc.GetPointCloudMap(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		pointCloudMapBytes, err := slam.HelperConcatenateChunksToFull(callback)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, pointCloudMapBytes, test.ShouldResemble, inputPointCloudMapBytes)
+	})
+
+	t.Run("pointcloud larger than 1 mb limit - success", func(t *testing.T) {
+		file := "viam-cartographer/outputs/viam-office-02-22-3/pointcloud/pointcloud_1.pcd"
+		inputPointCloudMapBytes, err := os.ReadFile(artifact.MustPath(file))
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, len(inputPointCloudMapBytes), test.ShouldBeGreaterThan, 1024*1024)
+
+		setMockGetPointCloudFunc(mockCartoFacade, inputPointCloudMapBytes)
+		callback, err := svc.GetPointCloudMap(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		pointCloudMapBytes, err := slam.HelperConcatenateChunksToFull(callback)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, pointCloudMapBytes, test.ShouldResemble, inputPointCloudMapBytes)
+	})
+
+	t.Run("no bytes success", func(t *testing.T) {
+		setMockGetPointCloudFunc(mockCartoFacade, []byte{})
+
+		callback, err := svc.GetPointCloudMap(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		pointCloudMapBytes, err := slam.HelperConcatenateChunksToFull(callback)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, pointCloudMapBytes, test.ShouldBeNil)
+	})
+
+	t.Run("cartofacade error", func(t *testing.T) {
+		setMockGetPointCloudFunc(mockCartoFacade, []byte{})
+
+		mockCartoFacade.GetPointCloudMapFunc = func(
+			ctx context.Context,
+			timeout time.Duration,
+		) ([]byte, error) {
+			return nil, errors.New("test")
+		}
+
+		callback, err := svc.GetPointCloudMap(context.Background())
+		test.That(t, callback, test.ShouldBeNil)
+		test.That(t, err, test.ShouldBeError, errors.New("test"))
+	})
+}
+
 //nolint:dupl
 func TestGetInternalStateEndpoint(t *testing.T) {
 	svc := &cartographerService{Named: resource.NewName(slam.API, "test").AsNamed()}

diff --git a/viam-cartographer_test.go b/viam-cartographer_test.go
@@ -18,6 +18,7 @@ import (
 	"github.com/edaniels/golog"
 	"github.com/pkg/errors"
 	viamgrpc "go.viam.com/rdk/grpc"
+	"go.viam.com/rdk/services/slam"
 	"go.viam.com/test"
 	"go.viam.com/utils/artifact"
 	"google.golang.org/grpc"
@@ -349,10 +350,20 @@ func TestNew(t *testing.T) {
 		test.That(t, err, test.ShouldBeNil)
 
 		// TODO: Implement these
+		_, componentReference, err := svc.GetPosition(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, componentReference, test.ShouldEqual, "replay_sensor")
+
 		// timestamp1, err := svc.GetLatestMapInfo(context.Background())
 		// test.That(t, err, test.ShouldBeNil)
-		// _, err = svc.GetPointCloudMap(context.Background())
-		// test.That(t, err, test.ShouldBeNil)
+
+		pcmFunc, err := svc.GetPointCloudMap(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+
+		pcm, err := slam.HelperConcatenateChunksToFull(pcmFunc)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, pcm, test.ShouldNotBeNil)
+
 		// timestamp2, err := svc.GetLatestMapInfo(context.Background())
 		// test.That(t, err, test.ShouldBeNil)
 		// test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
@@ -365,26 +376,36 @@ func TestNew(t *testing.T) {
 		termFunc := initTestCL(t, logger)
 		defer termFunc()
 
-		dataDirectory, err := os.MkdirTemp("", "*")
-		test.That(t, err, test.ShouldBeNil)
+		dataDirectory, fsCleanupFunc := initInternalState(t)
+		defer fsCleanupFunc()
 
 		attrCfg := &vcConfig.Config{
 			ModularizationV2Enabled: &_true,
-			Sensors:                 []string{"replay_sensor"},
+			Sensors:                 []string{"good_lidar"},
 			ConfigParams:            map[string]string{"mode": "2d"},
 			DataDirectory:           dataDirectory,
-			UseLiveData:             &_false,
-			MapRateSec:              &testMapRateSec,
+			DataRateMsec:            testDataRateMsec,
+			UseLiveData:             &_true,
 		}
 
 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 		test.That(t, err, test.ShouldBeNil)
 
 		// TODO: Implement these
+		_, componentReference, err := svc.GetPosition(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, componentReference, test.ShouldEqual, "good_lidar")
+
 		// timestamp1, err := svc.GetLatestMapInfo(context.Background())
 		// test.That(t, err, test.ShouldBeNil)
-		// _, err = svc.GetPointCloudMap(context.Background())
-		// test.That(t, err, test.ShouldBeNil)
+
+		pcmFunc, err := svc.GetPointCloudMap(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+
+		pcm, err := slam.HelperConcatenateChunksToFull(pcmFunc)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, pcm, test.ShouldNotBeNil)
+
 		// timestamp2, err := svc.GetLatestMapInfo(context.Background())
 		// test.That(t, err, test.ShouldBeNil)