-
Notifications
You must be signed in to change notification settings - Fork 109
/
server.go
139 lines (126 loc) · 3.89 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
127
128
129
130
131
132
133
134
135
136
137
138
139
// Package arm contains a gRPC based arm service server.
package arm
import (
"context"
"github.com/pkg/errors"
commonpb "go.viam.com/api/common/v1"
pb "go.viam.com/api/component/arm/v1"
"go.viam.com/rdk/operation"
"go.viam.com/rdk/protoutils"
"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
}
// DoCommand receives arbitrary commands.
func (s *subtypeServer) DoCommand(ctx context.Context,
req *commonpb.DoCommandRequest,
) (*commonpb.DoCommandResponse, error) {
arm, err := s.getArm(req.GetName())
if err != nil {
return nil, err
}
return protoutils.DoFromResourceServer(ctx, arm, req)
}