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 9 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
80 changes: 59 additions & 21 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.01
expectedOri := &spatialmath.OrientationVector{Theta: 0, OX: 0, OY: 0, OZ: -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.OX-toleranceOri, expectedOri.OX+toleranceOri)
Copy link
Contributor

Choose a reason for hiding this comment

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

Why are we comparing RX (axis angle) to OX (orientation vector) here?

Copy link
Author

Choose a reason for hiding this comment

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

Good call out, I'll make sure these are of the same type. Will most likely go with Axis angle due to readability and as that was what we were using before.

Copy link
Contributor

Choose a reason for hiding this comment

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

Sounds good, thanks!

test.That(t, actualOri.RY, test.ShouldBeBetween, expectedOri.OY-toleranceOri, expectedOri.OY+toleranceOri)
test.That(t, actualOri.RZ, test.ShouldBeBetween, expectedOri.OZ-toleranceOri, expectedOri.OZ+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 @@ -97,7 +124,8 @@ func integrationtestHelperCartographer(t *testing.T, mode slam.Mode) {
}
}

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 @@ -155,7 +183,8 @@ func integrationtestHelperCartographer(t *testing.T, mode slam.Mode) {
}
}

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 @@ -228,12 +257,18 @@ func integrationtestHelperCartographer(t *testing.T, mode slam.Mode) {
}
}

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 @@ -292,10 +327,13 @@ func integrationtestHelperCartographer(t *testing.T, mode slam.Mode) {
prevNumFiles = checkDeleteProcessedData(t, mode, name, prevNumFiles, len(attrCfg.Sensors) != 0, deleteProcessedData)
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
71 changes: 46 additions & 25 deletions services/slam/builtin/orbslam_int_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,11 @@ import (
"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"
"go.viam.com/utils/artifact"
Expand Down Expand Up @@ -66,35 +68,51 @@ func releaseImages(t *testing.T, mode slam.Mode) {
}
}

// Checks that we can get position and map, and that there are more than zero map points.
// Doesn't check precise values due to variations in orbslam results.
func testOrbslamPositionAndMap(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 RGBD are around (-0.001, -0.004, -0.008)
// Typical values for Mono without an existing map are around (0.020, -0.032, -0.053)
// Typical values for Mono with an existing map are around (0.023, -0.036, -0.040)
t.Logf("Position point: (%v, %v, %v)",
position.Pose().Point().X, position.Pose().Point().Y, position.Pose().Point().Z)
// Typical values for RGBD are around (0.602, -0.772, -0.202), theta=0.002
// Typical values for Mono without an existing map are around (0.144, 0.980, -0.137), theta=0.104
// Typical values for Mono with an existing map are around ( 0.092, 0.993, -0.068), theta=0.099
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 orbslam map and confirms there are more than zero map points.
func testOrbslamMap(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")
// Typical value for RGBD is 329
// Values for Mono vary
t.Logf("Pointcloud points: %v", pointcloud.Size())
test.That(t, pointcloud.Size(), test.ShouldBeGreaterThan, 0)
}

// Checks the orbslam position within a defined tolerance
func testOrbslamPosition(t *testing.T, svc slam.Service, mode, actionMode string) {
var expectedPos r3.Vector
expectedOri := &spatialmath.OrientationVector{}
tolerancePos := 0.05
toleranceOri := 0.5

switch {
case mode == "mono" && actionMode == "mapping":
expectedPos = r3.Vector{X: 0.020, Y: -0.032, Z: -0.053}
Copy link
Author

Choose a reason for hiding this comment

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

Defined expected position and orientation here as opposed to passing them into the function as it centralizes any changes to the expected values to one location.

Let me know if you'd prefer we handle this a different way.

expectedOri = &spatialmath.OrientationVector{Theta: 0.104, OX: 0.144, OY: 0.980, OZ: -0.137}
case mode == "mono" && actionMode == "updating":
expectedPos = r3.Vector{X: 0.023, Y: -0.036, Z: -0.040}
expectedOri = &spatialmath.OrientationVector{Theta: 0.099, OX: 0.092, OY: 0.993, OZ: -0.068}
case mode == "rgbd":
expectedPos = r3.Vector{X: -0.001, Y: -0.004, Z: -0.008}
expectedOri = &spatialmath.OrientationVector{Theta: 0.002, OX: 0.602, OY: -0.772, OZ: -0.202}
}

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.OX-toleranceOri, expectedOri.OX+toleranceOri)
Copy link
Contributor

Choose a reason for hiding this comment

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

Same here, it seems incorrect that we're comparing axis angle representation to orientation vector representation.

Copy link
Author

Choose a reason for hiding this comment

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

See previous comment

test.That(t, actualOri.RY, test.ShouldBeBetween, expectedOri.OY-toleranceOri, expectedOri.OY+toleranceOri)
test.That(t, actualOri.RZ, test.ShouldBeBetween, expectedOri.OZ-toleranceOri, expectedOri.OZ+toleranceOri)
test.That(t, actualOri.Theta, test.ShouldBeBetween, expectedOri.Theta-toleranceOri, expectedOri.Theta+toleranceOri)
}

func integrationTestHelperOrbslam(t *testing.T, mode slam.Mode) {
_, err := exec.LookPath("orb_grpc_server")
if err != nil {
Expand Down Expand Up @@ -184,7 +202,8 @@ func integrationTestHelperOrbslam(t *testing.T, mode slam.Mode) {
}
}

testOrbslamPositionAndMap(t, svc)
testOrbslamPosition(t, svc, reflect.ValueOf(mode).String(), "mapping")
testOrbslamMap(t, svc)

// Close out slam service
err = utils.TryClose(context.Background(), svc)
Expand Down Expand Up @@ -279,7 +298,8 @@ func integrationTestHelperOrbslam(t *testing.T, mode slam.Mode) {
}
}

testOrbslamPositionAndMap(t, svc)
testOrbslamPosition(t, svc, reflect.ValueOf(mode).String(), "mapping")
testOrbslamMap(t, svc)

if !orbslam_hangs {
// Wait for the final map to be saved
Expand Down Expand Up @@ -384,7 +404,8 @@ func integrationTestHelperOrbslam(t *testing.T, mode slam.Mode) {
}
}

testOrbslamPositionAndMap(t, svc)
testOrbslamPosition(t, svc, reflect.ValueOf(mode).String(), "updating")
testOrbslamMap(t, svc)

// Close out slam service
err = utils.TryClose(context.Background(), svc)
Expand Down
17 changes: 17 additions & 0 deletions services/slam/client.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -129,3 +129,20 @@ func (c *client) GetMap(

return mimeType, imageData, vObject, nil
}

// GetInternalState creates a request, calls the slam service GetInternalState, and parses the response into bytes.
func (c *client) GetInternalState(ctx context.Context, name string) ([]byte, error) {
ctx, span := trace.StartSpan(ctx, "slam::client::GetInternalState")
defer span.End()

req := &pb.GetInternalStateRequest{Name: name}

resp, err := c.client.GetInternalState(ctx, req)
if err != nil {
return nil, err
}

internalState := resp.GetInternalState()

return internalState, nil
}
Loading