From 575bc4d66b66c3731215f94d44b1e4bc93198e2d Mon Sep 17 00:00:00 2001 From: kim-mishra <121991867+kim-mishra@users.noreply.github.com> Date: Wed, 12 Jul 2023 15:33:53 -0400 Subject: [PATCH] RSDK-3965 - call into cgo for getPointCloudMap (#193) --- viam-cartographer.go | 25 ++++++++++ viam-cartographer_internal_test.go | 75 ++++++++++++++++++++++++++++++ viam-cartographer_test.go | 39 ++++++++++++---- 3 files changed, 130 insertions(+), 9 deletions(-) diff --git a/viam-cartographer.go b/viam-cartographer.go index 5f0a317a..4a6d77f6 100644 --- 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 index d6052ca0..779c73a1 100644 --- 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 index acecb92c..abcbaa95 100644 --- 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)