-
Notifications
You must be signed in to change notification settings - Fork 110
/
server.go
122 lines (110 loc) · 3.33 KB
/
server.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
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
// Package gantry contains a gRPC based gantry service server.
package gantry
import (
"context"
"github.com/pkg/errors"
commonpb "go.viam.com/api/common/v1"
pb "go.viam.com/api/component/gantry/v1"
"go.viam.com/rdk/operation"
"go.viam.com/rdk/protoutils"
"go.viam.com/rdk/referenceframe"
"go.viam.com/rdk/subtype"
)
// subtypeServer implements the GantryService from gantry.proto.
type subtypeServer struct {
pb.UnimplementedGantryServiceServer
s subtype.Service
}
// NewServer constructs an gantry gRPC service server.
func NewServer(s subtype.Service) pb.GantryServiceServer {
return &subtypeServer{s: s}
}
// getGantry returns the gantry specified, nil if not.
func (s *subtypeServer) getGantry(name string) (Gantry, error) {
resource := s.s.Resource(name)
if resource == nil {
return nil, errors.Errorf("no gantry with name (%s)", name)
}
gantry, ok := resource.(Gantry)
if !ok {
return nil, errors.Errorf("resource with name (%s) is not a gantry", name)
}
return gantry, nil
}
// GetPosition returns the position of the gantry specified.
func (s *subtypeServer) GetPosition(
ctx context.Context,
req *pb.GetPositionRequest,
) (*pb.GetPositionResponse, error) {
gantry, err := s.getGantry(req.Name)
if err != nil {
return nil, err
}
pos, err := gantry.Position(ctx, req.Extra.AsMap())
if err != nil {
return nil, err
}
return &pb.GetPositionResponse{PositionsMm: pos}, nil
}
// GetLengths gets the lengths of a gantry of the underlying robot.
func (s *subtypeServer) GetLengths(
ctx context.Context,
req *pb.GetLengthsRequest,
) (*pb.GetLengthsResponse, error) {
gantry, err := s.getGantry(req.Name)
if err != nil {
return nil, err
}
lengthsMm, err := gantry.Lengths(ctx, req.Extra.AsMap())
if err != nil {
return nil, err
}
return &pb.GetLengthsResponse{LengthsMm: lengthsMm}, nil
}
// MoveToPosition moves the gantry to the position specified.
func (s *subtypeServer) MoveToPosition(
ctx context.Context,
req *pb.MoveToPositionRequest,
) (*pb.MoveToPositionResponse, error) {
operation.CancelOtherWithLabel(ctx, req.Name)
gantry, err := s.getGantry(req.Name)
if err != nil {
return nil, err
}
worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState())
if err != nil {
return nil, err
}
return &pb.MoveToPositionResponse{}, gantry.MoveToPosition(ctx, req.PositionsMm, worldState, req.Extra.AsMap())
}
// Stop stops the gantry specified.
func (s *subtypeServer) Stop(ctx context.Context, req *pb.StopRequest) (*pb.StopResponse, error) {
operation.CancelOtherWithLabel(ctx, req.Name)
gantry, err := s.getGantry(req.Name)
if err != nil {
return nil, err
}
return &pb.StopResponse{}, gantry.Stop(ctx, req.Extra.AsMap())
}
// IsMoving queries of a component is in motion.
func (s *subtypeServer) IsMoving(ctx context.Context, req *pb.IsMovingRequest) (*pb.IsMovingResponse, error) {
gantry, err := s.getGantry(req.GetName())
if err != nil {
return nil, err
}
moving, err := gantry.IsMoving(ctx)
if err != nil {
return nil, err
}
return &pb.IsMovingResponse{IsMoving: moving}, nil
}
// DoCommand receives arbitrary commands.
func (s *subtypeServer) DoCommand(ctx context.Context,
req *commonpb.DoCommandRequest,
) (*commonpb.DoCommandResponse, error) {
gantry, err := s.getGantry(req.GetName())
if err != nil {
return nil, err
}
return protoutils.DoFromResourceServer(ctx, gantry, req)
}