Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

RSDK-1718 - Add GetInternalState to SLAM RDK #1776

Merged
13 commits merged into from Jan 30, 2023
Merged

RSDK-1718 - Add GetInternalState to SLAM RDK #1776

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
64c5e6f
add get internal to SLAM RDK
Jan 18, 2023
cac74a0
int tests for GetInternalState
Jan 19, 2023
76d6b16
PR review
Jan 24, 2023
f527161
error log fix
Jan 24, 2023
13ae36f
test fix
Jan 24, 2023
f124db9
error verbosity
Jan 24, 2023
d021e0d
comment fix
Jan 24, 2023
a7ffade
Merge branch 'main' of github.com:viamrobotics/rdk into RSDK-1718_Get…
Jan 25, 2023
a346a3e
adding orbslam tests for map and position
Jan 26, 2023
5a20544
Merge branch 'main' of github.com:viamrobotics/rdk into RSDK-1718_Get…
Jan 26, 2023
22d8f29
add failure unit test for GetInternalState
Jan 26, 2023
b6e1013
fix axis angle issue
Jan 30, 2023
f68d1e8
tolerance fix and variable rearrangment fix
Jan 30, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 17 additions & 0 deletions services/slam/builtin/builtin.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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")
nicksanford marked this conversation as resolved.
Show resolved Hide resolved
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")
Expand Down
26 changes: 15 additions & 11 deletions services/slam/builtin/builtin_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down Expand Up @@ -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")
Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Note: most testing of endpoints has to be done in integration tests. This just confirms the failure/error statement

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)

Expand Down Expand Up @@ -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:
Expand All @@ -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
Expand Down
88 changes: 63 additions & 25 deletions services/slam/builtin/cartographer_int_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,15 +5,18 @@ import (
"io/ioutil"
"os"
"os/exec"
"path/filepath"
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why are we changing integration tests if the PR description says that integration tests will come later? #1776 (comment)

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Talked offline but testing was needed to validate and it wasn't much work (replicating work done during the GetInternalState for carto) so I chose to include it here

"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"
)
Expand All @@ -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{}{})
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why is this changing?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Initially it was set up to test the various endpoints in one fucntion, however these endpoints and their outputs do not relate to eachother. With the addition of get internal state and the fact that it may not be called every run. I choose to make these checks independent so any combination could be called.

I also choose to increase the robustness of testing by doing an inBetween check for the Position instead of just logging he output.

Happy to discuss either of these points as they are improvements rather than requirements for this PR.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Did you run the test several times to determine reasonable values for the position and orientation tolerance?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also, should we make this change to the orbslam integration tests as well? Or expect to do that when we add GetInternalState to orbslam integration tests?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Re @tessavitabile 's first point: when I ran tests on my mac, they consistently fail due to an actual value being out of range:

go test -run TestCartographerIntegration2D -v
...
    logger.go:130: 2023-01-25T13:42:57.684-0500 ERROR   process.slam_cartographer_carto_grpc_server     pexec/managed_process.go:214    output  {"name": "StdErr", "data": "I20230125 13:42:57.684520 377790 callbacks.cc:125]    8  1.154142e-01    2.07e-07    8.74e-03   1.08e-04   2.90e-01  7.00e+03        1    1.09e-04    1.23e-03"}
    logger.go:130: 2023-01-25T13:42:57.684-0500 ERROR   process.slam_cartographer_carto_grpc_server     pexec/managed_process.go:214    output  {"name": "StdErr", "data": "I20230125 13:42:57.684589 377790 trust_region_minimizer.cc:743] Terminating: Function tolerance reached. |cost_change|/cost: 9.051883e-07 <= 1.000000e-06"}
    logger.go:130: 2023-01-25T13:42:57.684-0500 ERROR   process.slam_cartographer_carto_grpc_server     pexec/managed_process.go:214    output  {"name": "StdErr", "data": "I20230125 13:42:57.684779 377790 slam_service.cc:721] Passed sensor data to SLAM /var/folders/y0/ff4chsnj1z19b6ps4wslbhyh0000gn/T/668702867/data/cartographer_int_lidar_data_2023-01-25T18:42:57.2978Z.pcd"}
    logger.go:130: 2023-01-25T13:42:57.684-0500 ERROR   process.slam_cartographer_carto_grpc_server     pexec/managed_process.go:214    output  {"name": "StdErr", "data": "I20230125 13:42:57.684928 377790 slam_service.cc:575] No new files found"}
    cartographer_int_test.go:130: Expected '0.009272603568955304' to be between '0' and '0.008' (but it wasn't)!
--- FAIL: TestCartographerIntegration2D (4.73s)
FAIL
exit status 1
FAIL    go.viam.com/rdk/services/slam/builtin   6.416s

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We think this may be an issue where hardware is processing and save the artifact images a different speeds, leading to different timestamps and altering the calculation. I'll be investigating ways to mitigate that but we can't standardize the filename timestamps because that would defeat the purpose of the end to end test. At the very least we'll make sure tolerance values are sufficient (I've run it on my laptop's docker 20+ times with no failures)

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks, I would like to make sure this is stable, both locally and in CI, before merging this change.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This still fails on my mac:
go test -run TestCartographerIntegration2D -v

    logger.go:130: 2023-01-30T12:17:29.681-0500 ERROR   process.slam_cartographer_carto_grpc_server     pexec/managed_process.go:218    output  {"name": "StdErr", "data": "I20230130 12:17:29.681169 3117346 slam_service.cc:616] Passed sensor data to SLAM /var/folders/y0/ff4chsnj1z19b6ps4wslbhyh0000gn/T/2425438821/data/cartographer_int_lidar_data_2023-01-30T17:17:29.4168Z.pcd"}
    logger.go:130: 2023-01-30T12:17:29.681-0500 ERROR   process.slam_cartographer_carto_grpc_server     pexec/managed_process.go:218    output  {"name": "StdErr", "data": "I20230130 12:17:29.681468 3117346 slam_service.cc:470] No new files found"}
    cartographer_int_test.go:130: Expected '0.009183805047824956' to be between '0' and '0.008' (but it wasn't)!
--- FAIL: TestCartographerIntegration2D (7.20s)
FAIL
exit status 1
FAIL    go.viam.com/rdk/services/slam/builtin   8.997s

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) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why is this changing?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

See comment above

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 {
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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))
Expand All @@ -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
Expand Down Expand Up @@ -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)
This conversation was marked as resolved.
Show resolved Hide resolved

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
Expand Down Expand Up @@ -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)
Expand Down
Loading