[RSDK-7847] only allow motion to use slam in localization mode (viamr…

…obotics#4072)

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
 }
 

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 {

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},

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{

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)