[RSDK-6873] add edited map check to cartographer (#329)

go.mod

@@ -12,8 +12,8 @@ require (
 	go.opencensus.io v0.24.0
 	go.uber.org/multierr v1.11.0
 	go.uber.org/zap v1.26.0
-	go.viam.com/api v0.1.257
-	go.viam.com/rdk v0.20.1-0.20240207164326-b493060b7606
+	go.viam.com/api v0.1.274
+	go.viam.com/rdk v0.22.1-0.20240313160439-a7914ad18f7c
 	go.viam.com/test v1.1.1-0.20220913152726-5da9916c08a2
 	go.viam.com/utils v0.1.60
 )

go.sum

@@ -419,7 +419,7 @@ github.com/gobwas/glob v0.2.3/go.mod h1:d3Ez4x06l9bZtSvzIay5+Yzi0fmZzPgnTbPcKjJA
 github.com/gobwas/httphead v0.0.0-20180130184737-2c6c146eadee/go.mod h1:L0fX3K22YWvt/FAX9NnzrNzcI4wNYi9Yku4O0LKYflo=
 github.com/gobwas/pool v0.2.0/go.mod h1:q8bcK0KcYlCgd9e7WYLm9LpyS+YeLd8JVDW6WezmKEw=
 github.com/gobwas/ws v1.0.2/go.mod h1:szmBTxLgaFppYjEmNtny/v3w89xOydFnnZMcgRRu/EM=
-github.com/goccy/go-graphviz v0.1.2 h1:sWSJ6w13BCm/ZOUTHDVrdvbsxqN8yyzaFcHrH/hQ9Yg=
+github.com/goccy/go-graphviz v0.1.3-0.20240305010347-606fdf55b06d h1:V1/uQfndsWudlWLRn3pKUj6QZcifa9BZyeaedNA+Wik=
 github.com/goccy/go-json v0.10.2 h1:CrxCmQqYDkv1z7lO7Wbh2HN93uovUHgrECaO5ZrCXAU=
 github.com/goccy/go-json v0.10.2/go.mod h1:6MelG93GURQebXPDq3khkgXZkazVtN9CRI+MGFi0w8I=
 github.com/gofrs/flock v0.8.0/go.mod h1:F1TvTiK9OcQqauNUHlbJvyl9Qa1QvF/gOUDKA14jxHU=
@@ -1385,10 +1385,10 @@ go.uber.org/zap v1.18.1/go.mod h1:xg/QME4nWcxGxrpdeYfq7UvYrLh66cuVKdrbD1XF/NI=
 go.uber.org/zap v1.23.0/go.mod h1:D+nX8jyLsMHMYrln8A0rJjFt/T/9/bGgIhAqxv5URuY=
 go.uber.org/zap v1.26.0 h1:sI7k6L95XOKS281NhVKOFCUNIvv9e0w4BF8N3u+tCRo=
 go.uber.org/zap v1.26.0/go.mod h1:dtElttAiwGvoJ/vj4IwHBS/gXsEu/pZ50mUIRWuG0so=
-go.viam.com/api v0.1.257 h1:t1AJN99Us+H9p8iVftY4a/QLPejrzy+ypr6kYGpAd8w=
-go.viam.com/api v0.1.257/go.mod h1:msa4TPrMVeRDcG4YzKA/S6wLEUC7GyHQE973JklrQ10=
-go.viam.com/rdk v0.20.1-0.20240207164326-b493060b7606 h1:SH/TPZhralm5myCePGkPjPnmxxGJF7pSQUXqEEsEU8U=
-go.viam.com/rdk v0.20.1-0.20240207164326-b493060b7606/go.mod h1:C30jq8oLol7SQCYROrzbwKtunWWAL4wBYd340+DmOO8=
+go.viam.com/api v0.1.274 h1:xi3FD4xO9EpMjtJJxu8i6yuWsr6ACSTbjhSLIln6ZnE=
+go.viam.com/api v0.1.274/go.mod h1:msa4TPrMVeRDcG4YzKA/S6wLEUC7GyHQE973JklrQ10=
+go.viam.com/rdk v0.22.1-0.20240313160439-a7914ad18f7c h1:aKttkdKwP59npan5NKXDy6U0PzXv7RbWMT7xgTmAlZw=
+go.viam.com/rdk v0.22.1-0.20240313160439-a7914ad18f7c/go.mod h1:twRHd1dczEMSmplA/KfghC2MfHOKYdPphKsH0gZ0RlY=
 go.viam.com/test v1.1.1-0.20220913152726-5da9916c08a2 h1:oBiK580EnEIzgFLU4lHOXmGAE3MxnVbeR7s1wp/F3Ps=
 go.viam.com/test v1.1.1-0.20220913152726-5da9916c08a2/go.mod h1:XM0tej6riszsiNLT16uoyq1YjuYPWlRBweTPRDanIts=
 go.viam.com/utils v0.1.60 h1:Q1EtxJbTbdvFwDBj4M7uZ/3lLjY63JBrCLlZlYL9VBQ=

testhelper/integrationtesthelper.go

@@ -218,7 +218,7 @@ func testCartographerMap(t *testing.T, svc slam.Service, localizationMode bool)
 	test.That(t, props.CloudSlam, test.ShouldBeFalse)
 	test.That(t, props.MappingMode == slam.MappingModeLocalizationOnly, test.ShouldEqual, localizationMode)
 
-	pcd, err := slam.PointCloudMapFull(context.Background(), svc)
+	pcd, err := slam.PointCloudMapFull(context.Background(), svc, false)
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, pcd, test.ShouldNotBeNil)
 

