-
Notifications
You must be signed in to change notification settings - Fork 110
/
explore.go
46 lines (36 loc) · 1.22 KB
/
explore.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
package builtin
import (
"context"
"math"
"math/rand"
"github.com/golang/geo/r3"
"go.viam.com/utils"
"go.viam.com/rdk/referenceframe"
"go.viam.com/rdk/spatialmath"
)
const defaultDistanceMM = 50 * 1000
func (svc *builtIn) startExploreMode(ctx context.Context) {
svc.logger.CDebug(ctx, "startExploreMode called")
svc.activeBackgroundWorkers.Add(1)
utils.ManagedGo(func() {
// Send motionCfg parameters through extra until motionCfg can be added to Move()
extra := map[string]interface{}{"motionCfg": *svc.motionCfg}
for {
if ctx.Err() != nil {
return
}
// Choose a new random point using a normal distribution centered on the position directly the robot
randAngle := rand.NormFloat64() + math.Pi
destination := referenceframe.NewPoseInFrame(svc.base.Name().Name, spatialmath.NewPose(
r3.Vector{
X: math.Sin(randAngle),
Y: math.Cos(randAngle),
Z: 0.,
}.Normalize().Mul(defaultDistanceMM), spatialmath.NewOrientationVector()))
_, err := svc.exploreMotionService.Move(ctx, svc.base.Name(), destination, nil, nil, extra)
if err != nil {
svc.logger.CDebugf(ctx, "error occurred when moving to point %v: %v", destination, err)
}
}
}, svc.activeBackgroundWorkers.Done)
}