-
Notifications
You must be signed in to change notification settings - Fork 110
/
motion_configuration.go
85 lines (77 loc) · 2.89 KB
/
motion_configuration.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
package motion
import (
"math"
pb "go.viam.com/api/service/motion/v1"
"go.viam.com/rdk/protoutils"
)
func configurationFromProto(motionCfg *pb.MotionConfiguration) *MotionConfiguration {
obstacleDetectors := []ObstacleDetectorName{}
planDeviationM := 0.
positionPollingHz := 0.
obstaclePollingHz := 0.
linearMPerSec := 0.
angularDegsPerSec := 0.
if motionCfg != nil {
if motionCfg.ObstacleDetectors != nil {
for _, obstacleDetectorPair := range motionCfg.GetObstacleDetectors() {
obstacleDetectors = append(obstacleDetectors, ObstacleDetectorName{
VisionServiceName: protoutils.ResourceNameFromProto(obstacleDetectorPair.VisionService),
CameraName: protoutils.ResourceNameFromProto(obstacleDetectorPair.Camera),
})
}
}
if motionCfg.PositionPollingFrequencyHz != nil {
positionPollingHz = motionCfg.GetPositionPollingFrequencyHz()
}
if motionCfg.ObstaclePollingFrequencyHz != nil {
obstaclePollingHz = motionCfg.GetObstaclePollingFrequencyHz()
}
if motionCfg.PlanDeviationM != nil {
planDeviationM = motionCfg.GetPlanDeviationM()
}
if motionCfg.LinearMPerSec != nil {
linearMPerSec = motionCfg.GetLinearMPerSec()
}
if motionCfg.AngularDegsPerSec != nil {
angularDegsPerSec = motionCfg.GetAngularDegsPerSec()
}
}
return &MotionConfiguration{
ObstacleDetectors: obstacleDetectors,
PositionPollingFreqHz: positionPollingHz,
ObstaclePollingFreqHz: obstaclePollingHz,
PlanDeviationMM: 1e3 * planDeviationM,
LinearMPerSec: linearMPerSec,
AngularDegsPerSec: angularDegsPerSec,
}
}
func (motionCfg MotionConfiguration) toProto() *pb.MotionConfiguration {
proto := &pb.MotionConfiguration{}
if !math.IsNaN(motionCfg.LinearMPerSec) && motionCfg.LinearMPerSec != 0 {
proto.LinearMPerSec = &motionCfg.LinearMPerSec
}
if !math.IsNaN(motionCfg.AngularDegsPerSec) && motionCfg.AngularDegsPerSec != 0 {
proto.AngularDegsPerSec = &motionCfg.AngularDegsPerSec
}
if !math.IsNaN(motionCfg.ObstaclePollingFreqHz) && motionCfg.ObstaclePollingFreqHz > 0 {
proto.ObstaclePollingFrequencyHz = &motionCfg.ObstaclePollingFreqHz
}
if !math.IsNaN(motionCfg.PositionPollingFreqHz) && motionCfg.PositionPollingFreqHz > 0 {
proto.PositionPollingFrequencyHz = &motionCfg.PositionPollingFreqHz
}
if !math.IsNaN(motionCfg.PlanDeviationMM) && motionCfg.PlanDeviationMM >= 0 {
planDeviationM := 1e-3 * motionCfg.PlanDeviationMM
proto.PlanDeviationM = &planDeviationM
}
if len(motionCfg.ObstacleDetectors) > 0 {
pbObstacleDetector := []*pb.ObstacleDetector{}
for _, obstacleDetectorPair := range motionCfg.ObstacleDetectors {
pbObstacleDetector = append(pbObstacleDetector, &pb.ObstacleDetector{
VisionService: protoutils.ResourceNameToProto(obstacleDetectorPair.VisionServiceName),
Camera: protoutils.ResourceNameToProto(obstacleDetectorPair.CameraName),
})
}
proto.ObstacleDetectors = pbObstacleDetector
}
return proto
}