From beb6def4b919dff4d3e17eb3c7c97ff967b04d24 Mon Sep 17 00:00:00 2001
From: JohnN193 <92045055+JohnN193@users.noreply.github.com>
Date: Thu, 13 Jun 2024 18:32:16 -0400
Subject: [PATCH] [RSDK-7847] only allow motion to use slam in localization
 mode (#4072)

---
 services/motion/builtin/builtin_utils_test.go | 12 ++++++++++
 services/motion/builtin/move_request.go       |  9 +++++++
 services/slam/fake/slam.go                    |  4 +++-
 services/slam/fake/slam_test.go               |  2 +-
 services/slam/slam.go                         | 24 +++++++++++++++++++
 5 files changed, 49 insertions(+), 2 deletions(-)

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)