-
Notifications
You must be signed in to change notification settings - Fork 110
/
transformable.go
258 lines (228 loc) · 8.2 KB
/
transformable.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
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
package referenceframe
import (
"strconv"
"github.com/pkg/errors"
commonpb "go.viam.com/api/common/v1"
"go.viam.com/rdk/spatialmath"
)
// Transformable is an interface to describe elements that can be transformed by the frame system.
type Transformable interface {
Transform(*PoseInFrame) Transformable
Parent() string
}
// PoseInFrame is a data structure that packages a pose with the name of the
// frame in which it was observed.
type PoseInFrame struct {
parent string
pose spatialmath.Pose
name string
}
// LinkInFrame is a PoseInFrame plus a Geometry.
type LinkInFrame struct {
*PoseInFrame
geometry spatialmath.Geometry
}
// Geometry returns the Geometry of the LinkInFrame.
func (lF *LinkInFrame) Geometry() spatialmath.Geometry {
return lF.geometry
}
// ToStaticFrame converts a LinkInFrame into a staticFrame with a new name.
func (lF *LinkInFrame) ToStaticFrame(name string) (Frame, error) {
if name == "" {
name = lF.name
}
pose := lF.pose
if pose == nil {
pose = spatialmath.NewZeroPose()
}
if lF.geometry != nil {
return NewStaticFrameWithGeometry(name, pose, lF.geometry)
}
return NewStaticFrame(name, pose)
}
// Parent returns the name of the frame in which the pose was observed. Needed for Transformable interface.
func (pF *PoseInFrame) Parent() string {
return pF.parent
}
// SetParent sets the name of the frame in which the pose was observed.
func (pF *PoseInFrame) SetParent(parent string) {
pF.parent = parent
}
// Pose returns the pose that was observed.
func (pF *PoseInFrame) Pose() spatialmath.Pose {
return pF.pose
}
// Name returns the name of the PoseInFrame.
func (pF *PoseInFrame) Name() string {
return pF.name
}
// SetName sets the name of the PoseInFrame.
func (pF *PoseInFrame) SetName(name string) {
pF.name = name
}
// Transform changes the PoseInFrame pF into the reference frame specified by the tf argument.
// The tf PoseInFrame represents the pose of the pF reference frame with respect to the destination reference frame.
func (pF *PoseInFrame) Transform(tf *PoseInFrame) Transformable {
return NewPoseInFrame(tf.parent, spatialmath.Compose(tf.pose, pF.pose))
}
// NewPoseInFrame generates a new PoseInFrame.
func NewPoseInFrame(frame string, pose spatialmath.Pose) *PoseInFrame {
// fmt.Println("frame: ", frame)
return &PoseInFrame{
parent: frame,
pose: pose,
}
}
// NewLinkInFrame generates a new LinkInFrame.
func NewLinkInFrame(frame string, pose spatialmath.Pose, name string, geometry spatialmath.Geometry) *LinkInFrame {
return &LinkInFrame{
PoseInFrame: &PoseInFrame{
parent: frame,
pose: pose,
name: name,
},
geometry: geometry,
}
}
// PoseInFrameToProtobuf converts a PoseInFrame struct to a PoseInFrame protobuf message.
func PoseInFrameToProtobuf(framedPose *PoseInFrame) *commonpb.PoseInFrame {
poseProto := &commonpb.Pose{}
if framedPose.pose != nil {
poseProto = spatialmath.PoseToProtobuf(framedPose.pose)
}
return &commonpb.PoseInFrame{
ReferenceFrame: framedPose.parent,
Pose: poseProto,
}
}
// ProtobufToPoseInFrame converts a PoseInFrame protobuf message to a PoseInFrame struct.
func ProtobufToPoseInFrame(proto *commonpb.PoseInFrame) *PoseInFrame {
result := &PoseInFrame{}
result.pose = spatialmath.NewPoseFromProtobuf(proto.GetPose())
result.parent = proto.GetReferenceFrame()
return result
}
// LinkInFrameToTransformProtobuf converts a LinkInFrame struct to a Transform protobuf message.
func LinkInFrameToTransformProtobuf(framedLink *LinkInFrame) (*commonpb.Transform, error) {
if framedLink.PoseInFrame == nil {
return nil, ErrNilPoseInFrame
}
if framedLink.name == "" {
return nil, ErrEmptyStringFrameName
}
tform := &commonpb.Transform{
ReferenceFrame: framedLink.name,
PoseInObserverFrame: PoseInFrameToProtobuf(framedLink.PoseInFrame),
}
if framedLink.geometry != nil {
tform.PhysicalObject = framedLink.geometry.ToProtobuf()
}
return tform, nil
}
// LinkInFrameFromTransformProtobuf converts a Transform protobuf message to a LinkInFrame struct.
func LinkInFrameFromTransformProtobuf(proto *commonpb.Transform) (*LinkInFrame, error) {
var err error
frameName := proto.GetReferenceFrame()
if frameName == "" {
return nil, ErrEmptyStringFrameName
}
poseInObserverFrame := proto.GetPoseInObserverFrame()
parentFrame := poseInObserverFrame.GetReferenceFrame()
if parentFrame == "" {
return nil, ErrEmptyStringFrameName
}
poseMsg := poseInObserverFrame.GetPose()
pose := spatialmath.NewPoseFromProtobuf(poseMsg)
var geometry spatialmath.Geometry
if proto.PhysicalObject != nil {
geometry, err = spatialmath.NewGeometryFromProto(proto.PhysicalObject)
if err != nil {
return nil, err
}
}
return NewLinkInFrame(parentFrame, pose, frameName, geometry), nil
}
// LinkInFramesToTransformsProtobuf converts a slice of LinkInFrame structs to a slice of Transform protobuf messages.
// TODO(rb): use generics to operate on lists of arbirary types.
func LinkInFramesToTransformsProtobuf(linkSlice []*LinkInFrame) ([]*commonpb.Transform, error) {
protoTransforms := make([]*commonpb.Transform, 0, len(linkSlice))
for i, link := range linkSlice {
protoTf, err := LinkInFrameToTransformProtobuf(link)
if err != nil {
return nil, errors.Wrapf(err, "conversion error at index %d", i)
}
protoTransforms = append(protoTransforms, protoTf)
}
return protoTransforms, nil
}
// LinkInFramesFromTransformsProtobuf converts a slice of Transform protobuf messages to a slice of LinkInFrame structs.
// TODO(rb): use generics to operate on lists of arbirary proto types.
func LinkInFramesFromTransformsProtobuf(protoSlice []*commonpb.Transform) ([]*LinkInFrame, error) {
links := make([]*LinkInFrame, 0, len(protoSlice))
for i, protoTransform := range protoSlice {
link, err := LinkInFrameFromTransformProtobuf(protoTransform)
if err != nil {
return nil, errors.Wrapf(err, "conversion error at index %d", i)
}
links = append(links, link)
}
return links, nil
}
// GeometriesInFrame is a data structure that packages geometries with the name of the frame in which it was observed.
type GeometriesInFrame struct {
frame string
geometries map[string]spatialmath.Geometry
}
// Parent returns the name of the frame in which the geometries were observed.
func (gF *GeometriesInFrame) Parent() string {
return gF.frame
}
// Geometries returns the geometries observed.
func (gF *GeometriesInFrame) Geometries() map[string]spatialmath.Geometry {
if gF.geometries == nil {
return make(map[string]spatialmath.Geometry)
}
return gF.geometries
}
// Transform changes the GeometriesInFrame gF into the reference frame specified by the tf argument.
// The tf PoseInFrame represents the pose of the gF reference frame with respect to the destination reference frame.
func (gF *GeometriesInFrame) Transform(tf *PoseInFrame) Transformable {
geometries := make(map[string]spatialmath.Geometry)
for name, geometry := range gF.geometries {
geometries[name] = geometry.Transform(tf.pose)
}
return NewGeometriesInFrame(tf.parent, geometries)
}
// NewGeometriesInFrame generates a new GeometriesInFrame.
func NewGeometriesInFrame(frame string, geometries map[string]spatialmath.Geometry) *GeometriesInFrame {
return &GeometriesInFrame{
frame: frame,
geometries: geometries,
}
}
// GeometriesInFrameToProtobuf converts a GeometriesInFrame struct to a GeometriesInFrame message as specified in common.proto.
func GeometriesInFrameToProtobuf(framedGeometries *GeometriesInFrame) *commonpb.GeometriesInFrame {
var geometries []*commonpb.Geometry
for _, geometry := range framedGeometries.geometries {
geometries = append(geometries, geometry.ToProtobuf())
}
return &commonpb.GeometriesInFrame{
ReferenceFrame: framedGeometries.frame,
Geometries: geometries,
}
}
// ProtobufToGeometriesInFrame converts a GeometriesInFrame message as specified in common.proto to a GeometriesInFrame struct.
func ProtobufToGeometriesInFrame(proto *commonpb.GeometriesInFrame) (*GeometriesInFrame, error) {
geometries := make(map[string]spatialmath.Geometry)
for i, geometry := range proto.GetGeometries() {
g, err := spatialmath.NewGeometryFromProto(geometry)
if err != nil {
return nil, err
}
geometries[strconv.Itoa(i)] = g
}
return &GeometriesInFrame{
frame: proto.GetReferenceFrame(),
geometries: geometries,
}, nil
}