RSDK-6040 Add GetProperties endpoint to viam-cartographer (#310)

Co-authored-by: kim-mishra <[email protected]>
viam-modules · Jan 10, 2024 · c318e49 · c318e49
1 parent 60b6e8a
commit c318e49
Show file tree

Hide file tree

Showing 4 changed files with 90 additions and 41 deletions.
diff --git a/go.mod b/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.235
-	go.viam.com/rdk v0.15.1
+	go.viam.com/api v0.1.245
+	go.viam.com/rdk v0.18.0
 	go.viam.com/test v1.1.1-0.20220913152726-5da9916c08a2
 	go.viam.com/utils v0.1.54
 )

diff --git a/go.sum b/go.sum
@@ -1390,10 +1390,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.235 h1:xxrAVFTvIR93DpMfv4ixIoCC60PyVVOfKzgunyxRTeE=
-go.viam.com/api v0.1.235/go.mod h1:msa4TPrMVeRDcG4YzKA/S6wLEUC7GyHQE973JklrQ10=
-go.viam.com/rdk v0.15.1 h1:gsQWP3F3/JGJXfNrtH+qlJIYDfqP2RsfSzjNrpUR2i8=
-go.viam.com/rdk v0.15.1/go.mod h1:DPs/YSRJPpnfn0h2I1ZRFmXzQ02zfsyHDHcUXXsmG2Y=
+go.viam.com/api v0.1.245 h1:DL5x1fxg0VUwPJRI1KaSwXeCZR/PnknTpw7AU0Mmu18=
+go.viam.com/api v0.1.245/go.mod h1:msa4TPrMVeRDcG4YzKA/S6wLEUC7GyHQE973JklrQ10=
+go.viam.com/rdk v0.18.0 h1:PiqAdBrE+4xr6X43AD5qbELQIJ0jMWgj49uY6x1aJXA=
+go.viam.com/rdk v0.18.0/go.mod h1:zOfpk0LP2QKkdWmNwvSUTZldZ2KjrXCIo8KzF+eDfnk=
 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.54 h1:1ENNj0atwDngCVKAOr4gXSJnoQdVd88qm38tEnqghaE=

