Skip to content

Commit

Permalink
[RSDK-2238] add-final-slam-map-res-grpc-methods (#2130)
Browse files Browse the repository at this point in the history
  • Loading branch information
@nicksanford
nicksanford authored Mar 30, 2023
1 parent a11e615 commit 63c2263
Show file tree
Hide file tree
Showing 17 changed files with 422 additions and 180 deletions.
20 changes: 10 additions & 10 deletions services/slam/builtin/builtin.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -257,9 +257,9 @@ func (slamSvc *builtIn) GetPosition(ctx context.Context, name string) (spatialma
ctx, span := trace.StartSpan(ctx, "slam::builtIn::GetPosition")
defer span.End()

req := &pb.GetPositionNewRequest{Name: name}
req := &pb.GetPositionRequest{Name: name}

resp, err := slamSvc.clientAlgo.GetPositionNew(ctx, req)
resp, err := slamSvc.clientAlgo.GetPosition(ctx, req)
if err != nil {
return nil, "", errors.Wrap(err, "error getting SLAM position")
}
Expand All @@ -270,22 +270,22 @@ func (slamSvc *builtIn) GetPosition(ctx context.Context, name string) (spatialma
return slamUtils.CheckQuaternionFromClientAlgo(pose, componentReference, returnedExt)
}

// GetPointCloudMapStream creates a request, calls the slam algorithms GetPointCloudMapStream endpoint and returns a callback
// GetPointCloudMap creates a request, calls the slam algorithms GetPointCloudMap endpoint and returns a callback
// function which will return the next chunk of the current pointcloud map.
func (slamSvc *builtIn) GetPointCloudMapStream(ctx context.Context, name string) (func() ([]byte, error), error) {
ctx, span := trace.StartSpan(ctx, "slam::builtIn::GetPointCloudMapStream")
func (slamSvc *builtIn) GetPointCloudMap(ctx context.Context, name string) (func() ([]byte, error), error) {
ctx, span := trace.StartSpan(ctx, "slam::builtIn::GetPointCloudMap")
defer span.End()

return grpchelper.GetPointCloudMapStreamCallback(ctx, name, slamSvc.clientAlgo)
return grpchelper.GetPointCloudMapCallback(ctx, name, slamSvc.clientAlgo)
}

// GetInternalStateStream creates a request, calls the slam algorithms GetInternalStateStream endpoint and returns a callback
// GetInternalState creates a request, calls the slam algorithms GetInternalState endpoint and returns a callback
// function which will return the next chunk of the current internal state of the slam algo.
func (slamSvc *builtIn) GetInternalStateStream(ctx context.Context, name string) (func() ([]byte, error), error) {
ctx, span := trace.StartSpan(ctx, "slam::builtIn::GetInternalStateStream")
func (slamSvc *builtIn) GetInternalState(ctx context.Context, name string) (func() ([]byte, error), error) {
ctx, span := trace.StartSpan(ctx, "slam::builtIn::GetInternalState")
defer span.End()

return grpchelper.GetInternalStateStreamCallback(ctx, name, slamSvc.clientAlgo)
return grpchelper.GetInternalStateCallback(ctx, name, slamSvc.clientAlgo)
}

// NewBuiltIn returns a new slam service for the given robot.
Expand Down
4 changes: 2 additions & 2 deletions services/slam/builtin/builtin_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -982,14 +982,14 @@ func TestEndpointFailures(t *testing.T) {
test.That(t, frame, test.ShouldBeEmpty)
test.That(t, fmt.Sprint(err), test.ShouldContainSubstring, "error getting SLAM position")

callbackPointCloud, err := svc.GetPointCloudMapStream(context.Background(), "hi")
callbackPointCloud, err := svc.GetPointCloudMap(context.Background(), "hi")
test.That(t, err, test.ShouldBeNil)
test.That(t, callbackPointCloud, test.ShouldNotBeNil)
chunkPCD, err := callbackPointCloud()
test.That(t, err.Error(), test.ShouldContainSubstring, "error receiving pointcloud chunk")
test.That(t, chunkPCD, test.ShouldBeNil)

callbackInternalState, err := svc.GetInternalStateStream(context.Background(), "hi")
callbackInternalState, err := svc.GetInternalState(context.Background(), "hi")
test.That(t, err, test.ShouldBeNil)
test.That(t, callbackInternalState, test.ShouldNotBeNil)
chunkInternalState, err := callbackInternalState()
Expand Down
2 changes: 1 addition & 1 deletion services/slam/builtin/cartographer_int_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ func testCartographerInternalState(t *testing.T, svc slam.Service, dataDir strin
internalState, err := slam.GetInternalStateFull(context.Background(), svc, "test")
test.That(t, err, test.ShouldBeNil)

// Save the data from the call to GetInternalStateStream for use in next test.
// 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)
Expand Down
2 changes: 1 addition & 1 deletion services/slam/builtin/orbslam_int_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,7 @@ func testOrbslamInternalState(t *testing.T, svc slam.Service, dataDir string) {
internalState, err := slam.GetInternalStateFull(context.Background(), svc, "test")
test.That(t, err, test.ShouldBeNil)

// Save the data from the call to GetInternalStateStream for use in next test.
// Save the data from the call to GetInternalState for use in next test.
timeStamp := time.Now()
filename := filepath.Join(dataDir, "map", "orbslam_int_color_camera_data_"+timeStamp.UTC().Format(slamTimeFormat)+".osa")
err = os.WriteFile(filename, internalState, 0644)
Expand Down
20 changes: 10 additions & 10 deletions services/slam/client.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,11 +40,11 @@ func (c *client) GetPosition(ctx context.Context, name string) (spatialmath.Pose
ctx, span := trace.StartSpan(ctx, "slam::client::GetPosition")
defer span.End()

req := &pb.GetPositionNewRequest{
req := &pb.GetPositionRequest{
Name: name,
}

resp, err := c.client.GetPositionNew(ctx, req)
resp, err := c.client.GetPosition(ctx, req)
if err != nil {
return nil, "", err
}
Expand All @@ -55,22 +55,22 @@ func (c *client) GetPosition(ctx context.Context, name string) (spatialmath.Pose
return spatialmath.NewPoseFromProtobuf(p), componentReference, nil
}

// GetPointCloudMapStream creates a request, calls the slam service GetPointCloudMapStream and returns a callback
// GetPointCloudMap creates a request, calls the slam service GetPointCloudMap and returns a callback
// function which will return the next chunk of the current pointcloud map when called.
func (c *client) GetPointCloudMapStream(ctx context.Context, name string) (func() ([]byte, error), error) {
ctx, span := trace.StartSpan(ctx, "slam::client::GetPointCloudMapStream")
func (c *client) GetPointCloudMap(ctx context.Context, name string) (func() ([]byte, error), error) {
ctx, span := trace.StartSpan(ctx, "slam::client::GetPointCloudMap")
defer span.End()

return grpchelper.GetPointCloudMapStreamCallback(ctx, name, c.client)
return grpchelper.GetPointCloudMapCallback(ctx, name, c.client)
}

// GetInternalStateStream creates a request, calls the slam service GetInternalStateStream and returns a callback
// GetInternalState creates a request, calls the slam service GetInternalState and returns a callback
// function which will return the next chunk of the current internal state of the slam algo when called.
func (c *client) GetInternalStateStream(ctx context.Context, name string) (func() ([]byte, error), error) {
ctx, span := trace.StartSpan(ctx, "slam::client::GetInternalStateStream")
func (c *client) GetInternalState(ctx context.Context, name string) (func() ([]byte, error), error) {
ctx, span := trace.StartSpan(ctx, "slam::client::GetInternalState")
defer span.End()

return grpchelper.GetInternalStateStreamCallback(ctx, name, c.client)
return grpchelper.GetInternalStateCallback(ctx, name, c.client)
}

func (c *client) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) {
Expand Down
16 changes: 8 additions & 8 deletions services/slam/client_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ func TestClientWorkingService(t *testing.T) {
return poseSucc, componentRefSucc, nil
}

workingSLAMService.GetPointCloudMapStreamFunc = func(ctx context.Context, name string) (func() ([]byte, error), error) {
workingSLAMService.GetPointCloudMapFunc = func(ctx context.Context, name string) (func() ([]byte, error), error) {
reader := bytes.NewReader(pcd)
clientBuffer := make([]byte, chunkSizePointCloud)
f := func() ([]byte, error) {
Expand All @@ -73,7 +73,7 @@ func TestClientWorkingService(t *testing.T) {
return f, nil
}

workingSLAMService.GetInternalStateStreamFunc = func(ctx context.Context, name string) (func() ([]byte, error), error) {
workingSLAMService.GetInternalStateFunc = func(ctx context.Context, name string) (func() ([]byte, error), error) {
reader := bytes.NewReader(internalStateSucc)
clientBuffer := make([]byte, chunkSizeInternalState)
f := func() ([]byte, error) {
Expand Down Expand Up @@ -214,11 +214,11 @@ func TestFailingClient(t *testing.T) {
return nil, "", errors.New("failure to get position")
}

failingSLAMService.GetPointCloudMapStreamFunc = func(ctx context.Context, name string) (func() ([]byte, error), error) {
failingSLAMService.GetPointCloudMapFunc = func(ctx context.Context, name string) (func() ([]byte, error), error) {
return nil, errors.New("failure during get pointcloud map stream")
}

failingSLAMService.GetInternalStateStreamFunc = func(ctx context.Context, name string) (func() ([]byte, error), error) {
failingSLAMService.GetInternalStateFunc = func(ctx context.Context, name string) (func() ([]byte, error), error) {
return nil, errors.New("failure during get internal state stream")
}

Expand All @@ -241,9 +241,9 @@ func TestFailingClient(t *testing.T) {
ctx := context.Background()
cancelCtx, cancelFunc := context.WithCancel(ctx)
cancelFunc()
_, err = failingSLAMClient.GetPointCloudMapStream(cancelCtx, nameFail)
_, err = failingSLAMClient.GetPointCloudMap(cancelCtx, nameFail)
test.That(t, err.Error(), test.ShouldContainSubstring, "context cancel")
_, err = failingSLAMClient.GetInternalStateStream(cancelCtx, nameFail)
_, err = failingSLAMClient.GetInternalState(cancelCtx, nameFail)
test.That(t, err.Error(), test.ShouldContainSubstring, "context cancel")

// test get position
Expand All @@ -265,14 +265,14 @@ func TestFailingClient(t *testing.T) {
test.That(t, conn.Close(), test.ShouldBeNil)
})

failingSLAMService.GetPointCloudMapStreamFunc = func(ctx context.Context, name string) (func() ([]byte, error), error) {
failingSLAMService.GetPointCloudMapFunc = func(ctx context.Context, name string) (func() ([]byte, error), error) {
f := func() ([]byte, error) {
return nil, errors.New("failure during callback")
}
return f, nil
}

failingSLAMService.GetInternalStateStreamFunc = func(ctx context.Context, name string) (func() ([]byte, error), error) {
failingSLAMService.GetInternalStateFunc = func(ctx context.Context, name string) (func() ([]byte, error), error) {
f := func() ([]byte, error) {
return nil, errors.New("failure during callback")
}
Expand Down
4 changes: 2 additions & 2 deletions services/slam/fake/data_loader.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ const (
positionTemplate = "%s/position/position_%d.json"
)

func fakeGetPointCloudMapStream(_ context.Context, datasetDir string, slamSvc *SLAM) (func() ([]byte, error), error) {
func fakeGetPointCloudMap(_ context.Context, datasetDir string, slamSvc *SLAM) (func() ([]byte, error), error) {
path := filepath.Clean(artifact.MustPath(fmt.Sprintf(pcdTemplate, datasetDir, slamSvc.getCount())))
slamSvc.logger.Debug("Reading " + path)
file, err := os.Open(path)
Expand All @@ -65,7 +65,7 @@ func fakeGetPointCloudMapStream(_ context.Context, datasetDir string, slamSvc *S
return f, nil
}

func fakeGetInternalStateStream(_ context.Context, datasetDir string, slamSvc *SLAM) (func() ([]byte, error), error) {
func fakeGetInternalState(_ context.Context, datasetDir string, slamSvc *SLAM) (func() ([]byte, error), error) {
path := filepath.Clean(artifact.MustPath(fmt.Sprintf(internalStateTemplate, datasetDir, slamSvc.getCount())))
slamSvc.logger.Debug("Reading " + path)
file, err := os.Open(path)
Expand Down
16 changes: 8 additions & 8 deletions services/slam/fake/slam.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,21 +65,21 @@ func (slamSvc *SLAM) GetPosition(ctx context.Context, name string) (spatialmath.
return fakeGetPosition(ctx, datasetDirectory, slamSvc)
}

// GetPointCloudMapStream returns a callback function which will return the next chunk of the current pointcloud
// GetPointCloudMap returns a callback function which will return the next chunk of the current pointcloud
// map.
func (slamSvc *SLAM) GetPointCloudMapStream(ctx context.Context, name string) (func() ([]byte, error), error) {
ctx, span := trace.StartSpan(ctx, "slam::fake::GetPointCloudMapStream")
func (slamSvc *SLAM) GetPointCloudMap(ctx context.Context, name string) (func() ([]byte, error), error) {
ctx, span := trace.StartSpan(ctx, "slam::fake::GetPointCloudMap")
defer span.End()
slamSvc.incrementDataCount()
return fakeGetPointCloudMapStream(ctx, datasetDirectory, slamSvc)
return fakeGetPointCloudMap(ctx, datasetDirectory, slamSvc)
}

// GetInternalStateStream returns a callback function which will return the next chunk of the current internal
// GetInternalState returns a callback function which will return the next chunk of the current internal
// state of the slam algo.
func (slamSvc *SLAM) GetInternalStateStream(ctx context.Context, name string) (func() ([]byte, error), error) {
ctx, span := trace.StartSpan(ctx, "slam::fake::GetInternalStateStream")
func (slamSvc *SLAM) GetInternalState(ctx context.Context, name string) (func() ([]byte, error), error) {
ctx, span := trace.StartSpan(ctx, "slam::fake::GetInternalState")
defer span.End()
return fakeGetInternalStateStream(ctx, datasetDirectory, slamSvc)
return fakeGetInternalState(ctx, datasetDirectory, slamSvc)
}

// incrementDataCount is not thread safe but that is ok as we only intend a single user to be interacting
Expand Down
34 changes: 17 additions & 17 deletions services/slam/fake/slam_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,11 +57,11 @@ func TestFakeSLAMGetInternalState(t *testing.T) {
expectedData, err := os.ReadFile(path)
test.That(t, err, test.ShouldBeNil)

data := getDataFromStream(t, slamSvc.GetInternalStateStream, slamSvc.Name)
data := getDataFromStream(t, slamSvc.GetInternalState, slamSvc.Name)
test.That(t, len(data), test.ShouldBeGreaterThan, 0)
test.That(t, data, test.ShouldResemble, expectedData)

data2 := getDataFromStream(t, slamSvc.GetInternalStateStream, slamSvc.Name)
data2 := getDataFromStream(t, slamSvc.GetInternalState, slamSvc.Name)
test.That(t, len(data2), test.ShouldBeGreaterThan, 0)
test.That(t, data, test.ShouldResemble, data2)
test.That(t, data2, test.ShouldResemble, expectedData)
Expand All @@ -73,7 +73,7 @@ func TestFakeSLAMGetPointMap(t *testing.T) {
t.Run(testName, func(t *testing.T) {
slamSvc := NewSLAM("test", golog.NewTestLogger(t))

data := getDataFromStream(t, slamSvc.GetPointCloudMapStream, slamSvc.Name)
data := getDataFromStream(t, slamSvc.GetPointCloudMap, slamSvc.Name)
test.That(t, len(data), test.ShouldBeGreaterThan, 0)

path := filepath.Clean(artifact.MustPath(fmt.Sprintf(pcdTemplate, datasetDirectory, slamSvc.getCount())))
Expand All @@ -82,7 +82,7 @@ func TestFakeSLAMGetPointMap(t *testing.T) {

test.That(t, data, test.ShouldResemble, expectedData)

data2 := getDataFromStream(t, slamSvc.GetPointCloudMapStream, slamSvc.Name)
data2 := getDataFromStream(t, slamSvc.GetPointCloudMap, slamSvc.Name)
test.That(t, len(data2), test.ShouldBeGreaterThan, 0)

path2 := filepath.Clean(artifact.MustPath(fmt.Sprintf(pcdTemplate, datasetDirectory, slamSvc.getCount())))
Expand Down Expand Up @@ -120,11 +120,11 @@ func verifyGetPointCloudMapStateful(t *testing.T, slamSvc *SLAM) {
testDataCount := maxDataCount
getPointCloudMapResults := []float64{}
getPositionResults := []spatialmath.Pose{}
getInternalStateStreamResults := []int{}
getInternalStateResults := []int{}

// Call GetPointCloudMapStream twice for every testData artifact
// Call GetPointCloudMap twice for every testData artifact
for i := 0; i < testDataCount*2; i++ {
f, err := slamSvc.GetPointCloudMapStream(context.Background(), slamSvc.Name)
f, err := slamSvc.GetPointCloudMap(context.Background(), slamSvc.Name)
test.That(t, err, test.ShouldBeNil)
test.That(t, f, test.ShouldNotBeNil)
pcd, err := helperConcatenateChunksToFull(f)
Expand All @@ -139,39 +139,39 @@ func verifyGetPointCloudMapStateful(t *testing.T, slamSvc *SLAM) {
test.That(t, err, test.ShouldBeNil)
getPositionResults = append(getPositionResults, p)

f, err = slamSvc.GetInternalStateStream(context.Background(), slamSvc.Name)
f, err = slamSvc.GetInternalState(context.Background(), slamSvc.Name)
test.That(t, err, test.ShouldBeNil)
test.That(t, f, test.ShouldNotBeNil)
internalState, err := helperConcatenateChunksToFull(f)
test.That(t, err, test.ShouldBeNil)
getInternalStateStreamResults = append(getInternalStateStreamResults, len(internalState))
getInternalStateResults = append(getInternalStateResults, len(internalState))
}

getPositionResultsFirst := getPositionResults[len(getPositionResults)/2:]
getPositionResultsLast := getPositionResults[:len(getPositionResults)/2]

getInternalStateStreamResultsFirst := getInternalStateStreamResults[len(getInternalStateStreamResults)/2:]
getInternalStateStreamResultsLast := getInternalStateStreamResults[:len(getInternalStateStreamResults)/2]
getInternalStateResultsFirst := getInternalStateResults[len(getInternalStateResults)/2:]
getInternalStateResultsLast := getInternalStateResults[:len(getInternalStateResults)/2]

getPointCloudMapResultsFirst := getPointCloudMapResults[len(getPointCloudMapResults)/2:]
getPointCloudMapResultsLast := getPointCloudMapResults[:len(getPointCloudMapResults)/2]

// Confirm that the first half of the
// results equal the last.
// This proves that each call to GetPointCloudMapStream
// advances the test data (both for GetPointCloudMapStream & other endpoints)
// This proves that each call to GetPointCloudMap
// advances the test data (both for GetPointCloudMap & other endpoints)
// over a dataset of size maxDataCount that loops around.
test.That(t, getPositionResultsFirst, test.ShouldResemble, getPositionResultsLast)
test.That(t, getInternalStateStreamResultsFirst, test.ShouldResemble, getInternalStateStreamResultsLast)
test.That(t, getInternalStateResultsFirst, test.ShouldResemble, getInternalStateResultsLast)
test.That(t, getPointCloudMapResultsFirst, test.ShouldResemble, getPointCloudMapResultsLast)

// Confirm that the first half of the
// results do NOT equal the last half in reverse.
// This proves that each call to GetPointCloudMapStream
// advances the test data (both for GetPointCloudMapStream & other endpoints)
// This proves that each call to GetPointCloudMap
// advances the test data (both for GetPointCloudMap & other endpoints)
// over a dataset of size maxDataCount that loops around.
test.That(t, getPositionResultsFirst, test.ShouldNotResemble, reverse(getPositionResultsLast))
test.That(t, getInternalStateStreamResultsFirst, test.ShouldNotResemble, reverse(getInternalStateStreamResultsLast))
test.That(t, getInternalStateResultsFirst, test.ShouldNotResemble, reverse(getInternalStateResultsLast))
test.That(t, getPointCloudMapResultsFirst, test.ShouldNotResemble, reverse(getPointCloudMapResultsLast))
}

Expand Down
Loading

0 comments on commit 63c2263

Please sign in to comment.