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.

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

13 commits merged into from Jan 30, 2023

Conversation

ghost
Copy link

@ghost ghost commented Jan 18, 2023

This PR ands the GetInternalState functionality to the SLAM service in RDK. Similarly to the Position and GetMap, this function acts as a middleman, forwarding requests from RDK to the SLAM algorithm client and returning the response.

Currently GetInternalState returns a byte slice and an error.

Unit testing has been added as part of this PR for the client and server. Integration testing for cartographer has also been incorporated into this PR as it needed to be tested. Future integration testing will be necessary for orbslamv3.

JIRA Ticket: RSDK-1718
Associated JIRA Ticket: RSDK-1056

Associated PRs: PR #137 [carto] Implement GetInternalState

Note: Testing will not pass until the associated PR is merged.

@viambot viambot added the safe to test This pull request is marked safe to test from a trusted zone label Jan 18, 2023
@ghost ghost marked this pull request as ready for review January 18, 2023 06:30
@ghost ghost requested review from JohnN193 and nicksanford and removed request for nicksanford and JohnN193 January 18, 2023 06:30
@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 19, 2023
@nicksanford
Copy link
Member

nicksanford commented Jan 20, 2023

This PR ands the GetInternalState functionality to the SLAM service in RDK. Similarly to the Position and GetMap, this function acts as a middleman, forwarding requests from RDK to the SLAM algorithm client and returning the response.

Currently GetInternalState returns a byte slice and an error. Additionally, until a more significant use case for it has been implements it will save the map to a file located in the map folder in the directory specified by the slam config.

Unit testing has been added as part of this PR. Integration testing will be coming in a future PR once the SLAM GetInternalState is implemented in cartogrpaher and/or orbslamv3.

JIRA Ticket: RSDK-1718

Associated PRs

I don't understand why we would want this GRPC endpoint to have the side effect of saving the internal state to a file. Will that responsibility still live in RDK after slam modularization?

services/slam/builtin/builtin.go Outdated Show resolved Hide resolved
services/slam/slam.go Show resolved Hide resolved
testutils/inject/slam_service.go Show resolved Hide resolved
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

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")
test.That(t, pointcloud.Size(), test.ShouldBeGreaterThanOrEqualTo, 100)
}

// Checks the cartographer position.
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

services/slam/server_test.go Outdated Show resolved Hide resolved
services/slam/server_test.go Show resolved Hide resolved
services/slam/client_test.go Outdated Show resolved Hide resolved
services/slam/client_test.go Outdated Show resolved Hide resolved
@@ -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

