-
Notifications
You must be signed in to change notification settings - Fork 124
/
bcassume.go
91 lines (85 loc) · 1.85 KB
/
bcassume.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
package sdkapp
import (
"log"
"time"
"github.com/fforchino/vector-go-sdk/pkg/vectorpb"
)
func assumeBehaviorControl(robot Robot, robotIndex int, priority string) {
var controlRequest *vectorpb.BehaviorControlRequest
if priority == "high" {
controlRequest = &vectorpb.BehaviorControlRequest{
RequestType: &vectorpb.BehaviorControlRequest_ControlRequest{
ControlRequest: &vectorpb.ControlRequest{
Priority: vectorpb.ControlRequest_OVERRIDE_BEHAVIORS,
},
},
}
} else {
controlRequest = &vectorpb.BehaviorControlRequest{
RequestType: &vectorpb.BehaviorControlRequest_ControlRequest{
ControlRequest: &vectorpb.ControlRequest{
Priority: vectorpb.ControlRequest_DEFAULT,
},
},
}
}
go func() {
start := make(chan bool)
stop := make(chan bool)
robots[robotIndex].BcAssumption = true
go func() {
// * begin - modified from official vector-go-sdk
r, err := robot.Vector.Conn.BehaviorControl(
robot.Ctx,
)
if err != nil {
log.Println(err)
return
}
if err := r.Send(controlRequest); err != nil {
log.Println(err)
return
}
for {
ctrlresp, err := r.Recv()
if err != nil {
log.Println(err)
return
}
if ctrlresp.GetControlGrantedResponse() != nil {
start <- true
break
}
}
for {
select {
case <-stop:
if err := r.Send(
&vectorpb.BehaviorControlRequest{
RequestType: &vectorpb.BehaviorControlRequest_ControlRelease{
ControlRelease: &vectorpb.ControlRelease{},
},
},
); err != nil {
log.Println(err)
return
}
return
default:
continue
}
}
// * end - modified from official vector-go-sdk
}()
for range start {
for {
if robots[robotIndex].BcAssumption {
time.Sleep(time.Millisecond * 500)
} else {
break
}
}
stop <- true
}
}()
}