-
Notifications
You must be signed in to change notification settings - Fork 110
/
server.go
126 lines (114 loc) · 3.52 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
123
124
125
126
// Package arm contains a gRPC based arm service server.
package arm
import (
"context"
"github.com/pkg/errors"
pb "go.viam.com/api/component/arm/v1"
"go.viam.com/rdk/operation"
"go.viam.com/rdk/referenceframe"
"go.viam.com/rdk/spatialmath"
"go.viam.com/rdk/subtype"
)
// subtypeServer implements the ArmService from arm.proto.
type subtypeServer struct {
pb.UnimplementedArmServiceServer
s subtype.Service
}
// NewServer constructs an arm gRPC service server.
func NewServer(s subtype.Service) pb.ArmServiceServer {
return &subtypeServer{s: s}
}
// getArm returns the arm specified, nil if not.
func (s *subtypeServer) getArm(name string) (Arm, error) {
resource := s.s.Resource(name)
if resource == nil {
return nil, errors.Errorf("no arm with name (%s)", name)
}
arm, ok := resource.(Arm)
if !ok {
return nil, errors.Errorf("resource with name (%s) is not an arm", name)
}
return arm, nil
}
// GetEndPosition returns the position of the arm specified.
func (s *subtypeServer) GetEndPosition(
ctx context.Context,
req *pb.GetEndPositionRequest,
) (*pb.GetEndPositionResponse, error) {
arm, err := s.getArm(req.Name)
if err != nil {
return nil, err
}
pos, err := arm.EndPosition(ctx, req.Extra.AsMap())
if err != nil {
return nil, err
}
return &pb.GetEndPositionResponse{Pose: spatialmath.PoseToProtobuf(pos)}, nil
}
// GetJointPositions gets the current joint position of an arm of the underlying robot.
func (s *subtypeServer) GetJointPositions(
ctx context.Context,
req *pb.GetJointPositionsRequest,
) (*pb.GetJointPositionsResponse, error) {
arm, err := s.getArm(req.Name)
if err != nil {
return nil, err
}
pos, err := arm.JointPositions(ctx, req.Extra.AsMap())
if err != nil {
return nil, err
}
convertedPos := &pb.JointPositions{Values: pos.Values}
return &pb.GetJointPositionsResponse{Positions: convertedPos}, nil
}
// MoveToPosition returns the position of the arm specified.
func (s *subtypeServer) MoveToPosition(ctx context.Context, req *pb.MoveToPositionRequest) (*pb.MoveToPositionResponse, error) {
operation.CancelOtherWithLabel(ctx, req.Name)
arm, err := s.getArm(req.Name)
if err != nil {
return nil, err
}
worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState())
if err != nil {
return nil, err
}
return &pb.MoveToPositionResponse{}, arm.MoveToPosition(
ctx,
spatialmath.NewPoseFromProtobuf(req.GetTo()),
worldState,
req.Extra.AsMap(),
)
}
// MoveToJointPositions moves an arm of the underlying robot to the requested joint positions.
func (s *subtypeServer) MoveToJointPositions(
ctx context.Context,
req *pb.MoveToJointPositionsRequest,
) (*pb.MoveToJointPositionsResponse, error) {
operation.CancelOtherWithLabel(ctx, req.Name)
arm, err := s.getArm(req.Name)
if err != nil {
return nil, err
}
return &pb.MoveToJointPositionsResponse{}, arm.MoveToJointPositions(ctx, req.Positions, req.Extra.AsMap())
}
// Stop stops the arm specified.
func (s *subtypeServer) Stop(ctx context.Context, req *pb.StopRequest) (*pb.StopResponse, error) {
operation.CancelOtherWithLabel(ctx, req.Name)
arm, err := s.getArm(req.Name)
if err != nil {
return nil, err
}
return &pb.StopResponse{}, arm.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) {
arm, err := s.getArm(req.GetName())
if err != nil {
return nil, err
}
moving, err := arm.IsMoving(ctx)
if err != nil {
return nil, err
}
return &pb.IsMovingResponse{IsMoving: moving}, nil
}