Fix PR comments

sensors/movementsensor.go

@@ -18,9 +18,9 @@ import (
 )
 
 const (
-	replayTimeToleranceMsec           = 10 // Milliseconds
-	replayTimestampErrorMessage       = "replay sensor timestamp parse RFC3339Nano error"
-	timedMovementSensorReadingTimeout = 5 * time.Second
+	movementSensorReadingTimeToleranceMsec = 50 // Milliseconds
+	replayTimestampErrorMessage            = "replay sensor timestamp parse RFC3339Nano error"
+	timedMovementSensorReadingTimeout      = 5 * time.Second
 )
 
 var (
@@ -112,7 +112,7 @@ func (ms *MovementSensor) TimedMovementSensorReading(ctx context.Context) (Timed
 		for {
 			select {
 			case <-timeoutCtx.Done():
-				return TimedMovementSensorReadingResponse{}, timeoutCtx.Err()
+				return TimedMovementSensorReadingResponse{}, errors.Wrap(timeoutCtx.Err(), "timed out getting IMU data")
 			default:
 				if timedIMUReadingResponse, err = ms.timedIMUReading(timeoutCtx, &angVel, &linAcc,
 					&readingTimeAngularVel, &readingTimeLinearAcc); err != nil && !errors.Is(err, ErrNoValidReadingObtained) {
@@ -129,7 +129,7 @@ func (ms *MovementSensor) TimedMovementSensorReading(ctx context.Context) (Timed
 		for {
 			select {
 			case <-timeoutCtx.Done():
-				return TimedMovementSensorReadingResponse{}, timeoutCtx.Err()
+				return TimedMovementSensorReadingResponse{}, errors.Wrap(timeoutCtx.Err(), "timed out getting odometer data")
 			default:
 				if timedOdometerReadingResponse, err = ms.timedOdometerReading(timeoutCtx, position, &orientation,
 					&readingTimePosition, &readingTimeOrientation); err != nil && !errors.Is(err, ErrNoValidReadingObtained) {
@@ -156,15 +156,15 @@ func (ms *MovementSensor) timedIMUReading(ctx context.Context, angVel *spatialma
 	returnReadingIfTimestampsWithinTolerance := func(readingTimeAngularVel, readingTimeLinearAcc time.Time,
 		angVel *spatialmath.AngularVelocity, linAcc *r3.Vector,
 	) (TimedIMUReadingResponse, bool) {
-		if readingTimeAngularVel.Sub(readingTimeLinearAcc).Abs().Milliseconds() < replayTimeToleranceMsec {
+		if readingTimeAngularVel.Sub(readingTimeLinearAcc).Abs().Milliseconds() < movementSensorReadingTimeToleranceMsec {
 			return TimedIMUReadingResponse{
 				LinearAcceleration: *linAcc,
 				AngularVelocity: spatialmath.AngularVelocity{
 					X: rdkutils.DegToRad(angVel.X),
 					Y: rdkutils.DegToRad(angVel.Y),
 					Z: rdkutils.DegToRad(angVel.Z),
 				},
-				ReadingTime: readingTimeLinearAcc.Add((readingTimeLinearAcc.Sub(readingTimeAngularVel)).Abs() / 2),
+				ReadingTime: averageReadingTimes(readingTimeLinearAcc, readingTimeAngularVel),
 			}, true
 		}
 		return TimedIMUReadingResponse{}, false
@@ -173,7 +173,7 @@ func (ms *MovementSensor) timedIMUReading(ctx context.Context, angVel *spatialma
 	if *readingTimeLinearAcc == defaultTime || readingTimeLinearAcc.Sub(*readingTimeAngularVel).Milliseconds() < 0 {
 		ctxWithMetadata, md := contextutils.ContextWithMetadata(ctx)
 		if *linAcc, err = ms.sensor.LinearAcceleration(ctxWithMetadata, make(map[string]interface{})); err != nil {
-			return &TimedIMUReadingResponse{}, errors.Wrap(err, "LinearAcceleration error")
+			return &TimedIMUReadingResponse{}, errors.Wrap(err, "could not obtain LinearAcceleration")
 		}
 
 		if timeRequestedMetadata, ok := md[contextutils.TimeRequestedMetadataKey]; ok {
@@ -193,7 +193,7 @@ func (ms *MovementSensor) timedIMUReading(ctx context.Context, angVel *spatialma
 	if *readingTimeAngularVel == defaultTime || readingTimeAngularVel.Sub(*readingTimeLinearAcc).Milliseconds() < 0 {
 		ctxWithMetadata, md := contextutils.ContextWithMetadata(ctx)
 		if *angVel, err = ms.sensor.AngularVelocity(ctxWithMetadata, make(map[string]interface{})); err != nil {
-			return &TimedIMUReadingResponse{}, errors.Wrap(err, "AngularVelocity error")
+			return &TimedIMUReadingResponse{}, errors.Wrap(err, "could not obtain AngularVelocity")
 		}
 
 		if timeRequestedMetadata, ok := md[contextutils.TimeRequestedMetadataKey]; ok {
@@ -221,11 +221,11 @@ func (ms *MovementSensor) timedOdometerReading(ctx context.Context, position *ge
 	returnReadingIfTimestampsWithinTolerance := func(readingTimePosition, readingTimeOrientation time.Time,
 		position *geo.Point, orientation *spatialmath.Orientation,
 	) (TimedOdometerReadingResponse, bool) {
-		if readingTimeOrientation.Sub(readingTimePosition).Abs().Milliseconds() < replayTimeToleranceMsec {
+		if readingTimeOrientation.Sub(readingTimePosition).Abs().Milliseconds() < movementSensorReadingTimeToleranceMsec {
 			return TimedOdometerReadingResponse{
 				Position:    position,
 				Orientation: *orientation,
-				ReadingTime: readingTimePosition.Add((readingTimePosition.Sub(readingTimeOrientation) / 2).Abs()),
+				ReadingTime: averageReadingTimes(readingTimePosition, readingTimeOrientation),
 			}, true
 		}
 		return TimedOdometerReadingResponse{}, false
@@ -234,7 +234,7 @@ func (ms *MovementSensor) timedOdometerReading(ctx context.Context, position *ge
 	if *readingTimePosition == defaultTime || readingTimePosition.Sub(*readingTimeOrientation).Milliseconds() < 0 {
 		ctxWithMetadata, md := contextutils.ContextWithMetadata(ctx)
 		if position, _, err = ms.sensor.Position(ctxWithMetadata, make(map[string]interface{})); err != nil {
-			return &TimedOdometerReadingResponse{}, errors.Wrap(err, "Position error")
+			return &TimedOdometerReadingResponse{}, errors.Wrap(err, "could not obtain Position")
 		}
 
 		if timeRequestedMetadata, ok := md[contextutils.TimeRequestedMetadataKey]; ok {
@@ -254,7 +254,7 @@ func (ms *MovementSensor) timedOdometerReading(ctx context.Context, position *ge
 	if *readingTimeOrientation == defaultTime || readingTimeOrientation.Sub(*readingTimePosition).Milliseconds() < 0 {
 		ctxWithMetadata, md := contextutils.ContextWithMetadata(ctx)
 		if *orientation, err = ms.sensor.Orientation(ctxWithMetadata, make(map[string]interface{})); err != nil {
-			return &TimedOdometerReadingResponse{}, errors.Wrap(err, "Orientation error")
+			return &TimedOdometerReadingResponse{}, errors.Wrap(err, "could not obtain Orientation")
 		}
 
 		if timeRequestedMetadata, ok := md[contextutils.TimeRequestedMetadataKey]; ok {
@@ -322,3 +322,13 @@ func NewMovementSensor(
 		sensor:            movementSensor,
 	}, nil
 }
+
+func averageReadingTimes(a, b time.Time) time.Time {
+	if b.Equal(a) {
+		return a
+	} else if b.After(a) {
+		return a.Add(b.Sub(a) / 2)
+	} else {
+		return b.Add(a.Sub(b) / 2)
+	}
+}

sensors/movementsensor_internal_test.go

@@ -0,0 +1,28 @@
+package sensors
+
+import (
+	"testing"
+	"time"
+
+	"go.viam.com/test"
+)
+
+func TestAverageReadingTimes(t *testing.T) {
+	t.Run("a equals b", func(t *testing.T) {
+		a := time.Now()
+		b := a
+		test.That(t, a, test.ShouldEqual, averageReadingTimes(a, b))
+	})
+
+	t.Run("b 10 msec larger than a", func(t *testing.T) {
+		a := time.Now()
+		b := a.Add(10 * time.Millisecond)
+		test.That(t, averageReadingTimes(a, b), test.ShouldEqual, a.Add(5*time.Millisecond))
+	})
+
+	t.Run("a 66 msec larger than b", func(t *testing.T) {
+		b := time.Now()
+		a := b.Add(66 * time.Millisecond)
+		test.That(t, averageReadingTimes(a, b), test.ShouldEqual, b.Add(33*time.Millisecond))
+	})
+}

viam_cartographer.go

@@ -15,7 +15,6 @@ import (
 	"github.com/pkg/errors"
 	"go.opencensus.io/trace"
 	"go.uber.org/zap/zapcore"
-	"go.viam.com/rdk/components/movementsensor"
 	viamgrpc "go.viam.com/rdk/grpc"
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/services/slam"
@@ -210,14 +209,9 @@ func New(
 	var timedMovementSensor s.TimedMovementSensor
 	if movementSensorName == "" {
 		logger.Info("no movement sensor configured, proceeding without IMU and without odometer")
-	} else {
-		if err = checkIfIMUAndOdometerSupported(ctx, deps, movementSensorName); err != nil {
-			return nil, err
-		}
-		if timedMovementSensor, err = s.NewMovementSensor(ctx, deps, movementSensorName,
-			optionalConfigParams.MovementSensorDataFrequencyHz, logger); err != nil {
-			return nil, err
-		}
+	} else if timedMovementSensor, err = s.NewMovementSensor(ctx, deps, movementSensorName,
+		optionalConfigParams.MovementSensorDataFrequencyHz, logger); err != nil {
+		return nil, err
 	}
 
 	// Need to be able to shut down the sensor process before the cartoFacade
@@ -268,32 +262,6 @@ func New(
 	return cartoSvc, nil
 }
 
-func checkIfIMUAndOdometerSupported(ctx context.Context, deps resource.Dependencies, movementSensorName string) error {
-	movementSensor, err := movementsensor.FromDependencies(deps, movementSensorName)
-	if err != nil {
-		return errors.Wrapf(err, "error getting movement sensor \"%v\" for slam service", movementSensorName)
-	}
-
-	properties, err := movementSensor.Properties(ctx, make(map[string]interface{}))
-	if err != nil {
-		errMessage := "error getting movement sensor properties from movement sensor \"" + movementSensorName +
-			"\" for slam service"
-		return errors.Wrap(err, errMessage)
-	}
-
-	// A movement sensor used as an IMU must support LinearAcceleration and AngularVelocity.
-	imuSupported := properties.LinearAccelerationSupported && properties.AngularVelocitySupported
-
-	// A movement sensor used as an odometer must support Position and Orientation.
-	odometerSupported := properties.PositionSupported && properties.OrientationSupported
-
-	if !imuSupported && !odometerSupported {
-		return s.ErrMovementSensorNeitherIMUNorOdometer
-	}
-
-	return nil
-}
-
 func parseCartoAlgoConfig(configParams map[string]string, logger golog.Logger) (cartofacade.CartoAlgoConfig, error) {
 	cartoAlgoCfg := defaultCartoAlgoCfg
 	for k, val := range configParams {

viam_cartographer_internal_test.go

@@ -18,7 +18,6 @@ import (
 	"go.viam.com/utils/artifact"
 
 	"github.com/viamrobotics/viam-cartographer/cartofacade"
-	s "github.com/viamrobotics/viam-cartographer/sensors"
 	"github.com/viamrobotics/viam-cartographer/sensors/inject"
 )
 
@@ -490,34 +489,3 @@ func TestBuiltinQuaternion(t *testing.T) {
 		test.That(t, componentRef, test.ShouldBeEmpty)
 	})
 }
-
-func TestCheckIfIMUAndOdometerSupported(t *testing.T) {
-	ctx := context.Background()
-	t.Run("neither IMU nor Odometer supported", func(t *testing.T) {
-		lidar, movementSensor := s.NoLidar, s.MovementSensorNotIMUNotOdometer
-		err := checkIfIMUAndOdometerSupported(ctx,
-			s.SetupDeps(lidar, movementSensor), string(movementSensor))
-		test.That(t, err, test.ShouldNotBeNil)
-		test.That(t, err, test.ShouldBeError, s.ErrMovementSensorNeitherIMUNorOdometer)
-	})
-
-	t.Run("failure to get movement sensor from dependencies", func(t *testing.T) {
-		lidar, movementSensor := s.NoLidar, s.GibberishMovementSensor
-		errMessage := "error getting movement sensor \"" + string(movementSensor) + "\" for slam service: \"" +
-			"rdk:component:movement_sensor/" + string(movementSensor) + "\" missing from dependencies"
-		err := checkIfIMUAndOdometerSupported(ctx,
-			s.SetupDeps(lidar, movementSensor), string(movementSensor))
-		test.That(t, err, test.ShouldNotBeNil)
-		test.That(t, err, test.ShouldBeError, errors.New(errMessage))
-	})
-
-	t.Run("failure to get movement sensor properties", func(t *testing.T) {
-		lidar, movementSensor := s.NoLidar, s.MovementSensorWithErroringPropertiesFunc
-		errMessage := "error getting movement sensor properties from movement sensor \"" + string(movementSensor) +
-			"\" for slam service: error getting properties"
-		err := checkIfIMUAndOdometerSupported(ctx,
-			s.SetupDeps(lidar, movementSensor), string(movementSensor))
-		test.That(t, err, test.ShouldNotBeNil)
-		test.That(t, err, test.ShouldBeError, errors.New(errMessage))
-	})
-}