RSDK-1718 - Add GetInternalState to SLAM RDK (viamrobotics#1776)

services/slam/builtin/builtin.go

@@ -575,6 +575,23 @@ func (slamSvc *builtIn) GetMap(
 	return mimeType, imData, vObj, nil
 }
 
+// GetInternalState forwards the request for the SLAM algorithms's internal state. Once a response is received, it is returned
+// to the user.
+func (slamSvc *builtIn) GetInternalState(ctx context.Context, name string) ([]byte, error) {
+	ctx, span := trace.StartSpan(ctx, "slam::builtIn::GetInternalState")
+	defer span.End()
+
+	req := &pb.GetInternalStateRequest{Name: name}
+
+	resp, err := slamSvc.clientAlgo.GetInternalState(ctx, req)
+	if err != nil {
+		return nil, errors.Wrap(err, "error getting the internal state from the SLAM client")
+	}
+
+	internalState := resp.GetInternalState()
+	return internalState, err
+}
+
 // NewBuiltIn returns a new slam service for the given robot.
 func NewBuiltIn(ctx context.Context, deps registry.Dependencies, config config.Service, logger golog.Logger, bufferSLAMProcessLogs bool) (slam.Service, error) {
 	ctx, span := trace.StartSpan(ctx, "slam::slamService::New")

services/slam/builtin/builtin_test.go

@@ -1080,7 +1080,7 @@ func TestORBSLAMDataProcess(t *testing.T) {
 	closeOutSLAMService(t, name)
 }
 
-func TestGetMapAndPosition(t *testing.T) {
+func TestEndpointFailures(t *testing.T) {
 	name, err := createTempFolderArchitecture()
 	test.That(t, err, test.ShouldBeNil)
 
@@ -1117,6 +1117,10 @@ func TestGetMapAndPosition(t *testing.T) {
 	test.That(t, pc, test.ShouldBeNil)
 	test.That(t, fmt.Sprint(err), test.ShouldContainSubstring, "error getting SLAM map")
 
+	internalState, err := svc.GetInternalState(context.Background(), "hi")
+	test.That(t, fmt.Sprint(err), test.ShouldContainSubstring, "error getting the internal state from the SLAM client")
+	test.That(t, internalState, test.ShouldBeNil)
+
 	grpcServer.Stop()
 	test.That(t, utils.TryClose(context.Background(), svc), test.ShouldBeNil)
 
@@ -1324,26 +1328,26 @@ func resetFolder(path string) error {
 	return err
 }
 
-func checkDeleteProcessedData(t *testing.T, mode slam.Mode, dir string, prev int, deleteProcessedData, online bool) int {
+func checkDeleteProcessedData(t *testing.T, mode slam.Mode, dir string, prev int, deleteProcessedData, useLiveData bool) int {
 	var numFiles int
 
 	switch mode {
 	case slam.Mono:
-		numFilesRGB, err := checkDataDirForExpectedFiles(t, dir+"/data/rgb", prev, online, deleteProcessedData)
+		numFilesRGB, err := checkDataDirForExpectedFiles(t, dir+"/data/rgb", prev, deleteProcessedData, useLiveData)
 		test.That(t, err, test.ShouldBeNil)
 
 		numFiles = numFilesRGB
 	case slam.Rgbd:
-		numFilesRGB, err := checkDataDirForExpectedFiles(t, dir+"/data/rgb", prev, online, deleteProcessedData)
+		numFilesRGB, err := checkDataDirForExpectedFiles(t, dir+"/data/rgb", prev, deleteProcessedData, useLiveData)
 		test.That(t, err, test.ShouldBeNil)
 
-		numFilesDepth, err := checkDataDirForExpectedFiles(t, dir+"/data/depth", prev, online, deleteProcessedData)
+		numFilesDepth, err := checkDataDirForExpectedFiles(t, dir+"/data/depth", prev, deleteProcessedData, useLiveData)
 		test.That(t, err, test.ShouldBeNil)
 
 		test.That(t, numFilesRGB, test.ShouldEqual, numFilesDepth)
 		numFiles = numFilesRGB
 	case slam.Dim2d:
-		numFiles2D, err := checkDataDirForExpectedFiles(t, dir+"/data", prev, online, deleteProcessedData)
+		numFiles2D, err := checkDataDirForExpectedFiles(t, dir+"/data", prev, deleteProcessedData, useLiveData)
 		test.That(t, err, test.ShouldBeNil)
 		numFiles = numFiles2D
 	default:
@@ -1353,24 +1357,24 @@ func checkDeleteProcessedData(t *testing.T, mode slam.Mode, dir string, prev int
 
 // Compares the number of files found in a specified data directory with the previous number found and uses
 // the online state and delete_processed_data value to evaluate this comparison.
-func checkDataDirForExpectedFiles(t *testing.T, dir string, prev int, delete_processed_data, online bool) (int, error) {
+func checkDataDirForExpectedFiles(t *testing.T, dir string, prev int, delete_processed_data, useLiveData bool) (int, error) {
 
 	files, err := ioutil.ReadDir(dir)
 	test.That(t, err, test.ShouldBeNil)
 
 	if prev == 0 {
 		return len(files), nil
 	}
-	if delete_processed_data && online {
+	if delete_processed_data && useLiveData {
 		test.That(t, prev, test.ShouldBeLessThanOrEqualTo, dataBufferSize+1)
 	}
-	if !delete_processed_data && online {
+	if !delete_processed_data && useLiveData {
 		test.That(t, prev, test.ShouldBeLessThan, len(files))
 	}
-	if delete_processed_data && !online {
+	if delete_processed_data && !useLiveData {
 		return 0, errors.New("the delete_processed_data value cannot be true when running SLAM in offline mode")
 	}
-	if !delete_processed_data && !online {
+	if !delete_processed_data && !useLiveData {
 		test.That(t, prev, test.ShouldEqual, len(files))
 	}
 	return len(files), nil

services/slam/builtin/cartographer_int_test.go

@@ -5,15 +5,18 @@ import (
 	"io/ioutil"
 	"os"
 	"os/exec"
+	"path/filepath"
 	"reflect"
 	"strings"
 	"testing"
 	"time"
 
 	"github.com/edaniels/golog"
+	"github.com/golang/geo/r3"
 	"go.viam.com/rdk/services/slam"
 	"go.viam.com/rdk/services/slam/builtin"
 	"go.viam.com/rdk/services/slam/internal/testhelper"
+	"go.viam.com/rdk/spatialmath"
 	"go.viam.com/test"
 	"go.viam.com/utils"
 )
@@ -22,27 +25,51 @@ const (
 	cartoSleepMs = 100
 )
 
-// Checks the cartographer position and map.
-func testCartographerPositionAndMap(t *testing.T, svc slam.Service) {
-	t.Helper()
-
-	position, err := svc.Position(context.Background(), "test", map[string]interface{}{})
-	test.That(t, err, test.ShouldBeNil)
-	// Typical values for 2D lidar are around (-0.004, 0.004, 0) +- (0.001, 0.001, 0)
-	t.Logf("Position point: (%v, %v, %v)",
-		position.Pose().Point().X, position.Pose().Point().Y, position.Pose().Point().Z)
-	// Typical values for 2D lidar are around (0, 0, -1), theta=0.001 +- 0.001
-	t.Logf("Position orientation: RX: %v, RY: %v, RZ: %v, Theta: %v",
-		position.Pose().Orientation().AxisAngles().RX,
-		position.Pose().Orientation().AxisAngles().RY,
-		position.Pose().Orientation().AxisAngles().RZ,
-		position.Pose().Orientation().AxisAngles().Theta)
+// Checks the cartographer map and confirms there at least 100 map points.
+func testCartographerMap(t *testing.T, svc slam.Service) {
 	actualMIME, _, pointcloud, err := svc.GetMap(context.Background(), "test", "pointcloud/pcd", nil, false, map[string]interface{}{})
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, actualMIME, test.ShouldResemble, "pointcloud/pcd")
+	t.Logf("Pointcloud points: %v", pointcloud.Size())
 	test.That(t, pointcloud.Size(), test.ShouldBeGreaterThanOrEqualTo, 100)
 }
 
+// Checks the cartographer position within a defined tolerance.
+func testCartographerPosition(t *testing.T, svc slam.Service) {
+	expectedPos := r3.Vector{X: -0.004, Y: 0.004, Z: 0}
+	tolerancePos := 0.04
+	expectedOri := &spatialmath.R4AA{Theta: 0, RX: 0, RY: 0, RZ: -1}
+	toleranceOri := 0.5
+
+	position, err := svc.Position(context.Background(), "test", map[string]interface{}{})
+	test.That(t, err, test.ShouldBeNil)
+
+	actualPos := position.Pose().Point()
+	t.Logf("Position point: (%v, %v, %v)", actualPos.X, actualPos.Y, actualPos.Z)
+	test.That(t, actualPos.X, test.ShouldBeBetween, expectedPos.X-tolerancePos, expectedPos.X+tolerancePos)
+	test.That(t, actualPos.Y, test.ShouldBeBetween, expectedPos.Y-tolerancePos, expectedPos.Y+tolerancePos)
+	test.That(t, actualPos.Z, test.ShouldBeBetween, expectedPos.Z-tolerancePos, expectedPos.Z+tolerancePos)
+
+	actualOri := position.Pose().Orientation().AxisAngles()
+	t.Logf("Position orientation: RX: %v, RY: %v, RZ: %v, Theta: %v", actualOri.RX, actualOri.RY, actualOri.RZ, actualOri.Theta)
+	test.That(t, actualOri.RX, test.ShouldBeBetween, expectedOri.RX-toleranceOri, expectedOri.RX+toleranceOri)
+	test.That(t, actualOri.RY, test.ShouldBeBetween, expectedOri.RY-toleranceOri, expectedOri.RY+toleranceOri)
+	test.That(t, actualOri.RZ, test.ShouldBeBetween, expectedOri.RZ-toleranceOri, expectedOri.RZ+toleranceOri)
+	test.That(t, actualOri.Theta, test.ShouldBeBetween, expectedOri.Theta-toleranceOri, expectedOri.Theta+toleranceOri)
+}
+
+// Checks the cartographer internal state.
+func testCartographerInternalState(t *testing.T, svc slam.Service, dataDir string) {
+	internalState, err := svc.GetInternalState(context.Background(), "test")
+	test.That(t, err, test.ShouldBeNil)
+
+	// Save the data from the call to GetInternalState for use in next test.
+	timeStamp := time.Now()
+	filename := filepath.Join(dataDir, "map", "map_data_"+timeStamp.UTC().Format(slamTimeFormat)+".pbstream")
+	err = os.WriteFile(filename, internalState, 0644)
+	test.That(t, err, test.ShouldBeNil)
+}
+
 func integrationtestHelperCartographer(t *testing.T, mode slam.Mode) {
 	_, err := exec.LookPath("carto_grpc_server")
 	if err != nil {
@@ -91,13 +118,14 @@ func integrationtestHelperCartographer(t *testing.T, mode slam.Mode) {
 			line, err := logReader.ReadString('\n')
 			test.That(t, err, test.ShouldBeNil)
 			if strings.Contains(line, "Passed sensor data to SLAM") {
-				prevNumFiles = checkDeleteProcessedData(t, mode, name, prevNumFiles, len(attrCfg.Sensors) != 0, deleteProcessedData)
+				prevNumFiles = checkDeleteProcessedData(t, mode, name, prevNumFiles, deleteProcessedData, useLiveData)
 				break
 			}
 		}
 	}
 
-	testCartographerPositionAndMap(t, svc)
+	testCartographerPosition(t, svc)
+	testCartographerMap(t, svc)
 
 	// Close out slam service
 	test.That(t, utils.TryClose(context.Background(), svc), test.ShouldBeNil)
@@ -148,14 +176,15 @@ func integrationtestHelperCartographer(t *testing.T, mode slam.Mode) {
 		line, err := logReader.ReadString('\n')
 		test.That(t, err, test.ShouldBeNil)
 		if strings.Contains(line, "Passed sensor data to SLAM") {
-			prevNumFiles = checkDeleteProcessedData(t, mode, name, prevNumFiles, len(attrCfg.Sensors) != 0, deleteProcessedData)
+			prevNumFiles = checkDeleteProcessedData(t, mode, name, prevNumFiles, deleteProcessedData, useLiveData)
 		}
 		if strings.Contains(line, "Finished optimizing final map") {
 			break
 		}
 	}
 
-	testCartographerPositionAndMap(t, svc)
+	testCartographerPosition(t, svc)
+	testCartographerMap(t, svc)
 
 	// Sleep to ensure cartographer saves at least one map
 	time.Sleep(time.Second * time.Duration(*attrCfg.MapRateSec))
@@ -177,7 +206,7 @@ func integrationtestHelperCartographer(t *testing.T, mode slam.Mode) {
 	testCartographerDir(t, name, 1)
 
 	// Test online mode using the map generated in the offline test
-	t.Log("\n=== Testing online mode in localization mode ===\n")
+	t.Log("\n=== Testing online localization mode ===\n")
 
 	mapRate = 0
 	deleteProcessedData = true
@@ -222,18 +251,24 @@ func integrationtestHelperCartographer(t *testing.T, mode slam.Mode) {
 			line, err := logReader.ReadString('\n')
 			test.That(t, err, test.ShouldBeNil)
 			if strings.Contains(line, "Passed sensor data to SLAM") {
-				prevNumFiles = checkDeleteProcessedData(t, mode, name, prevNumFiles, len(attrCfg.Sensors) != 0, deleteProcessedData)
+				prevNumFiles = checkDeleteProcessedData(t, mode, name, prevNumFiles, deleteProcessedData, useLiveData)
 				break
 			}
 		}
 	}
 
-	testCartographerPositionAndMap(t, svc)
+	testCartographerPosition(t, svc)
+	testCartographerMap(t, svc)
+
+	// Remove maps so that testing is done on the map generated by the internal map
+	test.That(t, resetFolder(name+"/map"), test.ShouldBeNil)
+
+	testCartographerInternalState(t, svc, name)
 
 	// Close out slam service
 	test.That(t, utils.TryClose(context.Background(), svc), test.ShouldBeNil)
 
-	// Test that no new maps were generated
+	// Test that only the map present is the one generated by the GetInternalState call
 	testCartographerDir(t, name, 1)
 
 	// Don't clear out the directory, since we will re-use the maps for the next run
@@ -289,13 +324,16 @@ func integrationtestHelperCartographer(t *testing.T, mode slam.Mode) {
 			line, err := logReader.ReadString('\n')
 			test.That(t, err, test.ShouldBeNil)
 			if strings.Contains(line, "Passed sensor data to SLAM") {
-				prevNumFiles = checkDeleteProcessedData(t, mode, name, prevNumFiles, len(attrCfg.Sensors) != 0, deleteProcessedData)
+				prevNumFiles = checkDeleteProcessedData(t, mode, name, prevNumFiles, deleteProcessedData, useLiveData)
 				break
 			}
+			test.That(t, strings.Contains(line, "Failed to open proto stream"), test.ShouldBeFalse)
+			test.That(t, strings.Contains(line, "Failed to read SerializationHeader"), test.ShouldBeFalse)
 		}
 	}
 
-	testCartographerPositionAndMap(t, svc)
+	testCartographerPosition(t, svc)
+	testCartographerMap(t, svc)
 
 	// Close out slam service
 	test.That(t, utils.TryClose(context.Background(), svc), test.ShouldBeNil)