diff --git a/go.mod b/go.mod index 485b88cb3..777a32877 100644 --- 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 index f86291f50..bfcc5b3cf 100644 --- 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 index cbaa685f1..f0885d079 100644 --- 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 index 33757a59a..37bb8ff0d 100644 --- 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,12 +165,14 @@ 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) @@ -169,6 +180,7 @@ func TestNew(t *testing.T) { 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,6 +233,7 @@ 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) @@ -221,6 +241,7 @@ func TestNew(t *testing.T) { 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)