testhelper/testhelper.go

@@ -244,10 +244,10 @@ func InitTestCL(t *testing.T, logger logging.Logger) func() {
 	}
 }
 
-// InitInternalState creates the internal state directory witghin a temp directory
+// InitInternalState creates the internal state directory within a temp directory
 // with an internal state pbstream file & returns the data directory & a function
 // to delete the data directory.
-func InitInternalState(t *testing.T) (string, func()) {
+func InitInternalState(t *testing.T, includeEditedMap bool) (string, func()) {
 	dataDirectory, err := os.MkdirTemp("", "*")
 	test.That(t, err, test.ShouldBeNil)
 
@@ -264,6 +264,16 @@ func InitInternalState(t *testing.T) (string, func()) {
 	err = os.WriteFile(filename, internalState, os.ModePerm)
 	test.That(t, err, test.ShouldBeNil)
 
+	if includeEditedMap {
+		file := "viam-cartographer/mock_lidar/10.pcd"
+		editedPCD, err := os.ReadFile(artifact.MustPath(file))
+		test.That(t, err, test.ShouldBeNil)
+
+		filename := filepath.Join(dataDirectory+"/internal_state", "edited-map.pcd")
+		err = os.WriteFile(filename, editedPCD, os.ModePerm)
+		test.That(t, err, test.ShouldBeNil)
+	}
+
 	return filename, func() {
 		err := os.RemoveAll(dataDirectory)
 		test.That(t, err, test.ShouldBeNil)

viam_cartographer.go

@@ -62,6 +62,7 @@ const (
 	SuccessMessage = "success"
 	// PostprocessToggleResponseKey is the key sent back for the toggle postprocess command.
 	PostprocessToggleResponseKey = "postprocessed"
+	editedMapName                = "edited-map.pcd"
 )
 
 var defaultCartoAlgoCfg = cartofacade.CartoAlgoConfig{
@@ -270,6 +271,20 @@ func New(
 		}
 	}()
 
+	// if we have an existing map, check if there is an edited map within the package
+	if cartoSvc.existingMap != "" {
+		packageDir := filepath.Dir(svcConfig.ExistingMap)
+
+		filePath := filepath.Clean(filepath.Join(packageDir, editedMapName))
+		bytes, err := os.ReadFile(filePath)
+		if err != nil && !errors.Is(err, os.ErrNotExist) {
+			return nil, err
+		}
+		if len(bytes) > 0 {
+			cartoSvc.editedMap = &bytes
+		}
+	}
+
 	// do not initialize CartoFacade or Sensor Processes when using cloudslam
 	if svcConfig.UseCloudSlam != nil && *svcConfig.UseCloudSlam {
 		return &CartographerService{
@@ -531,6 +546,7 @@ type CartographerService struct {
 	postprocessed           atomic.Bool
 	postprocessingTasks     []postprocess.Task
 	postprocessedPointCloud *[]byte
+	editedMap               *[]byte
 
 	useCloudSlam  bool
 	enableMapping bool
@@ -566,7 +582,7 @@ func (cartoSvc *CartographerService) Position(ctx context.Context) (spatialmath.
 
 // PointCloudMap creates a request calls the slam algorithms PointCloudMap endpoint and returns a callback
 // function which will return the next chunk of the current pointcloud map.
-func (cartoSvc *CartographerService) PointCloudMap(ctx context.Context) (func() ([]byte, error), error) {
+func (cartoSvc *CartographerService) PointCloudMap(ctx context.Context, returnEditedMap bool) (func() ([]byte, error), error) {
 	ctx, span := trace.StartSpan(ctx, "viamcartographer::CartographerService::PointCloudMap")
 	defer span.End()
 
@@ -579,6 +595,9 @@ func (cartoSvc *CartographerService) PointCloudMap(ctx context.Context) (func()
 		cartoSvc.postprocessedPointCloud != nil to check that the pointcloud has been set.
 		cartoSvc.postprocessed.Load() to check if postprocessed has not been toggled off.
 	*/
+	if returnEditedMap && cartoSvc.editedMap != nil {
+		return toChunkedFunc(*cartoSvc.editedMap), nil
+	}
 	if cartoSvc.existingMap != "" && !cartoSvc.enableMapping && cartoSvc.postprocessedPointCloud != nil && cartoSvc.postprocessed.Load() {
 		return toChunkedFunc(*cartoSvc.postprocessedPointCloud), nil
 	}

viam_cartographer_internal_test.go

@@ -246,13 +246,16 @@ func TestPointCloudMapEndpoint(t *testing.T) {
 	mockCartoFacade := &cartofacade.Mock{}
 	pathToFileLargerThanChunkSize := "viam-cartographer/outputs/viam-office-02-22-3/pointcloud/pointcloud_0.pcd"
 	pathToFileSmallerThanChunkSize := "viam-cartographer/outputs/viam-office-02-22-3/pointcloud/pointcloud_1.pcd"
+	pointCloudFunc := func(ctx context.Context) (func() ([]byte, error), error) {
+		return svc.PointCloudMap(ctx, false)
+	}
 	testApisThatReturnCallbackFuncs(
 		t,
 		svc,
 		mockCartoFacade,
 		pathToFileLargerThanChunkSize,
 		pathToFileSmallerThanChunkSize,
-		svc.PointCloudMap,
+		pointCloudFunc,
 		setMockPointCloudFunc,
 	)
 
@@ -266,7 +269,7 @@ func TestPointCloudMapEndpoint(t *testing.T) {
 			return nil, errors.New("test")
 		}
 
-		callback, err := svc.PointCloudMap(context.Background())
+		callback, err := svc.PointCloudMap(context.Background(), false)
 		test.That(t, callback, test.ShouldBeNil)
 		test.That(t, err, test.ShouldBeError, errors.New("test"))
 	})

viam_cartographer_test.go

@@ -4,7 +4,7 @@
 // that test the interaction with the core C++ viam-cartographer code and the Golang implementation of the
 // cartographer slam service.
 //
-//nolint:dupl
+
 package viamcartographer_test
 
 import (
@@ -62,7 +62,7 @@ func TestNew(t *testing.T) {
 		test.That(t, componentRef, test.ShouldBeEmpty)
 		test.That(t, err, test.ShouldBeError, viamcartographer.ErrUseCloudSlamEnabled)
 
-		gpcmF, err := svc.PointCloudMap(ctx)
+		gpcmF, err := svc.PointCloudMap(ctx, false)
 		test.That(t, gpcmF, test.ShouldBeNil)
 		test.That(t, err, test.ShouldBeError, viamcartographer.ErrUseCloudSlamEnabled)
 
@@ -141,7 +141,7 @@ func TestNew(t *testing.T) {
 		termFunc := testhelper.InitTestCL(t, logger)
 		defer termFunc()
 
-		existingMap, fsCleanupFunc := testhelper.InitInternalState(t)
+		existingMap, fsCleanupFunc := testhelper.InitInternalState(t, false)
 		defer fsCleanupFunc()
 
 		attrCfg := &vcConfig.Config{
@@ -166,7 +166,7 @@ func TestNew(t *testing.T) {
 		test.That(t, err.Error(), test.ShouldContainSubstring, "VIAM_CARTO_GET_POSITION_NOT_INITIALIZED")
 
 		// Test pointcloud map
-		pcmFunc, err := svc.PointCloudMap(context.Background())
+		pcmFunc, err := svc.PointCloudMap(context.Background(), false)
 		test.That(t, err, test.ShouldBeNil)
 
 		pcm, err := slam.HelperConcatenateChunksToFull(pcmFunc)
@@ -194,7 +194,7 @@ func TestNew(t *testing.T) {
 		termFunc := testhelper.InitTestCL(t, logger)
 		defer termFunc()
 
-		existingMap, fsCleanupFunc := testhelper.InitInternalState(t)
+		existingMap, fsCleanupFunc := testhelper.InitInternalState(t, true)
 		defer fsCleanupFunc()
 
 		attrCfg := &vcConfig.Config{
@@ -219,13 +219,22 @@ func TestNew(t *testing.T) {
 		test.That(t, err.Error(), test.ShouldContainSubstring, "VIAM_CARTO_GET_POSITION_NOT_INITIALIZED")
 
 		// Test pointcloud map
-		pcmFunc, err := svc.PointCloudMap(context.Background())
+		pcmFunc, err := svc.PointCloudMap(context.Background(), false)
 		test.That(t, err, test.ShouldBeNil)
 
 		pcm, err := slam.HelperConcatenateChunksToFull(pcmFunc)
 		test.That(t, err, test.ShouldBeNil)
 		test.That(t, pcm, test.ShouldNotBeNil)
 
+		// Test returning edited pointcloud map
+		pcmEditedFunc, err := svc.PointCloudMap(context.Background(), true)
+		test.That(t, err, test.ShouldBeNil)
+
+		pcmEdited, err := slam.HelperConcatenateChunksToFull(pcmEditedFunc)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, pcmEdited, test.ShouldNotBeNil)
+		test.That(t, pcmEdited, test.ShouldNotResemble, pcm)
+
 		// Test internal state
 		isFunc, err := svc.InternalState(context.Background())
 		test.That(t, err, test.ShouldBeNil)
@@ -265,7 +274,7 @@ func TestNew(t *testing.T) {
 		termFunc := testhelper.InitTestCL(t, logger)
 		defer termFunc()
 
-		_, fsCleanupFunc := testhelper.InitInternalState(t)
+		_, fsCleanupFunc := testhelper.InitInternalState(t, false)
 		defer fsCleanupFunc()
 
 		attrCfg := &vcConfig.Config{
@@ -312,7 +321,7 @@ func TestClose(t *testing.T) {
 		test.That(t, componentRef, test.ShouldBeEmpty)
 		test.That(t, err, test.ShouldBeError, viamcartographer.ErrClosed)
 
-		gpcmF, err := svc.PointCloudMap(ctx)
+		gpcmF, err := svc.PointCloudMap(ctx, false)
 		test.That(t, gpcmF, test.ShouldBeNil)
 		test.That(t, err, test.ShouldBeError, viamcartographer.ErrClosed)