-
Notifications
You must be signed in to change notification settings - Fork 110
/
slam.go
89 lines (76 loc) · 2.75 KB
/
slam.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
// Package slam implements simultaneous localization and mapping.
// This is an Experimental package.
package slam
import (
"context"
"io"
"github.com/pkg/errors"
"go.opencensus.io/trace"
pb "go.viam.com/api/service/slam/v1"
"go.viam.com/rdk/resource"
"go.viam.com/rdk/robot"
"go.viam.com/rdk/spatialmath"
)
// TBD 05/04/2022: Needs more work once GRPC is included (future PR).
func init() {
resource.RegisterAPI(API, resource.APIRegistration[Service]{
RPCServiceServerConstructor: NewRPCServiceServer,
RPCServiceHandler: pb.RegisterSLAMServiceHandlerFromEndpoint,
RPCServiceDesc: &pb.SLAMService_ServiceDesc,
RPCClient: NewClientFromConn,
})
}
// SubtypeName is the name of the type of service.
const SubtypeName = "slam"
// API is a variable that identifies the slam resource API.
var API = resource.APINamespaceRDK.WithServiceType(SubtypeName)
// Named is a helper for getting the named service's typed resource name.
func Named(name string) resource.Name {
return resource.NewName(API, name)
}
// FromRobot is a helper for getting the named SLAM service from the given Robot.
func FromRobot(r robot.Robot, name string) (Service, error) {
return robot.ResourceFromRobot[Service](r, Named(name))
}
// Service describes the functions that are available to the service.
type Service interface {
resource.Resource
GetPosition(context.Context) (spatialmath.Pose, string, error)
GetPointCloudMap(ctx context.Context) (func() ([]byte, error), error)
GetInternalState(ctx context.Context) (func() ([]byte, error), error)
}
// HelperConcatenateChunksToFull concatenates the chunks from a streamed grpc endpoint.
func HelperConcatenateChunksToFull(f func() ([]byte, error)) ([]byte, error) {
var fullBytes []byte
for {
chunk, err := f()
if errors.Is(err, io.EOF) {
return fullBytes, nil
}
if err != nil {
return nil, err
}
fullBytes = append(fullBytes, chunk...)
}
}
// GetPointCloudMapFull concatenates the streaming responses from GetPointCloudMap into a full point cloud.
func GetPointCloudMapFull(ctx context.Context, slamSvc Service) ([]byte, error) {
ctx, span := trace.StartSpan(ctx, "slam::GetPointCloudMapFull")
defer span.End()
callback, err := slamSvc.GetPointCloudMap(ctx)
if err != nil {
return nil, err
}
return HelperConcatenateChunksToFull(callback)
}
// GetInternalStateFull concatenates the streaming responses from GetInternalState into
// the internal serialized state of the slam algorithm.
func GetInternalStateFull(ctx context.Context, slamSvc Service) ([]byte, error) {
ctx, span := trace.StartSpan(ctx, "slam::GetInternalStateFull")
defer span.End()
callback, err := slamSvc.GetInternalState(ctx)
if err != nil {
return nil, err
}
return HelperConcatenateChunksToFull(callback)
}