-
Notifications
You must be signed in to change notification settings - Fork 109
/
localizer.go
85 lines (74 loc) · 2.83 KB
/
localizer.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 (
"context"
"math"
geo "github.com/kellydunn/golang-geo"
"github.com/pkg/errors"
"go.viam.com/rdk/components/movementsensor"
"go.viam.com/rdk/referenceframe"
"go.viam.com/rdk/services/slam"
"go.viam.com/rdk/spatialmath"
)
// Localizer is an interface which both slam and movementsensor can satisfy when wrapped respectively.
type Localizer interface {
CurrentPosition(context.Context) (*referenceframe.PoseInFrame, error)
}
// slamLocalizer is a struct which only wraps an existing slam service.
type slamLocalizer struct {
slam.Service
}
// NewSLAMLocalizer creates a new Localizer that relies on a slam service to report Pose.
func NewSLAMLocalizer(slam slam.Service) Localizer {
return &slamLocalizer{Service: slam}
}
// CurrentPosition returns slam's current position.
func (s *slamLocalizer) CurrentPosition(ctx context.Context) (*referenceframe.PoseInFrame, error) {
pose, _, err := s.Position(ctx)
if err != nil {
return nil, err
}
return referenceframe.NewPoseInFrame(referenceframe.World, pose), err
}
// movementSensorLocalizer is a struct which only wraps an existing movementsensor.
type movementSensorLocalizer struct {
movementsensor.MovementSensor
origin *geo.Point
calibration spatialmath.Pose
}
// NewMovementSensorLocalizer creates a Localizer from a MovementSensor.
// An origin point must be specified and the localizer will return Poses relative to this point.
// A calibration pose can also be specified, which will adjust the location after it is calculated relative to the origin.
func NewMovementSensorLocalizer(ms movementsensor.MovementSensor, origin *geo.Point, calibration spatialmath.Pose) Localizer {
return &movementSensorLocalizer{MovementSensor: ms, origin: origin, calibration: calibration}
}
// CurrentPosition returns a movementsensor's current position.
func (m *movementSensorLocalizer) CurrentPosition(ctx context.Context) (*referenceframe.PoseInFrame, error) {
gp, _, err := m.Position(ctx, nil)
if err != nil {
return nil, err
}
var o spatialmath.Orientation
properties, err := m.Properties(ctx, nil)
if err != nil {
return nil, err
}
switch {
case properties.CompassHeadingSupported:
heading, err := m.CompassHeading(ctx, nil)
if err != nil {
return nil, err
}
o = &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: heading}
case properties.OrientationSupported:
o, err = m.Orientation(ctx, nil)
if err != nil {
return nil, err
}
default:
return nil, errors.New("could not get orientation from Localizer")
}
pose := spatialmath.NewPose(spatialmath.GeoPointToPose(gp, m.origin).Point(), o)
alignEast := spatialmath.NewPoseFromOrientation(&spatialmath.OrientationVector{OZ: 1, Theta: -math.Pi / 2})
correction := spatialmath.Compose(m.calibration, alignEast)
return referenceframe.NewPoseInFrame(m.Name().Name, spatialmath.Compose(pose, correction)), nil
}