-
Notifications
You must be signed in to change notification settings - Fork 110
/
replanner.go
64 lines (54 loc) · 1.91 KB
/
replanner.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
package builtin
import (
"context"
"fmt"
"sync/atomic"
"time"
"go.viam.com/rdk/motionplan"
"go.viam.com/rdk/services/motion/builtin/state"
)
// replanResponse is the struct returned by the replanner.
type replanResponse struct {
err error
executeResponse state.ExecuteResponse
}
// replanFn is an alias for a function that will be polled by a replanner.
type replanFn func(context.Context, motionplan.Plan, int) (state.ExecuteResponse, error)
func (rr replanResponse) String() string {
return fmt.Sprintf("builtin.replanResponse{executeResponse: %#v, err: %v}", rr.executeResponse, rr.err)
}
// replanner bundles everything needed to execute a function at a given interval and return.
type replanner struct {
period time.Duration
responseChan chan replanResponse
// needReplan is a function that returns a bool describing if a replan is needed, as well as an error
needReplan replanFn
}
// newReplanner is a constructor for a replanner.
func newReplanner(period time.Duration, fnToPoll replanFn) *replanner {
return &replanner{
period: period,
needReplan: fnToPoll,
responseChan: make(chan replanResponse, 1),
}
}
// startPolling executes the replanner's configured function at its configured period
// The caller of this function should read from the replanner's responseChan to know when a replan is requested.
func (r *replanner) startPolling(ctx context.Context, plan motionplan.Plan, waypointIndex *atomic.Int32) {
ticker := time.NewTicker(r.period)
defer ticker.Stop()
// this check ensures that if the context is cancelled we always return early at the top of the loop
for ctx.Err() == nil {
select {
case <-ctx.Done():
return
case <-ticker.C:
executeResp, err := r.needReplan(ctx, plan, int(waypointIndex.Load()))
if err != nil || executeResp.Replan {
res := replanResponse{executeResponse: executeResp, err: err}
r.responseChan <- res
return
}
}
}
}