@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 24, 2023
@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 24, 2023
@ghost ghost assigned kkufieta Jan 24, 2023
@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 24, 2023
@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 24, 2023
@@ -162,6 +175,21 @@ func TestClientWorkingService(t *testing.T) {
test.That(t, err, test.ShouldBeNil)
test.That(t, p.Parent(), test.ShouldEqual, pSucc.Parent())
test.That(t, extraOptions, test.ShouldResemble, extra)

// test get map
Copy link
Author

Choose a reason for hiding this comment

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

added missing test for GetMap

@viambot viambot removed the safe to test This pull request is marked safe to test from a trusted zone label Jan 24, 2023
@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 25, 2023
Copy link
Member

@JohnN193 JohnN193 left a comment

Choose a reason for hiding this comment

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

LGTM

@viambot viambot removed the safe to test This pull request is marked safe to test from a trusted zone label Jan 26, 2023

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.

@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 26, 2023
@ghost ghost requested a review from nicksanford January 26, 2023 19:13
@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 26, 2023
@@ -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

@ghost ghost requested a review from tessavitabile January 30, 2023 16:15
Copy link
Contributor

@tessavitabile tessavitabile left a comment

Choose a reason for hiding this comment

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

Just an issue in the test changes


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!


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

Copy link
Contributor

@tessavitabile tessavitabile left a comment

Choose a reason for hiding this comment

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

LGTM mod making sure we're comparing orientations of the same type

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.

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

@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 30, 2023
@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 30, 2023
@github-actions
Copy link
Contributor

Code Coverage

Package Line Rate Delta Health
go.viam.com/rdk/components/arm 56% 0.00%
go.viam.com/rdk/components/arm/universalrobots 41% 0.00%
go.viam.com/rdk/components/arm/xarm 2% 0.00%
go.viam.com/rdk/components/arm/yahboom 7% 0.00%
go.viam.com/rdk/components/audioinput 55% 0.00%
go.viam.com/rdk/components/base 63% 0.00%
go.viam.com/rdk/components/base/agilex 62% 0.00%
go.viam.com/rdk/components/base/boat 41% 0.00%
go.viam.com/rdk/components/base/wheeled 76% 0.00%
go.viam.com/rdk/components/board 66% 0.00%
go.viam.com/rdk/components/board/arduino 10% 0.00%
go.viam.com/rdk/components/board/commonsysfs 48% 0.00%
go.viam.com/rdk/components/board/fake 38% 0.00%
go.viam.com/rdk/components/board/numato 19% 0.00%
go.viam.com/rdk/components/board/pi 50% 0.00%
go.viam.com/rdk/components/camera 65% 0.00%
go.viam.com/rdk/components/camera/align 64% 0.00%
go.viam.com/rdk/components/camera/fake 64% 0.00%
go.viam.com/rdk/components/camera/ffmpeg 77% 0.00%
go.viam.com/rdk/components/camera/transformpipeline 78% -0.93%
go.viam.com/rdk/components/camera/videosource 46% +0.77%
go.viam.com/rdk/components/encoder 4% 0.00%
go.viam.com/rdk/components/encoder/fake 76% 0.00%
go.viam.com/rdk/components/gantry 60% 0.00%
go.viam.com/rdk/components/gantry/multiaxis 84% 0.00%
go.viam.com/rdk/components/gantry/oneaxis 86% 0.00%
go.viam.com/rdk/components/generic 83% 0.00%
go.viam.com/rdk/components/gripper 68% 0.00%
go.viam.com/rdk/components/input 87% 0.00%
go.viam.com/rdk/components/input/gpio 84% 0.00%
go.viam.com/rdk/components/motor 77% 0.00%
go.viam.com/rdk/components/motor/dimensionengineering 63% 0.00%
go.viam.com/rdk/components/motor/dmc4000 69% 0.00%
go.viam.com/rdk/components/motor/fake 57% 0.00%
go.viam.com/rdk/components/motor/gpio 64% +2.16%
go.viam.com/rdk/components/motor/gpiostepper 56% 0.00%
go.viam.com/rdk/components/motor/tmcstepper 62% 0.00%
go.viam.com/rdk/components/movementsensor 75% 0.00%
go.viam.com/rdk/components/movementsensor/cameramono 46% 0.00%
go.viam.com/rdk/components/movementsensor/gpsnmea 36% 0.00%
go.viam.com/rdk/components/movementsensor/gpsrtk 28% 0.00%
go.viam.com/rdk/components/posetracker 86% 0.00%
go.viam.com/rdk/components/sensor 86% 0.00%
go.viam.com/rdk/components/sensor/ultrasonic 26% 0.00%
go.viam.com/rdk/components/servo 68% 0.00%
go.viam.com/rdk/components/servo/gpio 71% 0.00%
go.viam.com/rdk/config 75% 0.00%
go.viam.com/rdk/control 57% 0.00%
go.viam.com/rdk/data 79% 0.00%
go.viam.com/rdk/examples/customresources/demos/complexmodule 0% 0.00%
go.viam.com/rdk/examples/customresources/demos/remoteserver 0% 0.00%
go.viam.com/rdk/grpc 25% 0.00%
go.viam.com/rdk/ml 67% 0.00%
go.viam.com/rdk/ml/inference 70% 0.00%
go.viam.com/rdk/module 64% 0.00%
go.viam.com/rdk/module/modmanager 79% 0.00%
go.viam.com/rdk/motionplan 70% -0.20%
go.viam.com/rdk/operation 82% 0.00%
go.viam.com/rdk/pointcloud 73% -0.08%
go.viam.com/rdk/protoutils 59% 0.00%
go.viam.com/rdk/referenceframe 71% 0.00%
go.viam.com/rdk/registry 89% 0.00%
go.viam.com/rdk/resource 84% 0.00%
go.viam.com/rdk/rimage 78% 0.00%
go.viam.com/rdk/rimage/depthadapter 94% 0.00%
go.viam.com/rdk/rimage/transform 75% 0.00%
go.viam.com/rdk/rimage/transform/cmd/extrinsic_calibration 67% 0.00%
go.viam.com/rdk/robot 85% 0.00%
go.viam.com/rdk/robot/client 81% 0.00%
go.viam.com/rdk/robot/framesystem 65% 0.00%
go.viam.com/rdk/robot/impl 80% 0.00%
go.viam.com/rdk/robot/server 55% 0.00%
go.viam.com/rdk/robot/web 61% 0.00%
go.viam.com/rdk/robot/web/stream 87% 0.00%
go.viam.com/rdk/services/armremotecontrol 71% 0.00%
go.viam.com/rdk/services/armremotecontrol/builtin 52% 0.00%
go.viam.com/rdk/services/baseremotecontrol 71% 0.00%
go.viam.com/rdk/services/baseremotecontrol/builtin 79% 0.00%
go.viam.com/rdk/services/datamanager 79% 0.00%
go.viam.com/rdk/services/datamanager/builtin 78% 0.00%
go.viam.com/rdk/services/datamanager/datacapture 70% 0.00%
go.viam.com/rdk/services/datamanager/datasync 0% 0.00%
go.viam.com/rdk/services/motion 63% 0.00%
go.viam.com/rdk/services/motion/builtin 88% 0.00%
go.viam.com/rdk/services/navigation 53% 0.00%
go.viam.com/rdk/services/sensors 77% 0.00%
go.viam.com/rdk/services/sensors/builtin 97% 0.00%
go.viam.com/rdk/services/shell 14% 0.00%
go.viam.com/rdk/services/slam 85% +0.83%
go.viam.com/rdk/services/slam/builtin 70% +0.06%
go.viam.com/rdk/services/vision 80% 0.00%
go.viam.com/rdk/services/vision/builtin 74% 0.00%
go.viam.com/rdk/session 97% 0.00%
go.viam.com/rdk/spatialmath 82% 0.00%
go.viam.com/rdk/subtype 92% 0.00%
go.viam.com/rdk/utils 72% +0.18%
go.viam.com/rdk/vision 26% 0.00%
go.viam.com/rdk/vision/chess 80% 0.00%
go.viam.com/rdk/vision/delaunay 87% 0.00%
go.viam.com/rdk/vision/keypoints 92% 0.00%
go.viam.com/rdk/vision/objectdetection 82% 0.00%
go.viam.com/rdk/vision/odometry 60% 0.00%
go.viam.com/rdk/vision/odometry/cmd 0% 0.00%
go.viam.com/rdk/vision/segmentation 49% 0.00%
go.viam.com/rdk/web/server 25% 0.00%
Summary 65% (21571 / 32948) +0.04%

@ghost ghost merged commit 84f30e3 into viamrobotics:main Jan 30, 2023
@ghost ghost deleted the RSDK-1718_GetInternalStateRDK branch February 1, 2023 19:49
randhid pushed a commit to randhid/rdk that referenced this pull request Feb 25, 2023
This pull request was closed.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
safe to test This pull request is marked safe to test from a trusted zone
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants