diff --git a/services/motion/builtin/builtin_utils_test.go b/services/motion/builtin/builtin_utils_test.go index a34e41f64ab..bec0d0cffdb 100644 --- a/services/motion/builtin/builtin_utils_test.go +++ b/services/motion/builtin/builtin_utils_test.go @@ -23,6 +23,7 @@ import ( "go.viam.com/rdk/robot/framesystem" robotimpl "go.viam.com/rdk/robot/impl" "go.viam.com/rdk/services/motion" + "go.viam.com/rdk/services/slam" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils/inject" ) @@ -86,6 +87,17 @@ func createInjectedSlam(name, pcdPath string, origin spatialmath.Pose) *inject.S } return spatialmath.NewZeroPose(), nil } + injectSlam.PropertiesFunc = func(ctx context.Context) (slam.Properties, error) { + return slam.Properties{ + CloudSlam: false, + MappingMode: slam.MappingModeLocalizationOnly, + InternalStateFileType: ".pbstream", + SensorInfo: []slam.SensorInfo{ + {Name: "my-camera", Type: slam.SensorTypeCamera}, + {Name: "my-movement-sensor", Type: slam.SensorTypeMovementSensor}, + }, + }, nil + } return injectSlam } diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 3a74bec5d27..a2fc735c679 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -578,6 +578,15 @@ func (ms *builtIn) newMoveOnMapRequest( return nil, resource.DependencyNotFoundError(req.SlamName) } + // verify slam is in localization mode + slamProps, err := slamSvc.Properties(ctx) + if err != nil { + return nil, err + } + if slamProps.MappingMode != slam.MappingModeLocalizationOnly { + return nil, fmt.Errorf("expected SLAM to be in localization only mode, got %v", slamProps.MappingMode) + } + // gets the extents of the SLAM map limits, err := slam.Limits(ctx, slamSvc, true) if err != nil { diff --git a/services/slam/fake/slam.go b/services/slam/fake/slam.go index ba7963be4f2..5941dc90218 100644 --- a/services/slam/fake/slam.go +++ b/services/slam/fake/slam.go @@ -95,9 +95,11 @@ func (slamSvc *SLAM) Properties(ctx context.Context) (slam.Properties, error) { _, span := trace.StartSpan(ctx, "slam::fake::Properties") defer span.End() + // MappingModeLocalizationOnly may cause the frontend to not refresh, but it allows motion to work with + // fakeslam. Can make changes in motion to only restrict for cartographer if this becomes a problem. prop := slam.Properties{ CloudSlam: false, - MappingMode: slam.MappingModeNewMap, + MappingMode: slam.MappingModeLocalizationOnly, InternalStateFileType: ".pbstream", SensorInfo: []slam.SensorInfo{ {Name: "my-camera", Type: slam.SensorTypeCamera}, diff --git a/services/slam/fake/slam_test.go b/services/slam/fake/slam_test.go index c9a7a853ea9..9c59facb0c8 100644 --- a/services/slam/fake/slam_test.go +++ b/services/slam/fake/slam_test.go @@ -45,7 +45,7 @@ func TestFakeProperties(t *testing.T) { prop, err := slamSvc.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, prop.MappingMode, test.ShouldEqual, slam.MappingModeLocalizationOnly) test.That(t, prop.InternalStateFileType, test.ShouldEqual, ".pbstream") test.That(t, prop.SensorInfo, test.ShouldResemble, []slam.SensorInfo{ diff --git a/services/slam/slam.go b/services/slam/slam.go index 5e0bcf2af91..bf3d5f466eb 100644 --- a/services/slam/slam.go +++ b/services/slam/slam.go @@ -45,12 +45,36 @@ const ( MappingModeUpdateExistingMap ) +func (t MappingMode) String() string { + switch t { + case MappingModeNewMap: + return "mapping mode" + case MappingModeLocalizationOnly: + return "localizing only mode" + case MappingModeUpdateExistingMap: + return "updating mode" + default: + return "unspecified mode" + } +} + // SensorTypeCamera is a camera sensor. const ( SensorTypeCamera = SensorType(iota) SensorTypeMovementSensor ) +func (t SensorType) String() string { + switch t { + case SensorTypeCamera: + return "camera" + case SensorTypeMovementSensor: + return "movement sensor" + default: + return "unsupported sensor type" + } +} + // API is a variable that identifies the slam resource API. var API = resource.APINamespaceRDK.WithServiceType(SubtypeName)