diff --git a/viam_cartographer.go b/viam_cartographer.go
@@ -518,14 +518,9 @@ type CartographerService struct {
 func (cartoSvc *CartographerService) Position(ctx context.Context) (spatialmath.Pose, string, error) {
 	ctx, span := trace.StartSpan(ctx, "viamcartographer::CartographerService::Position")
 	defer span.End()
-	if cartoSvc.useCloudSlam {
-		cartoSvc.logger.Warn("Position called with use_cloud_slam set to true")
-		return nil, "", ErrUseCloudSlamEnabled
-	}
 
-	if cartoSvc.closed {
-		cartoSvc.logger.Warn("Position called after shutting down of cartographer has been initiated")
-		return nil, "", ErrClosed
+	if err := cartoSvc.isOpenAndRunningLocally("Position"); err != nil {
+		return nil, "", err
 	}
 
 	pos, err := cartoSvc.cartofacade.Position(ctx, cartoSvc.cartoFacadeTimeout)
@@ -550,14 +545,9 @@ func (cartoSvc *CartographerService) Position(ctx context.Context) (spatialmath.
 func (cartoSvc *CartographerService) PointCloudMap(ctx context.Context) (func() ([]byte, error), error) {
 	ctx, span := trace.StartSpan(ctx, "viamcartographer::CartographerService::PointCloudMap")
 	defer span.End()
-	if cartoSvc.useCloudSlam {
-		cartoSvc.logger.Warn("PointCloudMap called with use_cloud_slam set to true")
-		return nil, ErrUseCloudSlamEnabled
-	}
 
-	if cartoSvc.closed {
-		cartoSvc.logger.Warn("PointCloudMap called after shutting down of cartographer has been initiated")
-		return nil, ErrClosed
+	if err := cartoSvc.isOpenAndRunningLocally("PointCloudMap"); err != nil {
+		return nil, err
 	}
 
 	/*
@@ -592,14 +582,9 @@ func (cartoSvc *CartographerService) PointCloudMap(ctx context.Context) (func()
 func (cartoSvc *CartographerService) InternalState(ctx context.Context) (func() ([]byte, error), error) {
 	ctx, span := trace.StartSpan(ctx, "viamcartographer::CartographerService::InternalState")
 	defer span.End()
-	if cartoSvc.useCloudSlam {
-		cartoSvc.logger.Warn("InternalState called with use_cloud_slam set to true")
-		return nil, ErrUseCloudSlamEnabled
-	}
 
-	if cartoSvc.closed {
-		cartoSvc.logger.Warn("InternalState called after shutting down of cartographer has been initiated")
-		return nil, ErrClosed
+	if err := cartoSvc.isOpenAndRunningLocally("InternalState"); err != nil {
+		return nil, err
 	}
 
 	is, err := cartoSvc.cartofacade.InternalState(ctx, cartoSvc.cartoFacadeTimeout)
@@ -625,19 +610,41 @@ func toChunkedFunc(b []byte) func() ([]byte, error) {
 	return f
 }
 
+// Properties returns information regarding the current SLAM session including the mapping mode and
+// is the session is being run in the cloud.
+func (cartoSvc *CartographerService) Properties(ctx context.Context) (slam.Properties, error) {
+	_, span := trace.StartSpan(ctx, "viamcartographer::CartographerService::Properties")
+	defer span.End()
+
+	if err := cartoSvc.isOpenAndRunningLocally("Properties"); err != nil {
+		return slam.Properties{}, err
+	}
+
+	props := slam.Properties{
+		CloudSlam: cartoSvc.useCloudSlam,
+	}
+
+	if cartoSvc.enableMapping && cartoSvc.existingMap == "" {
+		props.MappingMode = slam.MappingModeNewMap
+	} else if cartoSvc.enableMapping && cartoSvc.existingMap != "" {
+		props.MappingMode = slam.MappingModeUpdateExistingMap
+	} else if !cartoSvc.enableMapping && cartoSvc.existingMap != "" {
+		props.MappingMode = slam.MappingModeLocalizationOnly
+	} else {
+		return slam.Properties{}, errors.New("invalid mode: localizing requires an existing map")
+	}
+
+	return props, nil
+}
+
 // LatestMapInfo returns a new timestamp every time it is called when in mapping mode, to signal
 // that the map should be updated. In localizing, the timestamp returned is the timestamp of the session.
 func (cartoSvc *CartographerService) LatestMapInfo(ctx context.Context) (time.Time, error) {
 	_, span := trace.StartSpan(ctx, "viamcartographer::CartographerService::LatestMapInfo")
 	defer span.End()
-	if cartoSvc.useCloudSlam {
-		cartoSvc.logger.Warn("LatestMapInfo called with use_cloud_slam set to true")
-		return time.Time{}, ErrUseCloudSlamEnabled
-	}
 
-	if cartoSvc.closed {
-		cartoSvc.logger.Warn("LatestMapInfo called after shutting down of cartographer has been initiated")
-		return time.Time{}, ErrClosed
+	if err := cartoSvc.isOpenAndRunningLocally("LatestMapInfo"); err != nil {
+		return time.Time{}, err
 	}
 
 	if cartoSvc.SlamMode != cartofacade.LocalizingMode {
@@ -649,14 +656,11 @@ func (cartoSvc *CartographerService) LatestMapInfo(ctx context.Context) (time.Ti
 
 // DoCommand receives arbitrary commands.
 func (cartoSvc *CartographerService) DoCommand(ctx context.Context, req map[string]interface{}) (map[string]interface{}, error) {
-	if cartoSvc.useCloudSlam {
-		cartoSvc.logger.Warn("DoCommand called with use_cloud_slam set to true")
-		return nil, ErrUseCloudSlamEnabled
-	}
+	_, span := trace.StartSpan(ctx, "viamcartographer::CartographerService::DoCommand")
+	defer span.End()
 
-	if cartoSvc.closed {
-		cartoSvc.logger.Warn("DoCommand called after shutting down of cartographer has been initiated")
-		return nil, ErrClosed
+	if err := cartoSvc.isOpenAndRunningLocally("DoCommand"); err != nil {
+		return nil, err
 	}
 
 	if _, ok := req[JobDoneCommand]; ok {
@@ -774,3 +778,17 @@ func CheckQuaternionFromClientAlgo(pose spatialmath.Pose, componentReference str
 	}
 	return nil, "", errors.Errorf("error getting SLAM position: quaternion not given, %v", returnedExt)
 }
+
+// checkCloseAndCloudStatus returns an error if the cartographer service has been previously closed or is being run in the cloud.
+func (cartoSvc *CartographerService) isOpenAndRunningLocally(cmd string) error {
+	if cartoSvc.useCloudSlam {
+		cartoSvc.logger.Warnf("%v called with use_cloud_slam set to true", cmd)
+		return ErrUseCloudSlamEnabled
+	}
+	if cartoSvc.closed {
+		cartoSvc.logger.Warnf("%v called after closed", cmd)
+		return ErrClosed
+	}
+
+	return nil
+}
diff --git a/viam_cartographer_test.go b/viam_cartographer_test.go
@@ -74,6 +74,10 @@ func TestNew(t *testing.T) {
 		test.That(t, err, test.ShouldBeError, viamcartographer.ErrUseCloudSlamEnabled)
 		test.That(t, mapTime, test.ShouldResemble, time.Time{})
 
+		prop, err := svc.Properties(ctx)
+		test.That(t, err, test.ShouldBeError, viamcartographer.ErrUseCloudSlamEnabled)
+		test.That(t, prop, test.ShouldResemble, slam.Properties{})
+
 		cmd := map[string]interface{}{}
 		resp, err := svc.DoCommand(ctx, cmd)
 		test.That(t, resp, test.ShouldBeNil)
@@ -129,6 +133,11 @@ func TestNew(t *testing.T) {
 		svc, err := testhelper.CreateSLAMService(t, attrCfg, logger)
 		test.That(t, err, test.ShouldBeNil)
 
+		prop, err := svc.Properties(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, prop.CloudSlam, test.ShouldBeFalse)
+		test.That(t, prop.MappingMode, test.ShouldEqual, slam.MappingModeNewMap)
+
 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
 	})
 
@@ -156,19 +165,22 @@ func TestNew(t *testing.T) {
 		timestamp1, err := svc.LatestMapInfo(context.Background())
 		test.That(t, err, test.ShouldBeNil)
 
+		// Test position
 		pose, componentReference, err := svc.Position(context.Background())
 		test.That(t, pose, test.ShouldBeNil)
 		test.That(t, componentReference, test.ShouldBeEmpty)
 		test.That(t, err, test.ShouldNotBeNil)
 		test.That(t, err.Error(), test.ShouldContainSubstring, "VIAM_CARTO_GET_POSITION_NOT_INITIALIZED")
 
+		// Test pointcloud map
 		pcmFunc, err := svc.PointCloudMap(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)
 
+		// Test internal state
 		isFunc, err := svc.InternalState(context.Background())
 		test.That(t, err, test.ShouldBeNil)
 
@@ -181,6 +193,12 @@ func TestNew(t *testing.T) {
 		test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
 		test.That(t, timestamp1, test.ShouldResemble, timestamp2)
 
+		// Test properties
+		prop, err := svc.Properties(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, prop.CloudSlam, test.ShouldBeFalse)
+		test.That(t, prop.MappingMode, test.ShouldEqual, slam.MappingModeLocalizationOnly)
+
 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
 	})
 
@@ -205,6 +223,7 @@ func TestNew(t *testing.T) {
 		test.That(t, ok, test.ShouldBeTrue)
 		test.That(t, cs.SlamMode, test.ShouldEqual, cartofacade.UpdatingMode)
 
+		// Test position
 		pose, componentReference, err := svc.Position(context.Background())
 		test.That(t, pose, test.ShouldBeNil)
 		test.That(t, componentReference, test.ShouldBeEmpty)
@@ -214,13 +233,15 @@ func TestNew(t *testing.T) {
 		timestamp1, err := svc.LatestMapInfo(context.Background())
 		test.That(t, err, test.ShouldBeNil)
 
+		// Test pointcloud map
 		pcmFunc, err := svc.PointCloudMap(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)
 
+		// Test internal state
 		isFunc, err := svc.InternalState(context.Background())
 		test.That(t, err, test.ShouldBeNil)
 
@@ -234,6 +255,12 @@ func TestNew(t *testing.T) {
 		test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
 		test.That(t, timestamp2.After(timestamp1), test.ShouldBeTrue)
 
+		// Test properties
+		prop, err := svc.Properties(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, prop.CloudSlam, test.ShouldBeFalse)
+		test.That(t, prop.MappingMode, test.ShouldEqual, slam.MappingModeUpdateExistingMap)
+
 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
 	})
 
@@ -318,6 +345,10 @@ func TestClose(t *testing.T) {
 		test.That(t, err, test.ShouldBeError, viamcartographer.ErrClosed)
 		test.That(t, mapTime, test.ShouldResemble, time.Time{})
 
+		prop, err := svc.Properties(ctx)
+		test.That(t, err, test.ShouldBeError, viamcartographer.ErrClosed)
+		test.That(t, prop, test.ShouldResemble, slam.Properties{})
+
 		cmd := map[string]interface{}{}
 		resp, err := svc.DoCommand(ctx, cmd)
 		test.That(t, resp, test.ShouldBeNil)