From 4c3e62f76f4425cef58c3a2b023c33d580e73bdb Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Mon, 11 Dec 2023 10:06:16 -0500 Subject: [PATCH 01/21] Change defaultTime to undefinedTime --- sensors/movementsensor.go | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/sensors/movementsensor.go b/sensors/movementsensor.go index 6230b4e1..cb6c6791 100644 --- a/sensors/movementsensor.go +++ b/sensors/movementsensor.go @@ -24,7 +24,7 @@ const ( ) var ( - defaultTime = time.Time{} + undefinedTime = time.Time{} // ErrMovementSensorNeitherIMUNorOdometer denotes that the provided movement sensor does neither support // an IMU nor a movement sensor. ErrMovementSensorNeitherIMUNorOdometer = errors.New("'movement_sensor' must either support both LinearAcceleration and " + @@ -170,7 +170,7 @@ func (ms *MovementSensor) timedIMUReading(ctx context.Context, angVel *spatialma return TimedIMUReadingResponse{}, false } - if *readingTimeLinearAcc == defaultTime || readingTimeLinearAcc.Sub(*readingTimeAngularVel).Milliseconds() < 0 { + if *readingTimeLinearAcc == undefinedTime || 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, "could not obtain LinearAcceleration") @@ -190,7 +190,7 @@ func (ms *MovementSensor) timedIMUReading(ctx context.Context, angVel *spatialma return &response, nil } - if *readingTimeAngularVel == defaultTime || readingTimeAngularVel.Sub(*readingTimeLinearAcc).Milliseconds() < 0 { + if *readingTimeAngularVel == undefinedTime || 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, "could not obtain AngularVelocity") @@ -231,7 +231,7 @@ func (ms *MovementSensor) timedOdometerReading(ctx context.Context, position *ge return TimedOdometerReadingResponse{}, false } - if *readingTimePosition == defaultTime || readingTimePosition.Sub(*readingTimeOrientation).Milliseconds() < 0 { + if *readingTimePosition == undefinedTime || 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, "could not obtain Position") @@ -251,7 +251,7 @@ func (ms *MovementSensor) timedOdometerReading(ctx context.Context, position *ge return &response, nil } - if *readingTimeOrientation == defaultTime || readingTimeOrientation.Sub(*readingTimePosition).Milliseconds() < 0 { + if *readingTimeOrientation == undefinedTime || 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, "could not obtain Orientation") From 03243315bf41aa307af0fe2e96ce8029c9d2c519 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Mon, 11 Dec 2023 10:06:32 -0500 Subject: [PATCH 02/21] Change defaultTime to undefinedTime --- testhelper/integrationtesthelper.go | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/testhelper/integrationtesthelper.go b/testhelper/integrationtesthelper.go index 5ad80e1d..5038f085 100644 --- a/testhelper/integrationtesthelper.go +++ b/testhelper/integrationtesthelper.go @@ -51,7 +51,7 @@ const ( testTimeout = 20 * time.Second ) -var defaultTime = time.Time{} +var undefinedTime = time.Time{} // Test final position and orientation are at approximately the expected values. func testCartographerPosition(t *testing.T, svc slam.Service, useIMU bool, expectedComponentRef string) { @@ -197,7 +197,7 @@ func integrationTimedLidar( */ for { timeTracker.mu.Lock() - if timeTracker.imuTime == defaultTime { + if timeTracker.imuTime == undefinedTime { time.Sleep(sensorDataIngestionWaitTime) break } @@ -212,7 +212,7 @@ func integrationTimedLidar( // Communicate that all lidar readings have been sent to cartographer or if the last IMU reading has been sent, // checks if LastLidarTime has been defined. If so, simulate endOfDataSet error. t.Logf("TimedLidarReading Mock i: %d, closed: %v, readingTime: %s\n", i, closed, timeTracker.lidarTime.String()) - if i >= NumPointClouds || timeTracker.finalImuTime != defaultTime { + if i >= NumPointClouds || timeTracker.finalImuTime != undefinedTime { // Sends a signal to the integration sensor's done channel the first time end of dataset has been sent if !closed { done <- struct{}{} @@ -414,7 +414,7 @@ func integrationTimedIMU( // Communicate that all desired IMU readings have been sent or to cartographer or if the last lidar reading // has been sent by, checks if lastLidarTime has been defined. If so, simulate endOfDataSet error. t.Logf("TimedMovementSensorReading Mock i: %d, closed: %v, readingTime: %s\n", i, closed, timeTracker.imuTime.String()) - if int(i) >= len(mockDataset) || timeTracker.finalLidarTime != defaultTime { + if int(i) >= len(mockDataset) || timeTracker.finalLidarTime != undefinedTime { // Sends a signal to the integration sensor's done channel the first time end of dataset has been sent if !closed { done <- struct{}{} From 6c4df549b8427036f530ead8d84b08f39b91b926 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Mon, 11 Dec 2023 10:13:40 -0500 Subject: [PATCH 03/21] Add odometer to sensor process --- sensorprocess/movementsensorprocess.go | 104 +++++++++++++++++------- sensorprocess/sensorprocess.go | 105 +++++++++++++++++++----- sensorprocess/sensorprocess_test.go | 90 ++++++++++++--------- sensorprocess/testhelper.go | 15 ++-- viam_cartographer.go | 106 +++++++++++++++---------- 5 files changed, 282 insertions(+), 138 deletions(-) diff --git a/sensorprocess/movementsensorprocess.go b/sensorprocess/movementsensorprocess.go index ee42e619..e8ecfa83 100644 --- a/sensorprocess/movementsensorprocess.go +++ b/sensorprocess/movementsensorprocess.go @@ -13,81 +13,116 @@ import ( s "github.com/viamrobotics/viam-cartographer/sensors" ) -// StartIMU polls the IMU to get the next sensor reading and adds it to the cartofacade. -// Stops when the context is Done. -func (config *Config) StartIMU(ctx context.Context) { +// StartMovementSensor polls the movement sensor to get the next sensor reading +// and adds it to the cartofacade. Stops when the context is Done. +func (config *Config) StartMovementSensor(ctx context.Context) { for { select { case <-ctx.Done(): return default: - if err := config.addIMUReadingInOnline(ctx); err != nil { + if err := config.addMovementSensorReadingInOnline(ctx); err != nil { config.Logger.Warn(err) } } } } -// addIMUReadingInOnline ensures the most recent IMU scan, after corresponding lidar scans, gets processed by cartographer. -func (config *Config) addIMUReadingInOnline(ctx context.Context) error { - // get next IMU data response; ignoring status since it is always false - imuReading, err := config.IMU.TimedMovementSensorReading(ctx) +// addMovementSensorReadingInOnline ensures the most recent movement sensor scan, +// after corresponding lidar scans, gets processed by cartographer. +func (config *Config) addMovementSensorReadingInOnline(ctx context.Context) error { + // get next movement sensor data response; ignoring status since it is always false + movementSensorReading, err := config.MovementSensor.TimedMovementSensorReading(ctx) if err != nil { - config.Logger.Warn(err) if errors.Is(err, replaymovementsensor.ErrEndOfDataset) { time.Sleep(1 * time.Second) } return err } - // add IMU data to cartographer and sleep remainder of time interval - timeToSleep := config.tryAddIMUReadingOnce(ctx, *imuReading.TimedIMUResponse) + // add movement sensor data to cartographer and sleep remainder of time interval + timeToSleep := config.tryAddMovementSensorReadingOnce(ctx, movementSensorReading) - if !imuReading.TestIsReplaySensor { + if !movementSensorReading.TestIsReplaySensor { time.Sleep(time.Duration(timeToSleep) * time.Millisecond) - config.Logger.Debugf("imu sleep for %vms", timeToSleep) + config.Logger.Debugf("movement sensor sleep for %vms", timeToSleep) } return nil } -// tryAddIMUReadingUntilSuccess adds a reading to the cartofacade and retries on error (offline mode). +// tryAddMovementSensorReadingUntilSuccess adds a reading to the cartofacade and retries on error (offline mode). // While add sensor reading fails, keep trying to add the same reading - in offline mode we want to // process each reading so if we cannot acquire the lock we should try again. -func (config *Config) tryAddIMUReadingUntilSuccess(ctx context.Context, reading s.TimedIMUReadingResponse) error { +func (config *Config) tryAddMovementSensorReadingUntilSuccess(ctx context.Context, reading s.TimedMovementSensorReadingResponse) error { + var imuDone, odometerDone bool + if !config.MovementSensor.Properties().IMUSupported { + imuDone = true + } + if !config.MovementSensor.Properties().OdometerSupported { + odometerDone = true + } for { select { case <-ctx.Done(): return ctx.Err() default: - if err := config.tryAddIMUReading(ctx, reading); err != nil { - if !errors.Is(err, cartofacade.ErrUnableToAcquireLock) { - config.Logger.Warnw("Retrying sensor reading due to error from cartofacade", "error", err) + if !imuDone { + if err := config.tryAddIMUReading(ctx, *reading.TimedIMUResponse); err != nil { + if !errors.Is(err, cartofacade.ErrUnableToAcquireLock) { + config.Logger.Warnw("Retrying IMU sensor reading due to error from cartofacade", "error", err) + } + } else { + imuDone = true } - } else { + } + if !odometerDone { + if err := config.tryAddOdometerReading(ctx, *reading.TimedOdometerResponse); err != nil { + if !errors.Is(err, cartofacade.ErrUnableToAcquireLock) { + config.Logger.Warnw("Retrying odometer sensor reading due to error from cartofacade", "error", err) + } + } else { + odometerDone = true + } + } + if imuDone && odometerDone { return nil } } } } -// tryAddIMUReadingOnce adds a reading to the carto facade and does not retry. Returns remainder of time interval. -func (config *Config) tryAddIMUReadingOnce(ctx context.Context, reading s.TimedIMUReadingResponse) int { +// tryAddMovementSensorReadingOnce adds a reading to the carto facade and does not retry. Returns remainder of time interval. +func (config *Config) tryAddMovementSensorReadingOnce(ctx context.Context, reading s.TimedMovementSensorReadingResponse) int { startTime := time.Now().UTC() - if err := config.tryAddIMUReading(ctx, reading); err != nil { - if errors.Is(err, cartofacade.ErrUnableToAcquireLock) { - config.Logger.Debugw("Skipping sensor reading due to lock contention in cartofacade", "error", err) - } else { - config.Logger.Warnw("Skipping sensor reading due to error from cartofacade", "error", err) + + if config.MovementSensor.Properties().IMUSupported { + if err := config.tryAddIMUReading(ctx, *reading.TimedIMUResponse); err != nil { + if errors.Is(err, cartofacade.ErrUnableToAcquireLock) { + config.Logger.Debugw("Skipping IMU sensor reading due to lock contention in cartofacade", "error", err) + } else { + config.Logger.Warnw("Skipping IMU sensor reading due to error from cartofacade", "error", err) + } } } + + if config.MovementSensor.Properties().OdometerSupported { + if err := config.tryAddOdometerReading(ctx, *reading.TimedOdometerResponse); err != nil { + if errors.Is(err, cartofacade.ErrUnableToAcquireLock) { + config.Logger.Debugw("Skipping odometer sensor reading due to lock contention in cartofacade", "error", err) + } else { + config.Logger.Warnw("Skipping odometer sensor reading due to error from cartofacade", "error", err) + } + } + } + timeElapsedMs := int(time.Since(startTime).Milliseconds()) - return int(math.Max(0, float64(1000/config.IMU.DataFrequencyHz()-timeElapsedMs))) + return int(math.Max(0, float64(1000/config.MovementSensor.DataFrequencyHz()-timeElapsedMs))) } -// tryAddIMUReading tries to add a reading to the carto facade. +// tryAddIMUReading tries to add an IMU reading to the carto facade. func (config *Config) tryAddIMUReading(ctx context.Context, reading s.TimedIMUReadingResponse) error { - err := config.CartoFacade.AddIMUReading(ctx, config.Timeout, config.IMU.Name(), reading) + err := config.CartoFacade.AddIMUReading(ctx, config.Timeout, config.MovementSensor.Name(), reading) if err != nil { config.Logger.Debugf("%v \t | IMU | Failure \t \t | %v \n", reading.ReadingTime, reading.ReadingTime.Unix()) } else { @@ -95,3 +130,14 @@ func (config *Config) tryAddIMUReading(ctx context.Context, reading s.TimedIMURe } return err } + +// tryAddOdometerReading tries to add an odometer reading to the carto facade. +func (config *Config) tryAddOdometerReading(ctx context.Context, reading s.TimedOdometerReadingResponse) error { + err := config.CartoFacade.AddOdometerReading(ctx, config.Timeout, config.MovementSensor.Name(), reading) + if err != nil { + config.Logger.Debugf("%v \t | Odometer | Failure \t \t | %v \n", reading.ReadingTime, reading.ReadingTime.Unix()) + } else { + config.Logger.Debugf("%v \t | Odometer | Success \t \t | %v \n", reading.ReadingTime, reading.ReadingTime.Unix()) + } + return err +} diff --git a/sensorprocess/sensorprocess.go b/sensorprocess/sensorprocess.go index 0a56a545..a3bc776d 100644 --- a/sensorprocess/sensorprocess.go +++ b/sensorprocess/sensorprocess.go @@ -3,6 +3,8 @@ package sensorprocess import ( "context" + "errors" + "sort" "strings" "time" @@ -19,16 +21,48 @@ type Config struct { CartoFacade cartofacade.Interface IsOnline bool - Lidar s.TimedLidar - IMU s.TimedMovementSensor + Lidar s.TimedLidar + MovementSensor s.TimedMovementSensor Timeout time.Duration InternalTimeout time.Duration Logger logging.Logger } +// getInitialMovementSensorReading gets the initial movement sensor reading. +// It discards all movement sensor readings that were recorded before the first lidar reading. +func (config *Config) getInitialMovementSensorReading(ctx context.Context, + lidarReading s.TimedLidarReadingResponse, +) (s.TimedMovementSensorReadingResponse, error) { + if config.MovementSensor == nil || (!config.MovementSensor.Properties().IMUSupported && + !config.MovementSensor.Properties().OdometerSupported) { + return s.TimedMovementSensorReadingResponse{}, errors.New("movement sensor is not supported") + } + for { + movementSensorReading, err := config.MovementSensor.TimedMovementSensorReading(ctx) + if err != nil { + return s.TimedMovementSensorReadingResponse{}, err + } + + var readingTime time.Time + if config.MovementSensor.Properties().IMUSupported { + // we can assume that the imu reading time is earlier than the odometer reading + // time, since the imu reading is taken before the odometer reading + readingTime = movementSensorReading.TimedIMUResponse.ReadingTime + } else { + // we reach this case if the imu is not supported + readingTime = movementSensorReading.TimedOdometerResponse.ReadingTime + } + + if !readingTime.Before(lidarReading.ReadingTime) { + return movementSensorReading, nil + } + } +} + // StartOfflineSensorProcess starts the process of adding lidar and movement sensor data -// in a deterministically defined order to cartographer. +// in a deterministically defined order to cartographer. Returns a bool that indicates +// whether or not the end of either the lidar or movement sensor datasets have been reached. func (config *Config) StartOfflineSensorProcess(ctx context.Context) bool { // get the initial lidar reading lidarReading, err := config.Lidar.TimedLidarReading(ctx) @@ -37,32 +71,60 @@ func (config *Config) StartOfflineSensorProcess(ctx context.Context) bool { return strings.Contains(err.Error(), replaypcd.ErrEndOfDataset.Error()) } - var imuReading s.TimedIMUReadingResponse - if config.IMU != nil { + var movementSensorReading s.TimedMovementSensorReadingResponse + if config.MovementSensor != nil && (config.MovementSensor.Properties().IMUSupported || + config.MovementSensor.Properties().OdometerSupported) { // get the initial IMU reading; discard all IMU readings that were recorded before the first lidar reading - for { - movementSensorReading, err := config.IMU.TimedMovementSensorReading(ctx) - if err != nil { - config.Logger.Warn(err) - return strings.Contains(err.Error(), replaymovementsensor.ErrEndOfDataset.Error()) - } - - imuReading = *movementSensorReading.TimedIMUResponse - if !imuReading.ReadingTime.Before(lidarReading.ReadingTime) { - break - } + movementSensorReading, err = config.getInitialMovementSensorReading(ctx, lidarReading) + if err != nil { + config.Logger.Warn(err) + return strings.Contains(err.Error(), replaymovementsensor.ErrEndOfDataset.Error()) } } + type readingTime struct { + sensorType string + readingTime time.Time + } + // loop over all the data until one of the datasets has reached its end for { select { case <-ctx.Done(): return false default: + // create a map of supported sensors and their reading time stamps + readingTimes := []readingTime{ + {sensorType: "lidar", readingTime: lidarReading.ReadingTime}, + } + if config.MovementSensor != nil && config.MovementSensor.Properties().IMUSupported { + readingTimes = append(readingTimes, + readingTime{ + sensorType: "movement-sensor", + readingTime: movementSensorReading.TimedIMUResponse.ReadingTime, + }) + } else if config.MovementSensor != nil && config.MovementSensor.Properties().OdometerSupported { + readingTimes = append(readingTimes, + readingTime{ + sensorType: "movement-sensor", + readingTime: movementSensorReading.TimedOdometerResponse.ReadingTime, + }) + } + + // sort the readings based on their time stamp + sort.Slice(readingTimes, + func(i, j int) bool { + // if the timestamps are the same, we want to prioritize the lidar measurement before + // the movement sensor measurement + if readingTimes[i].readingTime.Equal(readingTimes[j].readingTime) { + return readingTimes[i].sensorType == "lidar" + } + return readingTimes[i].readingTime.Before(readingTimes[j].readingTime) + }) + // insert the reading with the earliest time stamp - if config.IMU == nil || - !lidarReading.ReadingTime.After(imuReading.ReadingTime) { + switch readingTimes[0].sensorType { + case "lidar": if err := config.tryAddLidarReadingUntilSuccess(ctx, lidarReading); err != nil { return false } @@ -76,11 +138,11 @@ func (config *Config) StartOfflineSensorProcess(ctx context.Context) bool { } return lidarEndOfDataSetReached } - } else { - if err := config.tryAddIMUReadingUntilSuccess(ctx, imuReading); err != nil { + case "movement-sensor": + if err := config.tryAddMovementSensorReadingUntilSuccess(ctx, movementSensorReading); err != nil { return false } - movementSensorReading, err := config.IMU.TimedMovementSensorReading(ctx) + movementSensorReading, err = config.MovementSensor.TimedMovementSensorReading(ctx) if err != nil { config.Logger.Warn(err) msEndOfDataSetReached := strings.Contains(err.Error(), replaymovementsensor.ErrEndOfDataset.Error()) @@ -89,7 +151,6 @@ func (config *Config) StartOfflineSensorProcess(ctx context.Context) bool { } return msEndOfDataSetReached } - imuReading = *movementSensorReading.TimedIMUResponse } } } diff --git a/sensorprocess/sensorprocess_test.go b/sensorprocess/sensorprocess_test.go index eb7530a6..6f1234b3 100644 --- a/sensorprocess/sensorprocess_test.go +++ b/sensorprocess/sensorprocess_test.go @@ -145,9 +145,9 @@ func TestStartOfflineSensorProcess(t *testing.T) { now := time.Now().UTC() if tt.imuEnabled { - config.IMU = &injectMovementSensor + config.MovementSensor = &injectMovementSensor } else { - config.IMU = nil + config.MovementSensor = nil } numLidarData := 0 @@ -169,6 +169,11 @@ func TestStartOfflineSensorProcess(t *testing.T) { } return s.TimedMovementSensorReadingResponse{}, replay.ErrEndOfDataset } + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + } + } actualDataInsertions := []string{} @@ -392,26 +397,35 @@ func TestStartLidar(t *testing.T) { func TestTryAddIMUReading(t *testing.T) { logger := logging.NewTestLogger(t) cf := cartofacade.Mock{} - reading := s.TimedIMUReadingResponse{ + imuReading := s.TimedIMUReadingResponse{ LinearAcceleration: r3.Vector{X: 1, Y: 1, Z: 1}, AngularVelocity: spatialmath.AngularVelocity{X: 1, Y: 1, Z: 1}, ReadingTime: time.Now().UTC(), } + reading := s.TimedMovementSensorReadingResponse{ + TimedIMUResponse: &imuReading, + } + injectLidar := inject.TimedLidar{} injectLidar.DataFrequencyHzFunc = func() int { return 5 } injectImu := inject.TimedMovementSensor{} injectImu.NameFunc = func() string { return "good_imu" } injectImu.DataFrequencyHzFunc = func() int { return 20 } + injectImu.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + } + } config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, - IMU: &injectImu, - Timeout: 10 * time.Second, + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + MovementSensor: &injectImu, + Timeout: 10 * time.Second, } t.Run("when AddIMUReading blocks for more than the date rate and succeeds, time to sleep is 0", func(t *testing.T) { @@ -425,7 +439,7 @@ func TestTryAddIMUReading(t *testing.T) { return nil } - timeToSleep := config.tryAddIMUReadingOnce(context.Background(), reading) + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) test.That(t, timeToSleep, test.ShouldEqual, 0) }) @@ -440,7 +454,7 @@ func TestTryAddIMUReading(t *testing.T) { return cartofacade.ErrUnableToAcquireLock } - timeToSleep := config.tryAddIMUReadingOnce(context.Background(), reading) + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) test.That(t, timeToSleep, test.ShouldEqual, 0) }) @@ -455,7 +469,7 @@ func TestTryAddIMUReading(t *testing.T) { return errUnknown } - timeToSleep := config.tryAddIMUReadingOnce(context.Background(), reading) + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) test.That(t, timeToSleep, test.ShouldEqual, 0) }) @@ -469,9 +483,9 @@ func TestTryAddIMUReading(t *testing.T) { return nil } - timeToSleep := config.tryAddIMUReadingOnce(context.Background(), reading) + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.IMU.DataFrequencyHz()) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) }) t.Run("when AddIMUReading is faster than the date rate and returns a lock error, time to sleep is <= date rate", func(t *testing.T) { @@ -484,9 +498,9 @@ func TestTryAddIMUReading(t *testing.T) { return cartofacade.ErrUnableToAcquireLock } - timeToSleep := config.tryAddIMUReadingOnce(context.Background(), reading) + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.IMU.DataFrequencyHz()) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) }) t.Run("when AddIMUReading is faster than date rate "+ @@ -500,9 +514,9 @@ func TestTryAddIMUReading(t *testing.T) { return errUnknown } - timeToSleep := config.tryAddIMUReadingOnce(context.Background(), reading) + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.IMU.DataFrequencyHz()) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) }) } @@ -515,11 +529,11 @@ func TestAddIMUReadingInOnline(t *testing.T) { injectImu.DataFrequencyHzFunc = func() int { return 20 } config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: true, - IMU: &injectImu, - Timeout: 10 * time.Second, + Logger: logger, + CartoFacade: &cf, + IsOnline: true, + MovementSensor: &injectImu, + Timeout: 10 * time.Second, } t.Run("returns error when IMU GetData returns error, doesn't try to add IMU data", func(t *testing.T) { @@ -551,12 +565,12 @@ func TestStartIMU(t *testing.T) { injectImu.DataFrequencyHzFunc = func() int { return 20 } config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, - IMU: &injectImu, - Timeout: 10 * time.Second, + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + MovementSensor: &injectImu, + Timeout: 10 * time.Second, } t.Run("exits loop when the context was cancelled", func(t *testing.T) { @@ -566,11 +580,11 @@ func TestStartIMU(t *testing.T) { replaySensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(lidar, imu), string(imu), 20, logger) test.That(t, err, test.ShouldBeNil) - config.IMU = replaySensor + config.MovementSensor = replaySensor cancelFunc() - config.StartIMU(cancelCtx) + config.StartMovementSensor(cancelCtx) }) } @@ -652,11 +666,11 @@ func TestTryAddIMUReadingUntilSuccess(t *testing.T) { injectImu.DataFrequencyHzFunc = func() int { return dataFrequencyHz } config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: false, - IMU: &injectImu, - Timeout: 10 * time.Second, + Logger: logger, + CartoFacade: &cf, + IsOnline: false, + MovementSensor: &injectImu, + Timeout: 10 * time.Second, } t.Run("replay IMU adds sensor data until success", func(t *testing.T) { @@ -688,10 +702,10 @@ func TestTryAddIMUReadingUntilSuccess(t *testing.T) { } return nil } - config.IMU = replayIMU + config.MovementSensor = replayIMU config.Lidar = &injectLidar - config.tryAddIMUReadingUntilSuccess(ctx, expectedIMUReading) + config.tryAddMovementSensorReadingUntilSuccess(ctx, expectedMovementSensorReading) test.That(t, len(calls), test.ShouldEqual, 3) firstTimestamp := calls[0].currentReading.ReadingTime diff --git a/sensorprocess/testhelper.go b/sensorprocess/testhelper.go index 9fbdd3d0..8e99b23e 100644 --- a/sensorprocess/testhelper.go +++ b/sensorprocess/testhelper.go @@ -45,6 +45,9 @@ DATA binary LinearAcceleration: r3.Vector{X: 1, Y: 1, Z: 1}, AngularVelocity: spatialmath.AngularVelocity{X: 0.017453292519943295, Y: 0.008726646259971648, Z: 0}, } + expectedMovementSensorReading = s.TimedMovementSensorReadingResponse{ + TimedIMUResponse: &expectedIMUReading, + } errUnknown = errors.New("unknown error") ) @@ -155,18 +158,18 @@ func onlineModeIMUTestHelper( } config.CartoFacade = &cf - config.IMU = imu + config.MovementSensor = imu config.IsOnline = true - err = config.addIMUReadingInOnline(ctx) + err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldBeNil) test.That(t, len(calls), test.ShouldEqual, 1) - err = config.addIMUReadingInOnline(ctx) + err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldBeNil) test.That(t, len(calls), test.ShouldEqual, 2) - err = config.addIMUReadingInOnline(ctx) + err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldBeNil) test.That(t, len(calls), test.ShouldEqual, 3) @@ -261,9 +264,9 @@ func invalidOnlineModeIMUTestHelper( injectLidar.DataFrequencyHzFunc = func() int { return lidarDataFrequencyHz } config.Lidar = &injectLidar - config.IMU = imu + config.MovementSensor = imu - err = config.addIMUReadingInOnline(ctx) + err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldNotBeNil) test.That(t, len(calls), test.ShouldEqual, 0) } diff --git a/viam_cartographer.go b/viam_cartographer.go index d1dea80f..608ec630 100644 --- a/viam_cartographer.go +++ b/viam_cartographer.go @@ -118,7 +118,7 @@ func initSensorProcesses(cancelCtx context.Context, cartoSvc *CartographerServic CartoFacade: cartoSvc.cartofacade, IsOnline: cartoSvc.lidar.DataFrequencyHz() != 0, Lidar: cartoSvc.lidar, - IMU: cartoSvc.movementSensor, + MovementSensor: cartoSvc.movementSensor, Timeout: cartoSvc.cartoFacadeTimeout, InternalTimeout: cartoSvc.cartoFacadeInternalTimeout, Logger: cartoSvc.logger, @@ -132,11 +132,11 @@ func initSensorProcesses(cancelCtx context.Context, cartoSvc *CartographerServic spConfig.StartLidar(cancelCtx) }() - if spConfig.IMU != nil { + if spConfig.MovementSensor != nil { cartoSvc.sensorProcessWorkers.Add(1) go func() { defer cartoSvc.sensorProcessWorkers.Done() - spConfig.StartIMU(cancelCtx) + spConfig.StartMovementSensor(cancelCtx) }() } } else { @@ -187,10 +187,19 @@ func New( return nil, err } + // Get the lidar for the Dim2D cartographer sub algorithm lidarName := svcConfig.Camera["name"] + timedLidar, err := s.NewLidar(ctx, deps, lidarName, optionalConfigParams.LidarDataFrequencyHz, logger) + if err != nil { + return nil, err + } + // Get the movement sensor if one is configured and check if it supports an IMU and/or odometer. movementSensorName := optionalConfigParams.MovementSensorName - if movementSensorName != "" { + var timedMovementSensor s.TimedMovementSensor + if movementSensorName == "" { + logger.Info("no movement sensor configured, proceeding without IMU and without odometer") + } else { if optionalConfigParams.LidarDataFrequencyHz == 0 && optionalConfigParams.MovementSensorDataFrequencyHz != 0 { return nil, errors.New("In offline mode, but movement sensor data frequency is nonzero") } @@ -198,21 +207,11 @@ func New( if optionalConfigParams.LidarDataFrequencyHz != 0 && optionalConfigParams.MovementSensorDataFrequencyHz == 0 { return nil, errors.New("In online mode, but movement sensor data frequency is zero") } - } - // Get the lidar for the Dim2D cartographer sub algorithm - timedLidar, err := s.NewLidar(ctx, deps, lidarName, optionalConfigParams.LidarDataFrequencyHz, logger) - if err != nil { - return nil, err - } - - // Get the movement sensor if one is configured and check if it supports an IMU and/or odometer. - var timedMovementSensor s.TimedMovementSensor - if movementSensorName == "" { - logger.Info("no movement sensor configured, proceeding without IMU and without odometer") - } else if timedMovementSensor, err = s.NewMovementSensor(ctx, deps, movementSensorName, - optionalConfigParams.MovementSensorDataFrequencyHz, logger); err != nil { - return nil, err + 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 @@ -232,7 +231,6 @@ func New( Named: c.ResourceName().AsNamed(), lidar: timedLidar, movementSensor: timedMovementSensor, - movementSensorName: movementSensorName, subAlgo: subAlgo, configParams: svcConfig.ConfigParams, cancelSensorProcessFunc: cancelSensorProcessFunc, @@ -395,18 +393,11 @@ func initCartoFacade(ctx context.Context, cartoSvc *CartographerService) error { return err } - cartoCfg := cartofacade.CartoConfig{ - Camera: cartoSvc.lidar.Name(), - MovementSensor: cartoSvc.movementSensorName, - ComponentReference: cartoSvc.lidar.Name(), - LidarConfig: cartofacade.TwoD, - EnableMapping: cartoSvc.enableMapping, - ExistingMap: cartoSvc.existingMap, - } - - if cartoSvc.movementSensorName == "" { + var movementSensorName string + if cartoSvc.movementSensor == nil { cartoSvc.logger.Debug("No movement sensor provided, setting use_imu_data to false") } else { + movementSensorName = cartoSvc.movementSensor.Name() movementSensorProperties := cartoSvc.movementSensor.Properties() if movementSensorProperties.IMUSupported { cartoSvc.logger.Warn("IMU configured, setting use_imu_data to true") @@ -414,6 +405,18 @@ func initCartoFacade(ctx context.Context, cartoSvc *CartographerService) error { } else { cartoSvc.logger.Warn("Movement sensor was provided but does not support IMU data, setting use_imu_data to false") } + if movementSensorProperties.OdometerSupported { + cartoSvc.logger.Debug("Odometer is supported") + } + } + + cartoCfg := cartofacade.CartoConfig{ + Camera: cartoSvc.lidar.Name(), + MovementSensor: movementSensorName, + ComponentReference: cartoSvc.lidar.Name(), + LidarConfig: cartofacade.TwoD, + EnableMapping: cartoSvc.enableMapping, + ExistingMap: cartoSvc.existingMap, } cf := cartofacade.New(&cartoLib, cartoCfg, cartoAlgoConfig) @@ -463,13 +466,13 @@ func terminateCartoFacade(ctx context.Context, cartoSvc *CartographerService) er type CartographerService struct { resource.Named resource.AlwaysRebuild - mu sync.Mutex - SlamMode cartofacade.SlamMode - closed bool - lidar s.TimedLidar - movementSensor s.TimedMovementSensor - movementSensorName string - subAlgo SubAlgo + SlamMode cartofacade.SlamMode + lidar s.TimedLidar + movementSensor s.TimedMovementSensor + subAlgo SubAlgo + + muClose sync.Mutex + closed bool configParams map[string]string @@ -500,7 +503,11 @@ func (cartoSvc *CartographerService) Position(ctx context.Context) (spatialmath. cartoSvc.logger.Warn("Position called with use_cloud_slam set to true") return nil, "", ErrUseCloudSlamEnabled } - if cartoSvc.closed { + + cartoSvc.muClose.Lock() + cartoSvcClosed := cartoSvc.closed + cartoSvc.muClose.Unlock() + if cartoSvcClosed { cartoSvc.logger.Warn("Position called after closed") return nil, "", ErrClosed } @@ -532,7 +539,10 @@ func (cartoSvc *CartographerService) PointCloudMap(ctx context.Context) (func() return nil, ErrUseCloudSlamEnabled } - if cartoSvc.closed { + cartoSvc.muClose.Lock() + cartoSvcClosed := cartoSvc.closed + cartoSvc.muClose.Unlock() + if cartoSvcClosed { cartoSvc.logger.Warn("PointCloudMap called after closed") return nil, ErrClosed } @@ -554,7 +564,10 @@ func (cartoSvc *CartographerService) InternalState(ctx context.Context) (func() return nil, ErrUseCloudSlamEnabled } - if cartoSvc.closed { + cartoSvc.muClose.Lock() + cartoSvcClosed := cartoSvc.closed + cartoSvc.muClose.Unlock() + if cartoSvcClosed { cartoSvc.logger.Warn("InternalState called after closed") return nil, ErrClosed } @@ -592,7 +605,10 @@ func (cartoSvc *CartographerService) LatestMapInfo(ctx context.Context) (time.Ti return time.Time{}, ErrUseCloudSlamEnabled } - if cartoSvc.closed { + cartoSvc.muClose.Lock() + cartoSvcClosed := cartoSvc.closed + cartoSvc.muClose.Unlock() + if cartoSvcClosed { cartoSvc.logger.Warn("LatestMapInfo called after closed") return time.Time{}, ErrClosed } @@ -610,7 +626,11 @@ func (cartoSvc *CartographerService) DoCommand(ctx context.Context, req map[stri cartoSvc.logger.Warn("DoCommand called with use_cloud_slam set to true") return nil, ErrUseCloudSlamEnabled } - if cartoSvc.closed { + + cartoSvc.muClose.Lock() + cartoSvcClosed := cartoSvc.closed + cartoSvc.muClose.Unlock() + if cartoSvcClosed { cartoSvc.logger.Warn("DoCommand called after closed") return nil, ErrClosed } @@ -624,14 +644,14 @@ func (cartoSvc *CartographerService) DoCommand(ctx context.Context, req map[stri // Close out of all slam related processes. func (cartoSvc *CartographerService) Close(ctx context.Context) error { - cartoSvc.mu.Lock() + cartoSvc.muClose.Lock() + defer cartoSvc.muClose.Unlock() if cartoSvc.useCloudSlam { return nil } cartoSvc.logger.Info("Closing cartographer module") - defer cartoSvc.mu.Unlock() if cartoSvc.closed { cartoSvc.logger.Warn("Close() called multiple times") return nil From bb0f9cc0e87fb9e25341200725332e755879b190 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Mon, 11 Dec 2023 10:19:16 -0500 Subject: [PATCH 04/21] Update comment --- sensorprocess/movementsensorprocess.go | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sensorprocess/movementsensorprocess.go b/sensorprocess/movementsensorprocess.go index e8ecfa83..6a4c6093 100644 --- a/sensorprocess/movementsensorprocess.go +++ b/sensorprocess/movementsensorprocess.go @@ -28,8 +28,8 @@ func (config *Config) StartMovementSensor(ctx context.Context) { } } -// addMovementSensorReadingInOnline ensures the most recent movement sensor scan, -// after corresponding lidar scans, gets processed by cartographer. +// addMovementSensorReadingInOnline attempts to get and add a movement sensor reading to the +// cartofacade. func (config *Config) addMovementSensorReadingInOnline(ctx context.Context) error { // get next movement sensor data response; ignoring status since it is always false movementSensorReading, err := config.MovementSensor.TimedMovementSensorReading(ctx) From c430759296e398806e47323c27af1fa8d93c4a32 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Mon, 11 Dec 2023 10:37:01 -0500 Subject: [PATCH 05/21] Process the odometer before the imu --- sensorprocess/movementsensorprocess.go | 32 +++++++++++++------------- sensorprocess/sensorprocess.go | 12 +++++----- sensors/movementsensor.go | 28 +++++++++++----------- 3 files changed, 36 insertions(+), 36 deletions(-) diff --git a/sensorprocess/movementsensorprocess.go b/sensorprocess/movementsensorprocess.go index 6a4c6093..de9d3d69 100644 --- a/sensorprocess/movementsensorprocess.go +++ b/sensorprocess/movementsensorprocess.go @@ -67,22 +67,22 @@ func (config *Config) tryAddMovementSensorReadingUntilSuccess(ctx context.Contex case <-ctx.Done(): return ctx.Err() default: - if !imuDone { - if err := config.tryAddIMUReading(ctx, *reading.TimedIMUResponse); err != nil { + if !odometerDone { + if err := config.tryAddOdometerReading(ctx, *reading.TimedOdometerResponse); err != nil { if !errors.Is(err, cartofacade.ErrUnableToAcquireLock) { - config.Logger.Warnw("Retrying IMU sensor reading due to error from cartofacade", "error", err) + config.Logger.Warnw("Retrying odometer sensor reading due to error from cartofacade", "error", err) } } else { - imuDone = true + odometerDone = true } } - if !odometerDone { - if err := config.tryAddOdometerReading(ctx, *reading.TimedOdometerResponse); err != nil { + if !imuDone { + if err := config.tryAddIMUReading(ctx, *reading.TimedIMUResponse); err != nil { if !errors.Is(err, cartofacade.ErrUnableToAcquireLock) { - config.Logger.Warnw("Retrying odometer sensor reading due to error from cartofacade", "error", err) + config.Logger.Warnw("Retrying IMU sensor reading due to error from cartofacade", "error", err) } } else { - odometerDone = true + imuDone = true } } if imuDone && odometerDone { @@ -96,22 +96,22 @@ func (config *Config) tryAddMovementSensorReadingUntilSuccess(ctx context.Contex func (config *Config) tryAddMovementSensorReadingOnce(ctx context.Context, reading s.TimedMovementSensorReadingResponse) int { startTime := time.Now().UTC() - if config.MovementSensor.Properties().IMUSupported { - if err := config.tryAddIMUReading(ctx, *reading.TimedIMUResponse); err != nil { + if config.MovementSensor.Properties().OdometerSupported { + if err := config.tryAddOdometerReading(ctx, *reading.TimedOdometerResponse); err != nil { if errors.Is(err, cartofacade.ErrUnableToAcquireLock) { - config.Logger.Debugw("Skipping IMU sensor reading due to lock contention in cartofacade", "error", err) + config.Logger.Debugw("Skipping odometer sensor reading due to lock contention in cartofacade", "error", err) } else { - config.Logger.Warnw("Skipping IMU sensor reading due to error from cartofacade", "error", err) + config.Logger.Warnw("Skipping odometer sensor reading due to error from cartofacade", "error", err) } } } - if config.MovementSensor.Properties().OdometerSupported { - if err := config.tryAddOdometerReading(ctx, *reading.TimedOdometerResponse); err != nil { + if config.MovementSensor.Properties().IMUSupported { + if err := config.tryAddIMUReading(ctx, *reading.TimedIMUResponse); err != nil { if errors.Is(err, cartofacade.ErrUnableToAcquireLock) { - config.Logger.Debugw("Skipping odometer sensor reading due to lock contention in cartofacade", "error", err) + config.Logger.Debugw("Skipping IMU sensor reading due to lock contention in cartofacade", "error", err) } else { - config.Logger.Warnw("Skipping odometer sensor reading due to error from cartofacade", "error", err) + config.Logger.Warnw("Skipping IMU sensor reading due to error from cartofacade", "error", err) } } } diff --git a/sensorprocess/sensorprocess.go b/sensorprocess/sensorprocess.go index a3bc776d..4452849a 100644 --- a/sensorprocess/sensorprocess.go +++ b/sensorprocess/sensorprocess.go @@ -45,13 +45,13 @@ func (config *Config) getInitialMovementSensorReading(ctx context.Context, } var readingTime time.Time - if config.MovementSensor.Properties().IMUSupported { - // we can assume that the imu reading time is earlier than the odometer reading - // time, since the imu reading is taken before the odometer reading - readingTime = movementSensorReading.TimedIMUResponse.ReadingTime - } else { - // we reach this case if the imu is not supported + if config.MovementSensor.Properties().OdometerSupported { + // we can assume that the odometer reading time is earlier than the imu reading + // time, since the odometer reading is taken before the imu reading readingTime = movementSensorReading.TimedOdometerResponse.ReadingTime + } else { + // we reach this case if the odometer is not supported + readingTime = movementSensorReading.TimedIMUResponse.ReadingTime } if !readingTime.Before(lidarReading.ReadingTime) { diff --git a/sensors/movementsensor.go b/sensors/movementsensor.go index cb6c6791..f4c660a3 100644 --- a/sensors/movementsensor.go +++ b/sensors/movementsensor.go @@ -107,36 +107,36 @@ func (ms *MovementSensor) TimedMovementSensorReading(ctx context.Context) (Timed timeoutCtx, cancel := context.WithTimeout(ctx, timedMovementSensorReadingTimeout) defer cancel() - if ms.imuSupported { - imuLoop: + if ms.odometerSupported { + odometerLoop: for { select { case <-timeoutCtx.Done(): - return TimedMovementSensorReadingResponse{}, errors.Wrap(timeoutCtx.Err(), "timed out getting IMU data") + return TimedMovementSensorReadingResponse{}, errors.Wrap(timeoutCtx.Err(), "timed out getting odometer data") default: - if timedIMUReadingResponse, err = ms.timedIMUReading(timeoutCtx, &angVel, &linAcc, - &readingTimeAngularVel, &readingTimeLinearAcc); err != nil && !errors.Is(err, ErrNoValidReadingObtained) { + if timedOdometerReadingResponse, err = ms.timedOdometerReading(timeoutCtx, position, &orientation, + &readingTimePosition, &readingTimeOrientation); err != nil && !errors.Is(err, ErrNoValidReadingObtained) { return TimedMovementSensorReadingResponse{}, err } - if timedIMUReadingResponse != nil { - break imuLoop + if timedOdometerReadingResponse != nil { + break odometerLoop } } } } - if ms.odometerSupported { - odometerLoop: + if ms.imuSupported { + imuLoop: for { select { case <-timeoutCtx.Done(): - return TimedMovementSensorReadingResponse{}, errors.Wrap(timeoutCtx.Err(), "timed out getting odometer data") + return TimedMovementSensorReadingResponse{}, errors.Wrap(timeoutCtx.Err(), "timed out getting IMU data") default: - if timedOdometerReadingResponse, err = ms.timedOdometerReading(timeoutCtx, position, &orientation, - &readingTimePosition, &readingTimeOrientation); err != nil && !errors.Is(err, ErrNoValidReadingObtained) { + if timedIMUReadingResponse, err = ms.timedIMUReading(timeoutCtx, &angVel, &linAcc, + &readingTimeAngularVel, &readingTimeLinearAcc); err != nil && !errors.Is(err, ErrNoValidReadingObtained) { return TimedMovementSensorReadingResponse{}, err } - if timedOdometerReadingResponse != nil { - break odometerLoop + if timedIMUReadingResponse != nil { + break imuLoop } } } From 244cd4e81c7c53d33d0acfd2dc1f80112500f619 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Mon, 11 Dec 2023 12:21:37 -0500 Subject: [PATCH 06/21] Add tests --- sensorprocess/sensorprocess.go | 4 + sensorprocess/sensorprocess_test.go | 278 ++++++++++++++++++++++++++-- 2 files changed, 264 insertions(+), 18 deletions(-) diff --git a/sensorprocess/sensorprocess.go b/sensorprocess/sensorprocess.go index 4452849a..c09c794c 100644 --- a/sensorprocess/sensorprocess.go +++ b/sensorprocess/sensorprocess.go @@ -97,6 +97,10 @@ func (config *Config) StartOfflineSensorProcess(ctx context.Context) bool { readingTimes := []readingTime{ {sensorType: "lidar", readingTime: lidarReading.ReadingTime}, } + // default to the slightly later imu timestamp, in case that the odometer time stamp was + // taken before the lidar time stamp, but the imu time stamp was taken after the lidar time + // stamp, we'll want to prioritize adding the lidar measurement before adding the movement + // sensor measurement if config.MovementSensor != nil && config.MovementSensor.Properties().IMUSupported { readingTimes = append(readingTimes, readingTime{ diff --git a/sensorprocess/sensorprocess_test.go b/sensorprocess/sensorprocess_test.go index 6f1234b3..ea6ff0bf 100644 --- a/sensorprocess/sensorprocess_test.go +++ b/sensorprocess/sensorprocess_test.go @@ -9,6 +9,7 @@ import ( "time" "github.com/golang/geo/r3" + geo "github.com/kellydunn/golang-geo" "go.viam.com/rdk/components/camera/replaypcd" "go.viam.com/rdk/components/movementsensor/replay" "go.viam.com/rdk/logging" @@ -20,6 +21,160 @@ import ( "github.com/viamrobotics/viam-cartographer/sensors/inject" ) +func TestGetInitialMovementSensorReading(t *testing.T) { + logger := logging.NewTestLogger(t) + + lidarReading := s.TimedLidarReadingResponse{ + Reading: []byte("12345"), + ReadingTime: time.Now().UTC(), + } + + odometerReading := s.TimedOdometerReadingResponse{ + Position: geo.NewPoint(5, 4), + Orientation: &spatialmath.Quaternion{Real: 0.1, Imag: -0.2, Jmag: 2.5, Kmag: -9.1}, + ReadingTime: time.Now().UTC(), + } + + imuReading := s.TimedIMUReadingResponse{ + LinearAcceleration: r3.Vector{X: 1, Y: 2, Z: 3}, + AngularVelocity: spatialmath.AngularVelocity{X: 4, Y: 5, Z: 6}, + ReadingTime: time.Now().UTC(), + } + + cf := cartofacade.Mock{} + + injectLidar := inject.TimedLidar{} + injectLidar.NameFunc = func() string { return "good_lidar" } + injectLidar.DataFrequencyHzFunc = func() int { return 0 } + + injectMovementSensor := inject.TimedMovementSensor{} + injectMovementSensor.NameFunc = func() string { return "good_movement_sensor" } + injectMovementSensor.DataFrequencyHzFunc = func() int { return 0 } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + Timeout: 10 * time.Second, + } + + t.Run("return error if movement sensor is not supported", func(t *testing.T) { + config.MovementSensor = nil + movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, errors.New("movement sensor is not supported")) + test.That(t, movementSensorReading, test.ShouldResemble, s.TimedMovementSensorReadingResponse{}) + }) + + t.Run("return error if neither IMU nor odometer are supported", func(t *testing.T) { + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{} + } + config.MovementSensor = &injectMovementSensor + movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, errors.New("movement sensor is not supported")) + test.That(t, movementSensorReading, test.ShouldResemble, s.TimedMovementSensorReadingResponse{}) + }) + + t.Run("return error if TimedMovementSensorReading errors out", func(t *testing.T) { + expectedErr := errors.New("error in TimedMovementSensorReading") + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + } + } + injectMovementSensor.TimedMovementSensorReadingFunc = func(ctx context.Context) (s.TimedMovementSensorReadingResponse, error) { + return s.TimedMovementSensorReadingResponse{}, expectedErr + } + config.MovementSensor = &injectMovementSensor + movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, expectedErr) + test.That(t, movementSensorReading, test.ShouldResemble, s.TimedMovementSensorReadingResponse{}) + }) + + t.Run("successful data insertion", func(t *testing.T) { + cases := []struct { + description string + imuEnabled bool + odometerEnabled bool + lidarReadingTimeAddedMs int + msReadingTimeAddedMs []int + expectedMsReadingTimeAddedMs int + }{ + { + description: "skip movement sensor data until first lidar data is inserted", + imuEnabled: true, + odometerEnabled: true, + lidarReadingTimeAddedMs: 5, + msReadingTimeAddedMs: []int{1, 2, 3, 4, 6, 7, 8}, + expectedMsReadingTimeAddedMs: 6, + }, + { + description: "skip imu data until first lidar data is inserted", + imuEnabled: true, + odometerEnabled: false, + lidarReadingTimeAddedMs: 7, + msReadingTimeAddedMs: []int{1, 2, 3, 4, 5, 6, 7, 8}, + expectedMsReadingTimeAddedMs: 7, + }, + { + description: "skip odometer data until first lidar data is inserted", + imuEnabled: false, + odometerEnabled: true, + lidarReadingTimeAddedMs: 3, + msReadingTimeAddedMs: []int{1, 2, 5, 6, 7, 8}, + expectedMsReadingTimeAddedMs: 5, + }, + } + + for _, tt := range cases { + t.Run(tt.description, func(t *testing.T) { + now := time.Now().UTC() + + if tt.imuEnabled || tt.odometerEnabled { + config.MovementSensor = &injectMovementSensor + } else { + config.MovementSensor = nil + } + + lidarReading.ReadingTime = now.Add(time.Duration(tt.lidarReadingTimeAddedMs) * time.Millisecond) + + numMovementSensorData := 0 + injectMovementSensor.TimedMovementSensorReadingFunc = func(ctx context.Context) (s.TimedMovementSensorReadingResponse, error) { + var movementSensorReading s.TimedMovementSensorReadingResponse + if numMovementSensorData < len(tt.msReadingTimeAddedMs) { + if tt.odometerEnabled { + movementSensorReading.TimedOdometerResponse = &odometerReading + movementSensorReading.TimedOdometerResponse.ReadingTime = now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) + } + if tt.imuEnabled { + movementSensorReading.TimedIMUResponse = &imuReading + movementSensorReading.TimedIMUResponse.ReadingTime = now.Add(time.Duration(float64(tt.msReadingTimeAddedMs[numMovementSensorData])+0.1) * time.Millisecond) + } + numMovementSensorData++ + return movementSensorReading, nil + } + return movementSensorReading, replay.ErrEndOfDataset + } + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: tt.imuEnabled, + OdometerSupported: tt.odometerEnabled, + } + } + + movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) + test.That(t, err, test.ShouldBeNil) + test.That(t, movementSensorReading, test.ShouldNotResemble, s.TimedMovementSensorReadingResponse{}) + test.That(t, tt.msReadingTimeAddedMs[numMovementSensorData-1], test.ShouldResemble, tt.expectedMsReadingTimeAddedMs) + }) + } + }) +} + func TestStartOfflineSensorProcess(t *testing.T) { logger := logging.NewTestLogger(t) @@ -28,12 +183,21 @@ func TestStartOfflineSensorProcess(t *testing.T) { ReadingTime: time.Now().UTC(), } + odometerReading := s.TimedOdometerReadingResponse{ + Position: geo.NewPoint(5, 4), + Orientation: &spatialmath.Quaternion{Real: 0.1, Imag: -0.2, Jmag: 2.5, Kmag: -9.1}, + ReadingTime: time.Now().UTC(), + } + + imuReading := s.TimedIMUReadingResponse{ + LinearAcceleration: r3.Vector{X: 1, Y: 2, Z: 3}, + AngularVelocity: spatialmath.AngularVelocity{X: 4, Y: 5, Z: 6}, + ReadingTime: time.Now().UTC(), + } + movementSensorReading := s.TimedMovementSensorReadingResponse{ - TimedIMUResponse: &s.TimedIMUReadingResponse{ - LinearAcceleration: r3.Vector{X: 1, Y: 2, Z: 3}, - AngularVelocity: spatialmath.AngularVelocity{X: 4, Y: 5, Z: 6}, - ReadingTime: time.Now().UTC(), - }, + TimedIMUResponse: &imuReading, + TimedOdometerResponse: &odometerReading, } cf := cartofacade.Mock{} @@ -59,6 +223,17 @@ func TestStartOfflineSensorProcess(t *testing.T) { return s.TimedLidarReadingResponse{}, replaypcd.ErrEndOfDataset } + injectMovementSensor.TimedMovementSensorReadingFunc = func(ctx context.Context) (s.TimedMovementSensorReadingResponse, error) { + return movementSensorReading, nil + } + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + OdometerSupported: true, + } + } + config.MovementSensor = &injectMovementSensor + countAddedLidarData := 0 cf.AddLidarReadingFunc = func(ctx context.Context, timeout time.Duration, lidarName string, currentReading s.TimedLidarReadingResponse, @@ -75,10 +250,19 @@ func TestStartOfflineSensorProcess(t *testing.T) { return nil } + countAddedOdometerData := 0 + cf.AddOdometerReadingFunc = func(ctx context.Context, timeout time.Duration, + odometerName string, currentReading s.TimedOdometerReadingResponse, + ) error { + countAddedIMUData++ + return nil + } + endOfDataSetReached := config.StartOfflineSensorProcess(context.Background()) test.That(t, endOfDataSetReached, test.ShouldBeTrue) test.That(t, countAddedLidarData, test.ShouldEqual, 0) test.That(t, countAddedIMUData, test.ShouldEqual, 0) + test.That(t, countAddedOdometerData, test.ShouldEqual, 0) }) t.Run("returns true when lidar reaches the end of the dataset and the optimization function fails", func(t *testing.T) { @@ -86,12 +270,13 @@ func TestStartOfflineSensorProcess(t *testing.T) { return errors.New("test error") } - lidar, imu := s.FinishedReplayLidar, s.NoMovementSensor + lidar, ms := s.FinishedReplayLidar, s.NoMovementSensor dataFrequencyHz := 0 - replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) + replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, ms), string(lidar), dataFrequencyHz, logger) test.That(t, err, test.ShouldBeNil) config.Lidar = replaySensor + config.MovementSensor = nil endOfDataSetReached := config.StartOfflineSensorProcess(context.Background()) test.That(t, endOfDataSetReached, test.ShouldBeTrue) @@ -106,45 +291,82 @@ func TestStartOfflineSensorProcess(t *testing.T) { cases := []struct { description string imuEnabled bool + odometerEnabled bool lidarReadingTimeAddedMs []int msReadingTimeAddedMs []int expectedDataInsertions []string }{ { - description: "no imu data is added if imu is not enabled", + description: "no movement sensor data is added if movement sensor is not enabled", + odometerEnabled: false, imuEnabled: false, lidarReadingTimeAddedMs: []int{0, 2, 4, 6, 8}, msReadingTimeAddedMs: []int{1, 3, 5}, expectedDataInsertions: []string{"lidar: 0", "lidar: 2", "lidar: 4", "lidar: 6", "lidar: 8"}, }, { - description: "skip imu data until first lidar data is inserted", + description: "skip movement sensor data until first lidar data is inserted", imuEnabled: true, + odometerEnabled: true, lidarReadingTimeAddedMs: []int{5, 7}, msReadingTimeAddedMs: []int{1, 2, 3, 4, 5, 6, 7, 8}, - expectedDataInsertions: []string{"lidar: 5", "imu: 5", "imu: 6", "lidar: 7"}, + expectedDataInsertions: []string{"lidar: 5", "odometer: 5", "imu: 5", "odometer: 6", "imu: 6", "lidar: 7"}, }, { description: "if imu data ends before lidar data ends, stop adding data once end of imu dataset is reached", imuEnabled: true, + odometerEnabled: false, lidarReadingTimeAddedMs: []int{2, 4, 6, 8, 10, 12}, msReadingTimeAddedMs: []int{1, 3, 5}, expectedDataInsertions: []string{"lidar: 2", "imu: 3", "lidar: 4", "imu: 5"}, }, + { + description: "if odometer data ends before lidar data ends, stop adding data once end of odometer dataset is reached", + imuEnabled: false, + odometerEnabled: true, + lidarReadingTimeAddedMs: []int{2, 4, 6, 8, 10, 12}, + msReadingTimeAddedMs: []int{1, 3, 5}, + expectedDataInsertions: []string{"lidar: 2", "odometer: 3", "lidar: 4", "odometer: 5"}, + }, + { + description: "if movement sensor data ends before lidar data ends, stop adding data once end of movement sensor dataset is reached", + imuEnabled: true, + odometerEnabled: true, + lidarReadingTimeAddedMs: []int{2, 4, 6, 8, 10, 12}, + msReadingTimeAddedMs: []int{1, 3, 5}, + expectedDataInsertions: []string{"lidar: 2", "odometer: 3", "imu: 3", "lidar: 4", "odometer: 5", "imu: 5"}, + }, { description: "if lidar data ends before imu data ends, stop adding data once end of lidar dataset is reached", imuEnabled: true, + odometerEnabled: false, lidarReadingTimeAddedMs: []int{1, 3, 5}, msReadingTimeAddedMs: []int{2, 3, 4, 6, 8, 10, 12}, expectedDataInsertions: []string{"lidar: 1", "imu: 2", "lidar: 3", "imu: 3", "imu: 4", "lidar: 5"}, }, + { + description: "if lidar data ends before odometer data ends, stop adding data once end of lidar dataset is reached", + imuEnabled: false, + odometerEnabled: true, + lidarReadingTimeAddedMs: []int{1, 3, 5}, + msReadingTimeAddedMs: []int{2, 3, 4, 6, 8, 10, 12}, + expectedDataInsertions: []string{"lidar: 1", "odometer: 2", "lidar: 3", "odometer: 3", "odometer: 4", "lidar: 5"}, + }, + { + description: "if lidar data ends before movement sensor data ends, stop adding data once end of lidar dataset is reached", + imuEnabled: true, + odometerEnabled: true, + lidarReadingTimeAddedMs: []int{1, 3, 5}, + msReadingTimeAddedMs: []int{2, 3, 4, 6, 8, 10, 12}, + expectedDataInsertions: []string{"lidar: 1", "odometer: 2", "imu: 2", "lidar: 3", "odometer: 3", "imu: 3", "odometer: 4", "imu: 4", "lidar: 5"}, + }, } for _, tt := range cases { t.Run(tt.description, func(t *testing.T) { now := time.Now().UTC() - if tt.imuEnabled { + if tt.imuEnabled || tt.odometerEnabled { config.MovementSensor = &injectMovementSensor } else { config.MovementSensor = nil @@ -160,18 +382,27 @@ func TestStartOfflineSensorProcess(t *testing.T) { return s.TimedLidarReadingResponse{}, replaypcd.ErrEndOfDataset } - numIMUData := 0 + numMovementSensorData := 0 injectMovementSensor.TimedMovementSensorReadingFunc = func(ctx context.Context) (s.TimedMovementSensorReadingResponse, error) { - if numIMUData < len(tt.msReadingTimeAddedMs) { - movementSensorReading.TimedIMUResponse.ReadingTime = now.Add(time.Duration(tt.msReadingTimeAddedMs[numIMUData]) * time.Millisecond) - numIMUData++ + var movementSensorReading s.TimedMovementSensorReadingResponse + if numMovementSensorData < len(tt.msReadingTimeAddedMs) { + if tt.odometerEnabled { + movementSensorReading.TimedOdometerResponse = &odometerReading + movementSensorReading.TimedOdometerResponse.ReadingTime = now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) + } + if tt.imuEnabled { + movementSensorReading.TimedIMUResponse = &imuReading + movementSensorReading.TimedIMUResponse.ReadingTime = now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) + } + numMovementSensorData++ return movementSensorReading, nil } - return s.TimedMovementSensorReadingResponse{}, replay.ErrEndOfDataset + return movementSensorReading, replay.ErrEndOfDataset } injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { return s.MovementSensorProperties{ - IMUSupported: true, + IMUSupported: tt.imuEnabled, + OdometerSupported: tt.odometerEnabled, } } @@ -190,11 +421,20 @@ func TestStartOfflineSensorProcess(t *testing.T) { cf.AddIMUReadingFunc = func(ctx context.Context, timeout time.Duration, imuName string, currentReading s.TimedIMUReadingResponse, ) error { - actualDataInsertions = append(actualDataInsertions, "imu: "+fmt.Sprint(tt.msReadingTimeAddedMs[numIMUData-1])) + actualDataInsertions = append(actualDataInsertions, "imu: "+fmt.Sprint(tt.msReadingTimeAddedMs[numMovementSensorData-1])) countAddedIMUData++ return nil } + countAddedOdometerData := 0 + cf.AddOdometerReadingFunc = func(ctx context.Context, timeout time.Duration, + odometerName string, currentReading s.TimedOdometerReadingResponse, + ) error { + actualDataInsertions = append(actualDataInsertions, "odometer: "+fmt.Sprint(tt.msReadingTimeAddedMs[numMovementSensorData-1])) + countAddedOdometerData++ + return nil + } + countItemsInList := func(list []string, keyword string) int { counter := 0 for _, item := range list { @@ -207,11 +447,13 @@ func TestStartOfflineSensorProcess(t *testing.T) { expectedCountAddedLidarData := countItemsInList(tt.expectedDataInsertions, "lidar") expectedCountAddedIMUData := countItemsInList(tt.expectedDataInsertions, "imu") + expectedCountAddedOdometerData := countItemsInList(tt.expectedDataInsertions, "odometer") endOfDataSetReached := config.StartOfflineSensorProcess(context.Background()) test.That(t, endOfDataSetReached, test.ShouldBeTrue) test.That(t, countAddedLidarData, test.ShouldEqual, expectedCountAddedLidarData) test.That(t, countAddedIMUData, test.ShouldEqual, expectedCountAddedIMUData) + test.That(t, countAddedOdometerData, test.ShouldEqual, expectedCountAddedOdometerData) test.That(t, actualDataInsertions, test.ShouldResemble, tt.expectedDataInsertions) }) } From 73c8fed07fc4f63fd8b5692eef9ff5de975ef2bb Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Mon, 11 Dec 2023 12:27:45 -0500 Subject: [PATCH 07/21] Move tests into separate files --- sensorprocess/lidarsensorprocess_test.go | 257 +++++++ sensorprocess/movementsensorprocess_test.go | 275 ++++++++ sensorprocess/sensorprocess_test.go | 728 +++----------------- 3 files changed, 645 insertions(+), 615 deletions(-) create mode 100644 sensorprocess/lidarsensorprocess_test.go create mode 100644 sensorprocess/movementsensorprocess_test.go diff --git a/sensorprocess/lidarsensorprocess_test.go b/sensorprocess/lidarsensorprocess_test.go new file mode 100644 index 00000000..68a1fd4c --- /dev/null +++ b/sensorprocess/lidarsensorprocess_test.go @@ -0,0 +1,257 @@ +package sensorprocess + +import ( + "context" + "testing" + "time" + + "go.viam.com/rdk/logging" + "go.viam.com/test" + + "github.com/viamrobotics/viam-cartographer/cartofacade" + s "github.com/viamrobotics/viam-cartographer/sensors" + "github.com/viamrobotics/viam-cartographer/sensors/inject" +) + +func TestTryAddLidarReading(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + reading := s.TimedLidarReadingResponse{ + Reading: []byte("12345"), + ReadingTime: time.Now().UTC(), + } + + dataFrequencyHz := 5 + injectLidar := inject.TimedLidar{} + injectLidar.NameFunc = func() string { return "good_lidar" } + injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + Timeout: 10 * time.Second, + } + t.Run("when AddLidarReading blocks for more than the data rate and succeeds, time to sleep is 0", func(t *testing.T) { + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return nil + } + + timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddLidarReading is slower than data rate and returns a lock error, time to sleep is 0", func(t *testing.T) { + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddLidarReading blocks for more than the date rate "+ + "and returns an unexpected error, time to sleep is 0", func(t *testing.T) { + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return errUnknown + } + + timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddLidarReading is faster than the date rate and succeeds, time to sleep is <= date rate", func(t *testing.T) { + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + return nil + } + + timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.Lidar.DataFrequencyHz()) + }) + + t.Run("when AddLidarReading is faster than the date rate "+ + "and returns lock error, time to sleep is <= date rate", func(t *testing.T) { + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.Lidar.DataFrequencyHz()) + }) + + t.Run("when AddLidarReading is faster than date rate "+ + "and returns an unexpected error, time to sleep is <= date rate", func(t *testing.T) { + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + return errUnknown + } + + timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.Lidar.DataFrequencyHz()) + }) +} + +func TestAddLidarReadingInOnline(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + + dataFrequencyHz := 5 + injectLidar := inject.TimedLidar{} + injectLidar.NameFunc = func() string { return "good_lidar" } + injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + Timeout: 10 * time.Second, + } + + t.Run("returns error when lidar GetData returns error, doesn't try to add lidar data", func(t *testing.T) { + invalidOnlineModeLidarTestHelper(context.Background(), t, cf, config, s.LidarWithErroringFunctions, 10) + }) + + t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { + invalidOnlineModeLidarTestHelper(context.Background(), t, cf, config, s.InvalidReplayLidar, 10) + }) + + t.Run("online lidar adds sensor reading once and ignores errors", func(t *testing.T) { + onlineModeLidarTestHelper(context.Background(), t, config, cf, s.GoodLidar) + }) +} + +func TestStartLidar(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + + dataFrequencyHz := 5 + injectLidar := inject.TimedLidar{} + injectLidar.NameFunc = func() string { return "good_lidar" } + injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + Timeout: 10 * time.Second, + } + + t.Run("exits loop when the context was cancelled", func(t *testing.T) { + cancelCtx, cancelFunc := context.WithCancel(context.Background()) + + lidar, imu := s.FinishedReplayLidar, s.NoMovementSensor + replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) + test.That(t, err, test.ShouldBeNil) + + config.Lidar = replaySensor + + cancelFunc() + + config.StartLidar(cancelCtx) + }) +} + +func TestTryAddLidarReadingUntilSuccess(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + cf.RunFinalOptimizationFunc = func(context.Context, time.Duration) error { + return nil + } + + dataFrequencyHz := 0 + + lidarReading := s.TimedLidarReadingResponse{ + Reading: []byte("12345"), + ReadingTime: time.Now().UTC(), + } + + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: false, + Lidar: &injectLidar, + Timeout: 10 * time.Second, + } + + t.Run("replay lidar adds sensor data until success", func(t *testing.T) { + lidar, imu := s.ReplayLidar, s.NoMovementSensor + replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) + test.That(t, err, test.ShouldBeNil) + + var calls []addLidarReadingArgs + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + args := addLidarReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + calls = append(calls, args) + if len(calls) == 1 { + return errUnknown + } + if len(calls) == 2 { + return cartofacade.ErrUnableToAcquireLock + } + return nil + } + config.Lidar = replaySensor + + config.tryAddLidarReadingUntilSuccess(context.Background(), lidarReading) + test.That(t, len(calls), test.ShouldEqual, 3) + + firstTimestamp := calls[0].currentReading.ReadingTime + for i, call := range calls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, string(lidar)) + test.That(t, call.currentReading.Reading, test.ShouldResemble, lidarReading.Reading) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + } + }) +} diff --git a/sensorprocess/movementsensorprocess_test.go b/sensorprocess/movementsensorprocess_test.go new file mode 100644 index 00000000..9d2a632a --- /dev/null +++ b/sensorprocess/movementsensorprocess_test.go @@ -0,0 +1,275 @@ +package sensorprocess + +import ( + "context" + "testing" + "time" + + "github.com/golang/geo/r3" + "go.viam.com/rdk/logging" + "go.viam.com/rdk/spatialmath" + "go.viam.com/test" + + "github.com/viamrobotics/viam-cartographer/cartofacade" + s "github.com/viamrobotics/viam-cartographer/sensors" + "github.com/viamrobotics/viam-cartographer/sensors/inject" +) + +func TestTryAddIMUReading(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + imuReading := s.TimedIMUReadingResponse{ + LinearAcceleration: r3.Vector{X: 1, Y: 1, Z: 1}, + AngularVelocity: spatialmath.AngularVelocity{X: 1, Y: 1, Z: 1}, + ReadingTime: time.Now().UTC(), + } + + reading := s.TimedMovementSensorReadingResponse{ + TimedIMUResponse: &imuReading, + } + + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return 5 } + + injectImu := inject.TimedMovementSensor{} + injectImu.NameFunc = func() string { return "good_imu" } + injectImu.DataFrequencyHzFunc = func() int { return 20 } + injectImu.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + } + } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + MovementSensor: &injectImu, + Timeout: 10 * time.Second, + } + + t.Run("when AddIMUReading blocks for more than the date rate and succeeds, time to sleep is 0", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return nil + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddIMUReading is slower than date rate and returns a lock error, time to sleep is 0", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddIMUReading blocks for more than the date rate and returns an unexpected error, time to sleep is 0", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return errUnknown + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddIMUReading is faster than the date rate and succeeds, time to sleep is <= date rate", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return nil + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + + t.Run("when AddIMUReading is faster than the date rate and returns a lock error, time to sleep is <= date rate", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + + t.Run("when AddIMUReading is faster than date rate "+ + "and returns an unexpected error, time to sleep is <= date rate", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return errUnknown + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) +} + +func TestAddIMUReadingInOnline(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + + injectImu := inject.TimedMovementSensor{} + injectImu.NameFunc = func() string { return "good_imu" } + injectImu.DataFrequencyHzFunc = func() int { return 20 } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: true, + MovementSensor: &injectImu, + Timeout: 10 * time.Second, + } + + t.Run("returns error when IMU GetData returns error, doesn't try to add IMU data", func(t *testing.T) { + invalidOnlineModeIMUTestHelper(context.Background(), t, cf, config, 10, s.IMUWithErroringFunctions, 10) + }) + + t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { + invalidOnlineModeIMUTestHelper(context.Background(), t, cf, config, 10, s.InvalidReplayIMU, 10) + }) + + t.Run("online replay IMU adds sensor reading once and ignores errors", func(t *testing.T) { + onlineModeIMUTestHelper(context.Background(), t, config, cf, s.ReplayIMU) + }) + + t.Run("online IMU adds sensor reading once and ignores errors", func(t *testing.T) { + onlineModeIMUTestHelper(context.Background(), t, config, cf, s.GoodIMU) + }) +} + +func TestStartIMU(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return 5 } + + injectImu := inject.TimedMovementSensor{} + injectImu.NameFunc = func() string { return "good_imu" } + injectImu.DataFrequencyHzFunc = func() int { return 20 } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + MovementSensor: &injectImu, + Timeout: 10 * time.Second, + } + + t.Run("exits loop when the context was cancelled", func(t *testing.T) { + cancelCtx, cancelFunc := context.WithCancel(context.Background()) + + lidar, imu := s.NoLidar, s.FinishedReplayIMU + replaySensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(lidar, imu), string(imu), 20, logger) + test.That(t, err, test.ShouldBeNil) + + config.MovementSensor = replaySensor + + cancelFunc() + + config.StartMovementSensor(cancelCtx) + }) +} + +func TestTryAddIMUReadingUntilSuccess(t *testing.T) { + logger := logging.NewTestLogger(t) + ctx := context.Background() + + cf := cartofacade.Mock{} + + dataFrequencyHz := 0 + injectImu := inject.TimedMovementSensor{} + injectImu.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: false, + MovementSensor: &injectImu, + Timeout: 10 * time.Second, + } + + t.Run("replay IMU adds sensor data until success", func(t *testing.T) { + lidar, imu := s.NoLidar, s.ReplayIMU + replayIMU, err := s.NewMovementSensor(context.Background(), s.SetupDeps(lidar, imu), string(imu), dataFrequencyHz, logger) + test.That(t, err, test.ShouldBeNil) + + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + var calls []addIMUReadingArgs + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + args := addIMUReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + calls = append(calls, args) + if len(calls) == 1 { + return errUnknown + } + if len(calls) == 2 { + return cartofacade.ErrUnableToAcquireLock + } + return nil + } + config.MovementSensor = replayIMU + config.Lidar = &injectLidar + + config.tryAddMovementSensorReadingUntilSuccess(ctx, expectedMovementSensorReading) + test.That(t, len(calls), test.ShouldEqual, 3) + + firstTimestamp := calls[0].currentReading.ReadingTime + for i, call := range calls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, string(imu)) + test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, expectedIMUReading.LinearAcceleration) + test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, expectedIMUReading.AngularVelocity) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + } + }) +} diff --git a/sensorprocess/sensorprocess_test.go b/sensorprocess/sensorprocess_test.go index ea6ff0bf..0c38ef25 100644 --- a/sensorprocess/sensorprocess_test.go +++ b/sensorprocess/sensorprocess_test.go @@ -21,160 +21,6 @@ import ( "github.com/viamrobotics/viam-cartographer/sensors/inject" ) -func TestGetInitialMovementSensorReading(t *testing.T) { - logger := logging.NewTestLogger(t) - - lidarReading := s.TimedLidarReadingResponse{ - Reading: []byte("12345"), - ReadingTime: time.Now().UTC(), - } - - odometerReading := s.TimedOdometerReadingResponse{ - Position: geo.NewPoint(5, 4), - Orientation: &spatialmath.Quaternion{Real: 0.1, Imag: -0.2, Jmag: 2.5, Kmag: -9.1}, - ReadingTime: time.Now().UTC(), - } - - imuReading := s.TimedIMUReadingResponse{ - LinearAcceleration: r3.Vector{X: 1, Y: 2, Z: 3}, - AngularVelocity: spatialmath.AngularVelocity{X: 4, Y: 5, Z: 6}, - ReadingTime: time.Now().UTC(), - } - - cf := cartofacade.Mock{} - - injectLidar := inject.TimedLidar{} - injectLidar.NameFunc = func() string { return "good_lidar" } - injectLidar.DataFrequencyHzFunc = func() int { return 0 } - - injectMovementSensor := inject.TimedMovementSensor{} - injectMovementSensor.NameFunc = func() string { return "good_movement_sensor" } - injectMovementSensor.DataFrequencyHzFunc = func() int { return 0 } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, - Timeout: 10 * time.Second, - } - - t.Run("return error if movement sensor is not supported", func(t *testing.T) { - config.MovementSensor = nil - movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) - test.That(t, err, test.ShouldNotBeNil) - test.That(t, err, test.ShouldBeError, errors.New("movement sensor is not supported")) - test.That(t, movementSensorReading, test.ShouldResemble, s.TimedMovementSensorReadingResponse{}) - }) - - t.Run("return error if neither IMU nor odometer are supported", func(t *testing.T) { - injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { - return s.MovementSensorProperties{} - } - config.MovementSensor = &injectMovementSensor - movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) - test.That(t, err, test.ShouldNotBeNil) - test.That(t, err, test.ShouldBeError, errors.New("movement sensor is not supported")) - test.That(t, movementSensorReading, test.ShouldResemble, s.TimedMovementSensorReadingResponse{}) - }) - - t.Run("return error if TimedMovementSensorReading errors out", func(t *testing.T) { - expectedErr := errors.New("error in TimedMovementSensorReading") - injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { - return s.MovementSensorProperties{ - IMUSupported: true, - } - } - injectMovementSensor.TimedMovementSensorReadingFunc = func(ctx context.Context) (s.TimedMovementSensorReadingResponse, error) { - return s.TimedMovementSensorReadingResponse{}, expectedErr - } - config.MovementSensor = &injectMovementSensor - movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) - test.That(t, err, test.ShouldNotBeNil) - test.That(t, err, test.ShouldBeError, expectedErr) - test.That(t, movementSensorReading, test.ShouldResemble, s.TimedMovementSensorReadingResponse{}) - }) - - t.Run("successful data insertion", func(t *testing.T) { - cases := []struct { - description string - imuEnabled bool - odometerEnabled bool - lidarReadingTimeAddedMs int - msReadingTimeAddedMs []int - expectedMsReadingTimeAddedMs int - }{ - { - description: "skip movement sensor data until first lidar data is inserted", - imuEnabled: true, - odometerEnabled: true, - lidarReadingTimeAddedMs: 5, - msReadingTimeAddedMs: []int{1, 2, 3, 4, 6, 7, 8}, - expectedMsReadingTimeAddedMs: 6, - }, - { - description: "skip imu data until first lidar data is inserted", - imuEnabled: true, - odometerEnabled: false, - lidarReadingTimeAddedMs: 7, - msReadingTimeAddedMs: []int{1, 2, 3, 4, 5, 6, 7, 8}, - expectedMsReadingTimeAddedMs: 7, - }, - { - description: "skip odometer data until first lidar data is inserted", - imuEnabled: false, - odometerEnabled: true, - lidarReadingTimeAddedMs: 3, - msReadingTimeAddedMs: []int{1, 2, 5, 6, 7, 8}, - expectedMsReadingTimeAddedMs: 5, - }, - } - - for _, tt := range cases { - t.Run(tt.description, func(t *testing.T) { - now := time.Now().UTC() - - if tt.imuEnabled || tt.odometerEnabled { - config.MovementSensor = &injectMovementSensor - } else { - config.MovementSensor = nil - } - - lidarReading.ReadingTime = now.Add(time.Duration(tt.lidarReadingTimeAddedMs) * time.Millisecond) - - numMovementSensorData := 0 - injectMovementSensor.TimedMovementSensorReadingFunc = func(ctx context.Context) (s.TimedMovementSensorReadingResponse, error) { - var movementSensorReading s.TimedMovementSensorReadingResponse - if numMovementSensorData < len(tt.msReadingTimeAddedMs) { - if tt.odometerEnabled { - movementSensorReading.TimedOdometerResponse = &odometerReading - movementSensorReading.TimedOdometerResponse.ReadingTime = now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) - } - if tt.imuEnabled { - movementSensorReading.TimedIMUResponse = &imuReading - movementSensorReading.TimedIMUResponse.ReadingTime = now.Add(time.Duration(float64(tt.msReadingTimeAddedMs[numMovementSensorData])+0.1) * time.Millisecond) - } - numMovementSensorData++ - return movementSensorReading, nil - } - return movementSensorReading, replay.ErrEndOfDataset - } - injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { - return s.MovementSensorProperties{ - IMUSupported: tt.imuEnabled, - OdometerSupported: tt.odometerEnabled, - } - } - - movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) - test.That(t, err, test.ShouldBeNil) - test.That(t, movementSensorReading, test.ShouldNotResemble, s.TimedMovementSensorReadingResponse{}) - test.That(t, tt.msReadingTimeAddedMs[numMovementSensorData-1], test.ShouldResemble, tt.expectedMsReadingTimeAddedMs) - }) - } - }) -} - func TestStartOfflineSensorProcess(t *testing.T) { logger := logging.NewTestLogger(t) @@ -460,158 +306,35 @@ func TestStartOfflineSensorProcess(t *testing.T) { }) } -func TestTryAddLidarReading(t *testing.T) { +func TestGetInitialMovementSensorReading(t *testing.T) { logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - reading := s.TimedLidarReadingResponse{ + + lidarReading := s.TimedLidarReadingResponse{ Reading: []byte("12345"), ReadingTime: time.Now().UTC(), } - dataFrequencyHz := 5 - injectLidar := inject.TimedLidar{} - injectLidar.NameFunc = func() string { return "good_lidar" } - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, - Timeout: 10 * time.Second, + odometerReading := s.TimedOdometerReadingResponse{ + Position: geo.NewPoint(5, 4), + Orientation: &spatialmath.Quaternion{Real: 0.1, Imag: -0.2, Jmag: 2.5, Kmag: -9.1}, + ReadingTime: time.Now().UTC(), } - t.Run("when AddLidarReading blocks for more than the data rate and succeeds, time to sleep is 0", func(t *testing.T) { - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return nil - } - - timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - t.Run("when AddLidarReading is slower than data rate and returns a lock error, time to sleep is 0", func(t *testing.T) { - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return cartofacade.ErrUnableToAcquireLock - } - - timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - - t.Run("when AddLidarReading blocks for more than the date rate "+ - "and returns an unexpected error, time to sleep is 0", func(t *testing.T) { - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return errUnknown - } - - timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - - t.Run("when AddLidarReading is faster than the date rate and succeeds, time to sleep is <= date rate", func(t *testing.T) { - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - return nil - } - - timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.Lidar.DataFrequencyHz()) - }) - - t.Run("when AddLidarReading is faster than the date rate "+ - "and returns lock error, time to sleep is <= date rate", func(t *testing.T) { - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - return cartofacade.ErrUnableToAcquireLock - } - - timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.Lidar.DataFrequencyHz()) - }) - - t.Run("when AddLidarReading is faster than date rate "+ - "and returns an unexpected error, time to sleep is <= date rate", func(t *testing.T) { - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - return errUnknown - } - - timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.Lidar.DataFrequencyHz()) - }) -} - -func TestAddLidarReadingInOnline(t *testing.T) { - logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - - dataFrequencyHz := 5 - injectLidar := inject.TimedLidar{} - injectLidar.NameFunc = func() string { return "good_lidar" } - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, - Timeout: 10 * time.Second, + imuReading := s.TimedIMUReadingResponse{ + LinearAcceleration: r3.Vector{X: 1, Y: 2, Z: 3}, + AngularVelocity: spatialmath.AngularVelocity{X: 4, Y: 5, Z: 6}, + ReadingTime: time.Now().UTC(), } - t.Run("returns error when lidar GetData returns error, doesn't try to add lidar data", func(t *testing.T) { - invalidOnlineModeLidarTestHelper(context.Background(), t, cf, config, s.LidarWithErroringFunctions, 10) - }) - - t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { - invalidOnlineModeLidarTestHelper(context.Background(), t, cf, config, s.InvalidReplayLidar, 10) - }) - - t.Run("online lidar adds sensor reading once and ignores errors", func(t *testing.T) { - onlineModeLidarTestHelper(context.Background(), t, config, cf, s.GoodLidar) - }) -} - -func TestStartLidar(t *testing.T) { - logger := logging.NewTestLogger(t) cf := cartofacade.Mock{} - dataFrequencyHz := 5 injectLidar := inject.TimedLidar{} injectLidar.NameFunc = func() string { return "good_lidar" } - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + injectLidar.DataFrequencyHzFunc = func() int { return 0 } + + injectMovementSensor := inject.TimedMovementSensor{} + injectMovementSensor.NameFunc = func() string { return "good_movement_sensor" } + injectMovementSensor.DataFrequencyHzFunc = func() int { return 0 } config := Config{ Logger: logger, @@ -621,343 +344,118 @@ func TestStartLidar(t *testing.T) { Timeout: 10 * time.Second, } - t.Run("exits loop when the context was cancelled", func(t *testing.T) { - cancelCtx, cancelFunc := context.WithCancel(context.Background()) - - lidar, imu := s.FinishedReplayLidar, s.NoMovementSensor - replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) - test.That(t, err, test.ShouldBeNil) - - config.Lidar = replaySensor - - cancelFunc() - - config.StartLidar(cancelCtx) - }) -} - -func TestTryAddIMUReading(t *testing.T) { - logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - imuReading := s.TimedIMUReadingResponse{ - LinearAcceleration: r3.Vector{X: 1, Y: 1, Z: 1}, - AngularVelocity: spatialmath.AngularVelocity{X: 1, Y: 1, Z: 1}, - ReadingTime: time.Now().UTC(), - } - - reading := s.TimedMovementSensorReadingResponse{ - TimedIMUResponse: &imuReading, - } - - injectLidar := inject.TimedLidar{} - injectLidar.DataFrequencyHzFunc = func() int { return 5 } - - injectImu := inject.TimedMovementSensor{} - injectImu.NameFunc = func() string { return "good_imu" } - injectImu.DataFrequencyHzFunc = func() int { return 20 } - injectImu.PropertiesFunc = func() s.MovementSensorProperties { - return s.MovementSensorProperties{ - IMUSupported: true, - } - } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, - MovementSensor: &injectImu, - Timeout: 10 * time.Second, - } - - t.Run("when AddIMUReading blocks for more than the date rate and succeeds, time to sleep is 0", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return nil - } - - timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - - t.Run("when AddIMUReading is slower than date rate and returns a lock error, time to sleep is 0", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return cartofacade.ErrUnableToAcquireLock - } - - timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - - t.Run("when AddIMUReading blocks for more than the date rate and returns an unexpected error, time to sleep is 0", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return errUnknown - } - - timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - - t.Run("when AddIMUReading is faster than the date rate and succeeds, time to sleep is <= date rate", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - return nil - } - - timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) - }) - - t.Run("when AddIMUReading is faster than the date rate and returns a lock error, time to sleep is <= date rate", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - return cartofacade.ErrUnableToAcquireLock - } - - timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + t.Run("return error if movement sensor is not supported", func(t *testing.T) { + config.MovementSensor = nil + movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, errors.New("movement sensor is not supported")) + test.That(t, movementSensorReading, test.ShouldResemble, s.TimedMovementSensorReadingResponse{}) }) - t.Run("when AddIMUReading is faster than date rate "+ - "and returns an unexpected error, time to sleep is <= date rate", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - return errUnknown + t.Run("return error if neither IMU nor odometer are supported", func(t *testing.T) { + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{} } - - timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) - }) -} - -func TestAddIMUReadingInOnline(t *testing.T) { - logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - - injectImu := inject.TimedMovementSensor{} - injectImu.NameFunc = func() string { return "good_imu" } - injectImu.DataFrequencyHzFunc = func() int { return 20 } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: true, - MovementSensor: &injectImu, - Timeout: 10 * time.Second, - } - - t.Run("returns error when IMU GetData returns error, doesn't try to add IMU data", func(t *testing.T) { - invalidOnlineModeIMUTestHelper(context.Background(), t, cf, config, 10, s.IMUWithErroringFunctions, 10) - }) - - t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { - invalidOnlineModeIMUTestHelper(context.Background(), t, cf, config, 10, s.InvalidReplayIMU, 10) - }) - - t.Run("online replay IMU adds sensor reading once and ignores errors", func(t *testing.T) { - onlineModeIMUTestHelper(context.Background(), t, config, cf, s.ReplayIMU) - }) - - t.Run("online IMU adds sensor reading once and ignores errors", func(t *testing.T) { - onlineModeIMUTestHelper(context.Background(), t, config, cf, s.GoodIMU) - }) -} - -func TestStartIMU(t *testing.T) { - logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - - injectLidar := inject.TimedLidar{} - injectLidar.DataFrequencyHzFunc = func() int { return 5 } - - injectImu := inject.TimedMovementSensor{} - injectImu.NameFunc = func() string { return "good_imu" } - injectImu.DataFrequencyHzFunc = func() int { return 20 } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, - MovementSensor: &injectImu, - Timeout: 10 * time.Second, - } - - t.Run("exits loop when the context was cancelled", func(t *testing.T) { - cancelCtx, cancelFunc := context.WithCancel(context.Background()) - - lidar, imu := s.NoLidar, s.FinishedReplayIMU - replaySensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(lidar, imu), string(imu), 20, logger) - test.That(t, err, test.ShouldBeNil) - - config.MovementSensor = replaySensor - - cancelFunc() - - config.StartMovementSensor(cancelCtx) + config.MovementSensor = &injectMovementSensor + movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, errors.New("movement sensor is not supported")) + test.That(t, movementSensorReading, test.ShouldResemble, s.TimedMovementSensorReadingResponse{}) }) -} - -func TestTryAddLidarReadingUntilSuccess(t *testing.T) { - logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - cf.RunFinalOptimizationFunc = func(context.Context, time.Duration) error { - return nil - } - - dataFrequencyHz := 0 - - lidarReading := s.TimedLidarReadingResponse{ - Reading: []byte("12345"), - ReadingTime: time.Now().UTC(), - } - - injectLidar := inject.TimedLidar{} - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: false, - Lidar: &injectLidar, - Timeout: 10 * time.Second, - } - - t.Run("replay lidar adds sensor data until success", func(t *testing.T) { - lidar, imu := s.ReplayLidar, s.NoMovementSensor - replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) - test.That(t, err, test.ShouldBeNil) - - var calls []addLidarReadingArgs - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - args := addLidarReadingArgs{ - timeout: timeout, - sensorName: sensorName, - currentReading: currentReading, - } - calls = append(calls, args) - if len(calls) == 1 { - return errUnknown - } - if len(calls) == 2 { - return cartofacade.ErrUnableToAcquireLock + t.Run("return error if TimedMovementSensorReading errors out", func(t *testing.T) { + expectedErr := errors.New("error in TimedMovementSensorReading") + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, } - return nil } - config.Lidar = replaySensor - - config.tryAddLidarReadingUntilSuccess(context.Background(), lidarReading) - test.That(t, len(calls), test.ShouldEqual, 3) - - firstTimestamp := calls[0].currentReading.ReadingTime - for i, call := range calls { - t.Logf("call %d", i) - test.That(t, call.sensorName, test.ShouldResemble, string(lidar)) - test.That(t, call.currentReading.Reading, test.ShouldResemble, lidarReading.Reading) - test.That(t, call.timeout, test.ShouldEqual, config.Timeout) - test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + injectMovementSensor.TimedMovementSensorReadingFunc = func(ctx context.Context) (s.TimedMovementSensorReadingResponse, error) { + return s.TimedMovementSensorReadingResponse{}, expectedErr } + config.MovementSensor = &injectMovementSensor + movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, expectedErr) + test.That(t, movementSensorReading, test.ShouldResemble, s.TimedMovementSensorReadingResponse{}) }) -} -func TestTryAddIMUReadingUntilSuccess(t *testing.T) { - logger := logging.NewTestLogger(t) - ctx := context.Background() - - cf := cartofacade.Mock{} - - dataFrequencyHz := 0 - injectImu := inject.TimedMovementSensor{} - injectImu.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + t.Run("successful data insertion", func(t *testing.T) { + cases := []struct { + description string + imuEnabled bool + odometerEnabled bool + lidarReadingTimeAddedMs int + msReadingTimeAddedMs []int + expectedMsReadingTimeAddedMs int + }{ + { + description: "skip movement sensor data until first lidar data is inserted", + imuEnabled: true, + odometerEnabled: true, + lidarReadingTimeAddedMs: 5, + msReadingTimeAddedMs: []int{1, 2, 3, 4, 6, 7, 8}, + expectedMsReadingTimeAddedMs: 6, + }, + { + description: "skip imu data until first lidar data is inserted", + imuEnabled: true, + odometerEnabled: false, + lidarReadingTimeAddedMs: 7, + msReadingTimeAddedMs: []int{1, 2, 3, 4, 5, 6, 7, 8}, + expectedMsReadingTimeAddedMs: 7, + }, + { + description: "skip odometer data until first lidar data is inserted", + imuEnabled: false, + odometerEnabled: true, + lidarReadingTimeAddedMs: 3, + msReadingTimeAddedMs: []int{1, 2, 5, 6, 7, 8}, + expectedMsReadingTimeAddedMs: 5, + }, + } - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: false, - MovementSensor: &injectImu, - Timeout: 10 * time.Second, - } + for _, tt := range cases { + t.Run(tt.description, func(t *testing.T) { + now := time.Now().UTC() - t.Run("replay IMU adds sensor data until success", func(t *testing.T) { - lidar, imu := s.NoLidar, s.ReplayIMU - replayIMU, err := s.NewMovementSensor(context.Background(), s.SetupDeps(lidar, imu), string(imu), dataFrequencyHz, logger) - test.That(t, err, test.ShouldBeNil) + if tt.imuEnabled || tt.odometerEnabled { + config.MovementSensor = &injectMovementSensor + } else { + config.MovementSensor = nil + } - injectLidar := inject.TimedLidar{} - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + lidarReading.ReadingTime = now.Add(time.Duration(tt.lidarReadingTimeAddedMs) * time.Millisecond) - var calls []addIMUReadingArgs - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - args := addIMUReadingArgs{ - timeout: timeout, - sensorName: sensorName, - currentReading: currentReading, - } - calls = append(calls, args) - if len(calls) == 1 { - return errUnknown - } - if len(calls) == 2 { - return cartofacade.ErrUnableToAcquireLock - } - return nil - } - config.MovementSensor = replayIMU - config.Lidar = &injectLidar + numMovementSensorData := 0 + injectMovementSensor.TimedMovementSensorReadingFunc = func(ctx context.Context) (s.TimedMovementSensorReadingResponse, error) { + var movementSensorReading s.TimedMovementSensorReadingResponse + if numMovementSensorData < len(tt.msReadingTimeAddedMs) { + if tt.odometerEnabled { + movementSensorReading.TimedOdometerResponse = &odometerReading + movementSensorReading.TimedOdometerResponse.ReadingTime = now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) + } + if tt.imuEnabled { + movementSensorReading.TimedIMUResponse = &imuReading + movementSensorReading.TimedIMUResponse.ReadingTime = now.Add(time.Duration(float64(tt.msReadingTimeAddedMs[numMovementSensorData])+0.1) * time.Millisecond) + } + numMovementSensorData++ + return movementSensorReading, nil + } + return movementSensorReading, replay.ErrEndOfDataset + } + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: tt.imuEnabled, + OdometerSupported: tt.odometerEnabled, + } + } - config.tryAddMovementSensorReadingUntilSuccess(ctx, expectedMovementSensorReading) - test.That(t, len(calls), test.ShouldEqual, 3) - - firstTimestamp := calls[0].currentReading.ReadingTime - for i, call := range calls { - t.Logf("call %d", i) - test.That(t, call.sensorName, test.ShouldResemble, string(imu)) - test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, expectedIMUReading.LinearAcceleration) - test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, expectedIMUReading.AngularVelocity) - test.That(t, call.timeout, test.ShouldEqual, config.Timeout) - test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) + test.That(t, err, test.ShouldBeNil) + test.That(t, movementSensorReading, test.ShouldNotResemble, s.TimedMovementSensorReadingResponse{}) + test.That(t, tt.msReadingTimeAddedMs[numMovementSensorData-1], test.ShouldResemble, tt.expectedMsReadingTimeAddedMs) + }) } }) } From 2305bbd62dfe7f3d0b8b0a4302128d8dbe45397d Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Mon, 11 Dec 2023 12:49:14 -0500 Subject: [PATCH 08/21] Add tests --- sensorprocess/lidarsensorprocess_test.go | 250 ++++++++++------- sensorprocess/movementsensorprocess_test.go | 294 ++++++++++++++------ 2 files changed, 360 insertions(+), 184 deletions(-) diff --git a/sensorprocess/lidarsensorprocess_test.go b/sensorprocess/lidarsensorprocess_test.go index 68a1fd4c..34001162 100644 --- a/sensorprocess/lidarsensorprocess_test.go +++ b/sensorprocess/lidarsensorprocess_test.go @@ -2,6 +2,7 @@ package sensorprocess import ( "context" + "errors" "testing" "time" @@ -13,7 +14,136 @@ import ( "github.com/viamrobotics/viam-cartographer/sensors/inject" ) -func TestTryAddLidarReading(t *testing.T) { +func TestStartLidar(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + + dataFrequencyHz := 5 + injectLidar := inject.TimedLidar{} + injectLidar.NameFunc = func() string { return "good_lidar" } + injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + Timeout: 10 * time.Second, + } + + t.Run("exits loop when the context was cancelled", func(t *testing.T) { + cancelCtx, cancelFunc := context.WithCancel(context.Background()) + + lidar, imu := s.FinishedReplayLidar, s.NoMovementSensor + replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) + test.That(t, err, test.ShouldBeNil) + + config.Lidar = replaySensor + + cancelFunc() + + config.StartLidar(cancelCtx) + }) +} + +func TestAddLidarReadingInOnline(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + + dataFrequencyHz := 5 + injectLidar := inject.TimedLidar{} + injectLidar.NameFunc = func() string { return "good_lidar" } + injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + Timeout: 10 * time.Second, + } + + t.Run("returns error when lidar GetData returns error, doesn't try to add lidar data", func(t *testing.T) { + invalidOnlineModeLidarTestHelper(context.Background(), t, cf, config, s.LidarWithErroringFunctions, 10) + }) + + t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { + invalidOnlineModeLidarTestHelper(context.Background(), t, cf, config, s.InvalidReplayLidar, 10) + }) + + t.Run("online lidar adds sensor reading once and ignores errors", func(t *testing.T) { + onlineModeLidarTestHelper(context.Background(), t, config, cf, s.GoodLidar) + }) +} + +func TestTryAddLidarReadingUntilSuccess(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + cf.RunFinalOptimizationFunc = func(context.Context, time.Duration) error { + return nil + } + + dataFrequencyHz := 0 + + lidarReading := s.TimedLidarReadingResponse{ + Reading: []byte("12345"), + ReadingTime: time.Now().UTC(), + } + + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: false, + Lidar: &injectLidar, + Timeout: 10 * time.Second, + } + + t.Run("replay lidar adds sensor data until success", func(t *testing.T) { + lidar, imu := s.ReplayLidar, s.NoMovementSensor + replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) + test.That(t, err, test.ShouldBeNil) + + var calls []addLidarReadingArgs + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + args := addLidarReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + calls = append(calls, args) + if len(calls) == 1 { + return errUnknown + } + if len(calls) == 2 { + return cartofacade.ErrUnableToAcquireLock + } + return nil + } + config.Lidar = replaySensor + + config.tryAddLidarReadingUntilSuccess(context.Background(), lidarReading) + test.That(t, len(calls), test.ShouldEqual, 3) + + firstTimestamp := calls[0].currentReading.ReadingTime + for i, call := range calls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, string(lidar)) + test.That(t, call.currentReading.Reading, test.ShouldResemble, lidarReading.Reading) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + } + }) +} + +func TestTryAddLidarReadingOnce(t *testing.T) { logger := logging.NewTestLogger(t) cf := cartofacade.Mock{} reading := s.TimedLidarReadingResponse{ @@ -127,40 +257,14 @@ func TestTryAddLidarReading(t *testing.T) { }) } -func TestAddLidarReadingInOnline(t *testing.T) { +func TestTryAddLidarReading(t *testing.T) { logger := logging.NewTestLogger(t) cf := cartofacade.Mock{} - - dataFrequencyHz := 5 - injectLidar := inject.TimedLidar{} - injectLidar.NameFunc = func() string { return "good_lidar" } - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, - Timeout: 10 * time.Second, + reading := s.TimedLidarReadingResponse{ + Reading: []byte("12345"), + ReadingTime: time.Now().UTC(), } - t.Run("returns error when lidar GetData returns error, doesn't try to add lidar data", func(t *testing.T) { - invalidOnlineModeLidarTestHelper(context.Background(), t, cf, config, s.LidarWithErroringFunctions, 10) - }) - - t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { - invalidOnlineModeLidarTestHelper(context.Background(), t, cf, config, s.InvalidReplayLidar, 10) - }) - - t.Run("online lidar adds sensor reading once and ignores errors", func(t *testing.T) { - onlineModeLidarTestHelper(context.Background(), t, config, cf, s.GoodLidar) - }) -} - -func TestStartLidar(t *testing.T) { - logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - dataFrequencyHz := 5 injectLidar := inject.TimedLidar{} injectLidar.NameFunc = func() string { return "good_lidar" } @@ -173,85 +277,33 @@ func TestStartLidar(t *testing.T) { Lidar: &injectLidar, Timeout: 10 * time.Second, } + t.Run("return error when AddLidarReading errors out", func(t *testing.T) { + expectedErr := errors.New("failed to get lidar reading") + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + return expectedErr + } - t.Run("exits loop when the context was cancelled", func(t *testing.T) { - cancelCtx, cancelFunc := context.WithCancel(context.Background()) - - lidar, imu := s.FinishedReplayLidar, s.NoMovementSensor - replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) - test.That(t, err, test.ShouldBeNil) - - config.Lidar = replaySensor - - cancelFunc() - - config.StartLidar(cancelCtx) + err := config.tryAddLidarReading(context.Background(), reading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, expectedErr) }) -} - -func TestTryAddLidarReadingUntilSuccess(t *testing.T) { - logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - cf.RunFinalOptimizationFunc = func(context.Context, time.Duration) error { - return nil - } - - dataFrequencyHz := 0 - - lidarReading := s.TimedLidarReadingResponse{ - Reading: []byte("12345"), - ReadingTime: time.Now().UTC(), - } - - injectLidar := inject.TimedLidar{} - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: false, - Lidar: &injectLidar, - Timeout: 10 * time.Second, - } - - t.Run("replay lidar adds sensor data until success", func(t *testing.T) { - lidar, imu := s.ReplayLidar, s.NoMovementSensor - replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) - test.That(t, err, test.ShouldBeNil) - var calls []addLidarReadingArgs + t.Run("succeeds when AddLidarReading succeeds", func(t *testing.T) { cf.AddLidarReadingFunc = func( ctx context.Context, timeout time.Duration, sensorName string, currentReading s.TimedLidarReadingResponse, ) error { - args := addLidarReadingArgs{ - timeout: timeout, - sensorName: sensorName, - currentReading: currentReading, - } - calls = append(calls, args) - if len(calls) == 1 { - return errUnknown - } - if len(calls) == 2 { - return cartofacade.ErrUnableToAcquireLock - } return nil } - config.Lidar = replaySensor - - config.tryAddLidarReadingUntilSuccess(context.Background(), lidarReading) - test.That(t, len(calls), test.ShouldEqual, 3) - firstTimestamp := calls[0].currentReading.ReadingTime - for i, call := range calls { - t.Logf("call %d", i) - test.That(t, call.sensorName, test.ShouldResemble, string(lidar)) - test.That(t, call.currentReading.Reading, test.ShouldResemble, lidarReading.Reading) - test.That(t, call.timeout, test.ShouldEqual, config.Timeout) - test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) - } + err := config.tryAddLidarReading(context.Background(), reading) + test.That(t, err, test.ShouldBeNil) }) } diff --git a/sensorprocess/movementsensorprocess_test.go b/sensorprocess/movementsensorprocess_test.go index 9d2a632a..ca93253b 100644 --- a/sensorprocess/movementsensorprocess_test.go +++ b/sensorprocess/movementsensorprocess_test.go @@ -2,10 +2,12 @@ package sensorprocess import ( "context" + "errors" "testing" "time" "github.com/golang/geo/r3" + geo "github.com/kellydunn/golang-geo" "go.viam.com/rdk/logging" "go.viam.com/rdk/spatialmath" "go.viam.com/test" @@ -15,7 +17,140 @@ import ( "github.com/viamrobotics/viam-cartographer/sensors/inject" ) -func TestTryAddIMUReading(t *testing.T) { +func TestStartMovementSensor(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return 5 } + + injectImu := inject.TimedMovementSensor{} + injectImu.NameFunc = func() string { return "good_imu" } + injectImu.DataFrequencyHzFunc = func() int { return 20 } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + MovementSensor: &injectImu, + Timeout: 10 * time.Second, + } + + t.Run("exits loop when the context was cancelled", func(t *testing.T) { + cancelCtx, cancelFunc := context.WithCancel(context.Background()) + + lidar, imu := s.NoLidar, s.FinishedReplayIMU + replaySensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(lidar, imu), string(imu), 20, logger) + test.That(t, err, test.ShouldBeNil) + + config.MovementSensor = replaySensor + + cancelFunc() + + config.StartMovementSensor(cancelCtx) + }) +} + +func TestAddMovementSensorReadingInOnline(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + + injectImu := inject.TimedMovementSensor{} + injectImu.NameFunc = func() string { return "good_imu" } + injectImu.DataFrequencyHzFunc = func() int { return 20 } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: true, + MovementSensor: &injectImu, + Timeout: 10 * time.Second, + } + + t.Run("returns error when IMU GetData returns error, doesn't try to add IMU data", func(t *testing.T) { + invalidOnlineModeIMUTestHelper(context.Background(), t, cf, config, 10, s.IMUWithErroringFunctions, 10) + }) + + t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { + invalidOnlineModeIMUTestHelper(context.Background(), t, cf, config, 10, s.InvalidReplayIMU, 10) + }) + + t.Run("online replay IMU adds sensor reading once and ignores errors", func(t *testing.T) { + onlineModeIMUTestHelper(context.Background(), t, config, cf, s.ReplayIMU) + }) + + t.Run("online IMU adds sensor reading once and ignores errors", func(t *testing.T) { + onlineModeIMUTestHelper(context.Background(), t, config, cf, s.GoodIMU) + }) +} + +func TestTryAddMovementSensorReadingUntilSuccess(t *testing.T) { + logger := logging.NewTestLogger(t) + ctx := context.Background() + + cf := cartofacade.Mock{} + + dataFrequencyHz := 0 + injectImu := inject.TimedMovementSensor{} + injectImu.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: false, + MovementSensor: &injectImu, + Timeout: 10 * time.Second, + } + + t.Run("replay IMU adds sensor data until success", func(t *testing.T) { + lidar, imu := s.NoLidar, s.ReplayIMU + replayIMU, err := s.NewMovementSensor(context.Background(), s.SetupDeps(lidar, imu), string(imu), dataFrequencyHz, logger) + test.That(t, err, test.ShouldBeNil) + + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + var calls []addIMUReadingArgs + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + args := addIMUReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + calls = append(calls, args) + if len(calls) == 1 { + return errUnknown + } + if len(calls) == 2 { + return cartofacade.ErrUnableToAcquireLock + } + return nil + } + config.MovementSensor = replayIMU + config.Lidar = &injectLidar + + config.tryAddMovementSensorReadingUntilSuccess(ctx, expectedMovementSensorReading) + test.That(t, len(calls), test.ShouldEqual, 3) + + firstTimestamp := calls[0].currentReading.ReadingTime + for i, call := range calls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, string(imu)) + test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, expectedIMUReading.LinearAcceleration) + test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, expectedIMUReading.AngularVelocity) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + } + }) +} + +func TestTryAddMovementSensorReadingOnce(t *testing.T) { logger := logging.NewTestLogger(t) cf := cartofacade.Mock{} imuReading := s.TimedIMUReadingResponse{ @@ -141,135 +276,124 @@ func TestTryAddIMUReading(t *testing.T) { }) } -func TestAddIMUReadingInOnline(t *testing.T) { +func TestTryAddIMUReading(t *testing.T) { logger := logging.NewTestLogger(t) cf := cartofacade.Mock{} + imuReading := s.TimedIMUReadingResponse{ + LinearAcceleration: r3.Vector{X: 1, Y: 1, Z: 1}, + AngularVelocity: spatialmath.AngularVelocity{X: 1, Y: 1, Z: 1}, + ReadingTime: time.Now().UTC(), + } + + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return 5 } injectImu := inject.TimedMovementSensor{} injectImu.NameFunc = func() string { return "good_imu" } injectImu.DataFrequencyHzFunc = func() int { return 20 } + injectImu.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + } + } config := Config{ Logger: logger, CartoFacade: &cf, - IsOnline: true, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, MovementSensor: &injectImu, Timeout: 10 * time.Second, } - t.Run("returns error when IMU GetData returns error, doesn't try to add IMU data", func(t *testing.T) { - invalidOnlineModeIMUTestHelper(context.Background(), t, cf, config, 10, s.IMUWithErroringFunctions, 10) - }) + t.Run("return error when AddIMUReading errors out", func(t *testing.T) { + expectedErr := errors.New("failed to get imu reading") + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return expectedErr + } - t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { - invalidOnlineModeIMUTestHelper(context.Background(), t, cf, config, 10, s.InvalidReplayIMU, 10) + err := config.tryAddIMUReading(context.Background(), imuReading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, expectedErr) }) - t.Run("online replay IMU adds sensor reading once and ignores errors", func(t *testing.T) { - onlineModeIMUTestHelper(context.Background(), t, config, cf, s.ReplayIMU) - }) + t.Run("succeeds when AddIMUReading succeeds", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return nil + } - t.Run("online IMU adds sensor reading once and ignores errors", func(t *testing.T) { - onlineModeIMUTestHelper(context.Background(), t, config, cf, s.GoodIMU) + err := config.tryAddIMUReading(context.Background(), imuReading) + test.That(t, err, test.ShouldBeNil) }) } -func TestStartIMU(t *testing.T) { +func TestTryAddOdometerReading(t *testing.T) { logger := logging.NewTestLogger(t) cf := cartofacade.Mock{} + odometerReading := s.TimedOdometerReadingResponse{ + Position: geo.NewPoint(5, 4), + Orientation: &spatialmath.Quaternion{Real: 0.1, Imag: -0.2, Jmag: 2.5, Kmag: -9.1}, + ReadingTime: time.Now().UTC(), + } injectLidar := inject.TimedLidar{} injectLidar.DataFrequencyHzFunc = func() int { return 5 } - injectImu := inject.TimedMovementSensor{} - injectImu.NameFunc = func() string { return "good_imu" } - injectImu.DataFrequencyHzFunc = func() int { return 20 } + injectOdometer := inject.TimedMovementSensor{} + injectOdometer.NameFunc = func() string { return "good_odometer" } + injectOdometer.DataFrequencyHzFunc = func() int { return 20 } + injectOdometer.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + OdometerSupported: true, + } + } config := Config{ Logger: logger, CartoFacade: &cf, IsOnline: injectLidar.DataFrequencyHzFunc() != 0, Lidar: &injectLidar, - MovementSensor: &injectImu, + MovementSensor: &injectOdometer, Timeout: 10 * time.Second, } - t.Run("exits loop when the context was cancelled", func(t *testing.T) { - cancelCtx, cancelFunc := context.WithCancel(context.Background()) - - lidar, imu := s.NoLidar, s.FinishedReplayIMU - replaySensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(lidar, imu), string(imu), 20, logger) - test.That(t, err, test.ShouldBeNil) - - config.MovementSensor = replaySensor - - cancelFunc() + t.Run("return error when AddOdometerReading errors out", func(t *testing.T) { + expectedErr := errors.New("failed to get odometer reading") + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + return expectedErr + } - config.StartMovementSensor(cancelCtx) + err := config.tryAddOdometerReading(context.Background(), odometerReading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, expectedErr) }) -} - -func TestTryAddIMUReadingUntilSuccess(t *testing.T) { - logger := logging.NewTestLogger(t) - ctx := context.Background() - - cf := cartofacade.Mock{} - - dataFrequencyHz := 0 - injectImu := inject.TimedMovementSensor{} - injectImu.DataFrequencyHzFunc = func() int { return dataFrequencyHz } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: false, - MovementSensor: &injectImu, - Timeout: 10 * time.Second, - } - - t.Run("replay IMU adds sensor data until success", func(t *testing.T) { - lidar, imu := s.NoLidar, s.ReplayIMU - replayIMU, err := s.NewMovementSensor(context.Background(), s.SetupDeps(lidar, imu), string(imu), dataFrequencyHz, logger) - test.That(t, err, test.ShouldBeNil) - injectLidar := inject.TimedLidar{} - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } - - var calls []addIMUReadingArgs - cf.AddIMUReadingFunc = func( + t.Run("succeeds when AddOdometerReading succeeds", func(t *testing.T) { + cf.AddOdometerReadingFunc = func( ctx context.Context, timeout time.Duration, sensorName string, - currentReading s.TimedIMUReadingResponse, + currentReading s.TimedOdometerReadingResponse, ) error { - args := addIMUReadingArgs{ - timeout: timeout, - sensorName: sensorName, - currentReading: currentReading, - } - calls = append(calls, args) - if len(calls) == 1 { - return errUnknown - } - if len(calls) == 2 { - return cartofacade.ErrUnableToAcquireLock - } return nil } - config.MovementSensor = replayIMU - config.Lidar = &injectLidar - - config.tryAddMovementSensorReadingUntilSuccess(ctx, expectedMovementSensorReading) - test.That(t, len(calls), test.ShouldEqual, 3) - firstTimestamp := calls[0].currentReading.ReadingTime - for i, call := range calls { - t.Logf("call %d", i) - test.That(t, call.sensorName, test.ShouldResemble, string(imu)) - test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, expectedIMUReading.LinearAcceleration) - test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, expectedIMUReading.AngularVelocity) - test.That(t, call.timeout, test.ShouldEqual, config.Timeout) - test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) - } + err := config.tryAddOdometerReading(context.Background(), odometerReading) + test.That(t, err, test.ShouldBeNil) }) } From 19bd38c71d221ae298068c6116c3c8609f7372a3 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Mon, 11 Dec 2023 14:26:13 -0500 Subject: [PATCH 09/21] Rename helper functions --- sensorprocess/lidarsensorprocess_test.go | 16 ++++------ sensorprocess/movementsensorprocess_test.go | 33 +++++++++------------ sensorprocess/testhelper.go | 8 ++--- 3 files changed, 23 insertions(+), 34 deletions(-) diff --git a/sensorprocess/lidarsensorprocess_test.go b/sensorprocess/lidarsensorprocess_test.go index 34001162..570a0008 100644 --- a/sensorprocess/lidarsensorprocess_test.go +++ b/sensorprocess/lidarsensorprocess_test.go @@ -18,16 +18,10 @@ func TestStartLidar(t *testing.T) { logger := logging.NewTestLogger(t) cf := cartofacade.Mock{} - dataFrequencyHz := 5 - injectLidar := inject.TimedLidar{} - injectLidar.NameFunc = func() string { return "good_lidar" } - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } - config := Config{ Logger: logger, CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, + IsOnline: true, Timeout: 10 * time.Second, } @@ -35,7 +29,7 @@ func TestStartLidar(t *testing.T) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) lidar, imu := s.FinishedReplayLidar, s.NoMovementSensor - replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) + replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), 5, logger) test.That(t, err, test.ShouldBeNil) config.Lidar = replaySensor @@ -64,15 +58,15 @@ func TestAddLidarReadingInOnline(t *testing.T) { } t.Run("returns error when lidar GetData returns error, doesn't try to add lidar data", func(t *testing.T) { - invalidOnlineModeLidarTestHelper(context.Background(), t, cf, config, s.LidarWithErroringFunctions, 10) + invalidAddLidarReadingInOnlineTestHelper(context.Background(), t, cf, config, s.LidarWithErroringFunctions, 10) }) t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { - invalidOnlineModeLidarTestHelper(context.Background(), t, cf, config, s.InvalidReplayLidar, 10) + invalidAddLidarReadingInOnlineTestHelper(context.Background(), t, cf, config, s.InvalidReplayLidar, 10) }) t.Run("online lidar adds sensor reading once and ignores errors", func(t *testing.T) { - onlineModeLidarTestHelper(context.Background(), t, config, cf, s.GoodLidar) + validAddLidarReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodLidar) }) } diff --git a/sensorprocess/movementsensorprocess_test.go b/sensorprocess/movementsensorprocess_test.go index ca93253b..bceebf67 100644 --- a/sensorprocess/movementsensorprocess_test.go +++ b/sensorprocess/movementsensorprocess_test.go @@ -24,17 +24,12 @@ func TestStartMovementSensor(t *testing.T) { injectLidar := inject.TimedLidar{} injectLidar.DataFrequencyHzFunc = func() int { return 5 } - injectImu := inject.TimedMovementSensor{} - injectImu.NameFunc = func() string { return "good_imu" } - injectImu.DataFrequencyHzFunc = func() int { return 20 } - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, - MovementSensor: &injectImu, - Timeout: 10 * time.Second, + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + Timeout: 10 * time.Second, } t.Run("exits loop when the context was cancelled", func(t *testing.T) { @@ -56,32 +51,32 @@ func TestAddMovementSensorReadingInOnline(t *testing.T) { logger := logging.NewTestLogger(t) cf := cartofacade.Mock{} - injectImu := inject.TimedMovementSensor{} - injectImu.NameFunc = func() string { return "good_imu" } - injectImu.DataFrequencyHzFunc = func() int { return 20 } + injectMovementSensor := inject.TimedMovementSensor{} + injectMovementSensor.NameFunc = func() string { return "good_movement_sensor" } + injectMovementSensor.DataFrequencyHzFunc = func() int { return 20 } config := Config{ Logger: logger, CartoFacade: &cf, IsOnline: true, - MovementSensor: &injectImu, + MovementSensor: &injectMovementSensor, Timeout: 10 * time.Second, } - t.Run("returns error when IMU GetData returns error, doesn't try to add IMU data", func(t *testing.T) { - invalidOnlineModeIMUTestHelper(context.Background(), t, cf, config, 10, s.IMUWithErroringFunctions, 10) + t.Run("returns error when movement sensor GetData returns error, doesn't try to add IMU data", func(t *testing.T) { + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, 10, s.IMUWithErroringFunctions, 10) }) t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { - invalidOnlineModeIMUTestHelper(context.Background(), t, cf, config, 10, s.InvalidReplayIMU, 10) + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, 10, s.InvalidReplayIMU, 10) }) t.Run("online replay IMU adds sensor reading once and ignores errors", func(t *testing.T) { - onlineModeIMUTestHelper(context.Background(), t, config, cf, s.ReplayIMU) + validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.ReplayIMU) }) t.Run("online IMU adds sensor reading once and ignores errors", func(t *testing.T) { - onlineModeIMUTestHelper(context.Background(), t, config, cf, s.GoodIMU) + validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodIMU) }) } diff --git a/sensorprocess/testhelper.go b/sensorprocess/testhelper.go index 8e99b23e..e70dcaeb 100644 --- a/sensorprocess/testhelper.go +++ b/sensorprocess/testhelper.go @@ -51,7 +51,7 @@ DATA binary errUnknown = errors.New("unknown error") ) -func onlineModeLidarTestHelper( +func validAddLidarReadingInOnlineTestHelper( ctx context.Context, t *testing.T, config Config, @@ -123,7 +123,7 @@ func onlineModeLidarTestHelper( } } -func onlineModeIMUTestHelper( +func validAddMovementSensorReadingInOnlineTestHelper( ctx context.Context, t *testing.T, config Config, @@ -195,7 +195,7 @@ func onlineModeIMUTestHelper( } } -func invalidOnlineModeLidarTestHelper( +func invalidAddLidarReadingInOnlineTestHelper( ctx context.Context, t *testing.T, cartoFacadeMock cartofacade.Mock, @@ -230,7 +230,7 @@ func invalidOnlineModeLidarTestHelper( test.That(t, len(calls), test.ShouldEqual, 0) } -func invalidOnlineModeIMUTestHelper( +func invalidAddMovementSensorReadingInOnlineTestHelper( ctx context.Context, t *testing.T, cartoFacadeMock cartofacade.Mock, From 3f393e33c2634451d76df8eadbe958ae19ca4dc3 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Mon, 11 Dec 2023 14:53:54 -0500 Subject: [PATCH 10/21] Rename/add imu to odometer --- sensorprocess/movementsensorprocess_test.go | 26 +++-- sensorprocess/testhelper.go | 101 +++++++++++++++----- sensors/test_deps.go | 53 ++++++++++ 3 files changed, 149 insertions(+), 31 deletions(-) diff --git a/sensorprocess/movementsensorprocess_test.go b/sensorprocess/movementsensorprocess_test.go index bceebf67..438405ec 100644 --- a/sensorprocess/movementsensorprocess_test.go +++ b/sensorprocess/movementsensorprocess_test.go @@ -63,20 +63,34 @@ func TestAddMovementSensorReadingInOnline(t *testing.T) { Timeout: 10 * time.Second, } - t.Run("returns error when movement sensor GetData returns error, doesn't try to add IMU data", func(t *testing.T) { - invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, 10, s.IMUWithErroringFunctions, 10) + t.Run("returns error when LinearAcceleration or AngularVelocity returns error, doesn't try to add IMU data", func(t *testing.T) { + lidarFrequencyHz := 10 + movementSensorFrequencyHz := 10 + imuEnabled := true + odometerEnabled := false + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, + lidarFrequencyHz, s.IMUWithErroringFunctions, movementSensorFrequencyHz, imuEnabled, odometerEnabled) }) - t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { - invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, 10, s.InvalidReplayIMU, 10) + t.Run("returns error when IMU replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { + lidarFrequencyHz := 10 + movementSensorFrequencyHz := 10 + imuEnabled := true + odometerEnabled := false + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, + lidarFrequencyHz, s.InvalidReplayIMU, movementSensorFrequencyHz, imuEnabled, odometerEnabled) }) t.Run("online replay IMU adds sensor reading once and ignores errors", func(t *testing.T) { - validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.ReplayIMU) + imuEnabled := true + odometerEnabled := false + validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.ReplayIMU, imuEnabled, odometerEnabled) }) t.Run("online IMU adds sensor reading once and ignores errors", func(t *testing.T) { - validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodIMU) + imuEnabled := true + odometerEnabled := false + validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodIMU, imuEnabled, odometerEnabled) }) } diff --git a/sensorprocess/testhelper.go b/sensorprocess/testhelper.go index e70dcaeb..527815fc 100644 --- a/sensorprocess/testhelper.go +++ b/sensorprocess/testhelper.go @@ -28,6 +28,12 @@ type addIMUReadingArgs struct { currentReading s.TimedIMUReadingResponse } +type addOdometerReadingArgs struct { + timeout time.Duration + sensorName string + currentReading s.TimedOdometerReadingResponse +} + var ( //nolint:dupword expectedPCD = []byte(`VERSION .7 @@ -128,14 +134,17 @@ func validAddMovementSensorReadingInOnlineTestHelper( t *testing.T, config Config, cf cartofacade.Mock, - testImu s.TestSensor, + testMovementSensor s.TestSensor, + imuEnabled bool, + odometerEnabled bool, ) { logger := logging.NewTestLogger(t) dataFrequencyHz := 100 - imu, err := s.NewMovementSensor(context.Background(), s.SetupDeps(s.NoLidar, testImu), string(testImu), dataFrequencyHz, logger) + movementSensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(s.NoLidar, testMovementSensor), + string(testMovementSensor), dataFrequencyHz, logger) test.That(t, err, test.ShouldBeNil) - var calls []addIMUReadingArgs + var imuCalls []addIMUReadingArgs cf.AddIMUReadingFunc = func( ctx context.Context, timeout time.Duration, @@ -147,35 +156,57 @@ func validAddMovementSensorReadingInOnlineTestHelper( sensorName: sensorName, currentReading: currentReading, } - calls = append(calls, args) - if len(calls) == 1 { + imuCalls = append(imuCalls, args) + if len(imuCalls) == 1 { return errUnknown } - if len(calls) == 2 { + if len(imuCalls) == 2 { + return cartofacade.ErrUnableToAcquireLock + } + return nil + } + + var odometerCalls []addOdometerReadingArgs + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + args := addOdometerReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + odometerCalls = append(odometerCalls, args) + if len(odometerCalls) == 1 { + return errUnknown + } + if len(odometerCalls) == 2 { return cartofacade.ErrUnableToAcquireLock } return nil } config.CartoFacade = &cf - config.MovementSensor = imu + config.MovementSensor = movementSensor config.IsOnline = true err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldBeNil) - test.That(t, len(calls), test.ShouldEqual, 1) + test.That(t, len(imuCalls), test.ShouldEqual, 1) err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldBeNil) - test.That(t, len(calls), test.ShouldEqual, 2) + test.That(t, len(imuCalls), test.ShouldEqual, 2) err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldBeNil) - test.That(t, len(calls), test.ShouldEqual, 3) + test.That(t, len(imuCalls), test.ShouldEqual, 3) - for i, call := range calls { + for i, call := range imuCalls { t.Logf("call %d", i) - test.That(t, call.sensorName, test.ShouldResemble, string(testImu)) + test.That(t, call.sensorName, test.ShouldResemble, string(testMovementSensor)) // the IMU test fixture happens to always return the same readings currently // in reality they are likely different every time test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, expectedIMUReading.LinearAcceleration) @@ -183,15 +214,15 @@ func validAddMovementSensorReadingInOnlineTestHelper( test.That(t, call.timeout, test.ShouldEqual, config.Timeout) } - if testImu == s.GoodIMU { - test.That(t, calls[0].currentReading.ReadingTime.Before(calls[1].currentReading.ReadingTime), test.ShouldBeTrue) - test.That(t, calls[1].currentReading.ReadingTime.Before(calls[2].currentReading.ReadingTime), test.ShouldBeTrue) - } else if testImu == s.ReplayIMU { + if testMovementSensor == s.GoodIMU { + test.That(t, imuCalls[0].currentReading.ReadingTime.Before(imuCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) + test.That(t, imuCalls[1].currentReading.ReadingTime.Before(imuCalls[2].currentReading.ReadingTime), test.ShouldBeTrue) + } else if testMovementSensor == s.ReplayIMU { readingTime, err := time.Parse(time.RFC3339Nano, s.TestTimestamp) test.That(t, err, test.ShouldBeNil) - test.That(t, calls[0].currentReading.ReadingTime.Equal(readingTime), test.ShouldBeTrue) + test.That(t, imuCalls[0].currentReading.ReadingTime.Equal(readingTime), test.ShouldBeTrue) } else { - t.Errorf("no timestamp tests provided for %v", string(testImu)) + t.Errorf("no timestamp tests provided for %v", string(testMovementSensor)) } } @@ -236,14 +267,17 @@ func invalidAddMovementSensorReadingInOnlineTestHelper( cartoFacadeMock cartofacade.Mock, config Config, lidarDataFrequencyHz int, - testIMU s.TestSensor, - imuDataFrequencyHz int, + testMovementSensor s.TestSensor, + movementSensorDataFrequencyHz int, + imuEnabled bool, + odometerEnabled bool, ) { logger := logging.NewTestLogger(t) - imu, err := s.NewMovementSensor(context.Background(), s.SetupDeps(s.NoLidar, testIMU), string(testIMU), imuDataFrequencyHz, logger) + movementSensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(s.NoLidar, testMovementSensor), + string(testMovementSensor), movementSensorDataFrequencyHz, logger) test.That(t, err, test.ShouldBeNil) - var calls []addIMUReadingArgs + var imuCalls []addIMUReadingArgs cartoFacadeMock.AddIMUReadingFunc = func( ctx context.Context, timeout time.Duration, @@ -255,7 +289,23 @@ func invalidAddMovementSensorReadingInOnlineTestHelper( sensorName: sensorName, currentReading: currentReading, } - calls = append(calls, args) + imuCalls = append(imuCalls, args) + return nil + } + + var odometerCalls []addOdometerReadingArgs + cartoFacadeMock.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + args := addOdometerReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + odometerCalls = append(odometerCalls, args) return nil } config.CartoFacade = &cartoFacadeMock @@ -264,9 +314,10 @@ func invalidAddMovementSensorReadingInOnlineTestHelper( injectLidar.DataFrequencyHzFunc = func() int { return lidarDataFrequencyHz } config.Lidar = &injectLidar - config.MovementSensor = imu + config.MovementSensor = movementSensor err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldNotBeNil) - test.That(t, len(calls), test.ShouldEqual, 0) + test.That(t, len(imuCalls), test.ShouldEqual, 0) + test.That(t, len(odometerCalls), test.ShouldEqual, 0) } diff --git a/sensors/test_deps.go b/sensors/test_deps.go index 5e680a71..77c0d32f 100644 --- a/sensors/test_deps.go +++ b/sensors/test_deps.go @@ -81,6 +81,14 @@ const ( // OdometerWithErroringFunctions is an Odometer whose functions return errors. OdometerWithErroringFunctions TestSensor = "odometer_with_erroring_functions" + // ReplayOdometer is an odometer that works as expected and returns position and orientation values. + ReplayOdometer TestSensor = "replay_odometer" + // InvalidReplayOdometer is an odometer whose meta timestamp is invalid. + InvalidReplayOdometer TestSensor = "invalid_replay_odometer" + // FinishedReplayOdometer is an odometer whose Position and Orientation functions return an end of + // dataset error. + FinishedReplayOdometer TestSensor = "finished_replay_odometer" + // MovementSensorNotIMUNotOdometer is a movement sensor that does neither support an IMU nor an odometer. MovementSensorNotIMUNotOdometer TestSensor = "movement_sensor_not_imu_not_odometer" // MovementSensorBothIMUAndOdometer is a movement sensor that dsupports both an IMU nor an odometer. @@ -114,6 +122,9 @@ var ( FinishedReplayIMU: func() *inject.MovementSensor { return getFinishedReplayIMU() }, GoodOdometer: getGoodOdometer, OdometerWithErroringFunctions: getOdometerWithErroringFunctions, + ReplayOdometer: func() *inject.MovementSensor { return getReplayOdometer(TestTimestamp) }, + InvalidReplayOdometer: func() *inject.MovementSensor { return getReplayOdometer(BadTime) }, + FinishedReplayOdometer: func() *inject.MovementSensor { return getFinishedReplayOdometer() }, MovementSensorNotIMUNotOdometer: getMovementSensorNotIMUAndNotOdometer, MovementSensorBothIMUAndOdometer: getMovementSensorBothIMUAndOdometer, MovementSensorWithErroringPropertiesFunc: getMovementSensorWithErroringPropertiesFunc, @@ -356,6 +367,48 @@ func getOdometerWithErroringFunctions() *inject.MovementSensor { return odometer } +func getReplayOdometer(testTime string) *inject.MovementSensor { + odometer := &inject.MovementSensor{} + odometer.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + md := ctx.Value(contextutils.MetadataContextKey) + if mdMap, ok := md.(map[string][]string); ok { + mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} + } + return Position, 1.2, nil + } + odometer.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + md := ctx.Value(contextutils.MetadataContextKey) + if mdMap, ok := md.(map[string][]string); ok { + mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} + } + return Orientation, nil + } + odometer.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + PositionSupported: true, + OrientationSupported: true, + }, nil + } + return odometer +} + +func getFinishedReplayOdometer() *inject.MovementSensor { + odometer := &inject.MovementSensor{} + odometer.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return geo.NewPoint(0, 0), 0.0, replay.ErrEndOfDataset + } + odometer.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + return &spatialmath.Quaternion{}, replay.ErrEndOfDataset + } + odometer.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + PositionSupported: true, + OrientationSupported: true, + }, nil + } + return odometer +} + func getMovementSensorNotIMUAndNotOdometer() *inject.MovementSensor { movementSensor := &inject.MovementSensor{} movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { From c3fa74bc6917a4fda5cea73cb68045c6bd9ccbaf Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Tue, 12 Dec 2023 12:59:29 -0500 Subject: [PATCH 11/21] Improve tests --- sensorprocess/movementsensorprocess_test.go | 47 ++++--- sensorprocess/testhelper.go | 137 +++++++++++--------- sensors/movementsensor.go | 1 + sensors/movementsensor_test.go | 28 ++-- sensors/test_deps.go | 44 +++---- 5 files changed, 147 insertions(+), 110 deletions(-) diff --git a/sensorprocess/movementsensorprocess_test.go b/sensorprocess/movementsensorprocess_test.go index 438405ec..b8833dda 100644 --- a/sensorprocess/movementsensorprocess_test.go +++ b/sensorprocess/movementsensorprocess_test.go @@ -63,34 +63,40 @@ func TestAddMovementSensorReadingInOnline(t *testing.T) { Timeout: 10 * time.Second, } - t.Run("returns error when LinearAcceleration or AngularVelocity returns error, doesn't try to add IMU data", func(t *testing.T) { + t.Run("returns error when LinearAcceleration or AngularVelocity return an error, doesn't try to add IMU data", func(t *testing.T) { lidarFrequencyHz := 10 movementSensorFrequencyHz := 10 - imuEnabled := true - odometerEnabled := false invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, - lidarFrequencyHz, s.IMUWithErroringFunctions, movementSensorFrequencyHz, imuEnabled, odometerEnabled) + lidarFrequencyHz, s.IMUWithErroringFunctions, movementSensorFrequencyHz) + }) + + t.Run("returns error when Position or Orientation return an error, doesn't try to add odometer data", func(t *testing.T) { + lidarFrequencyHz := 10 + movementSensorFrequencyHz := 10 + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, + lidarFrequencyHz, s.OdometerWithErroringFunctions, movementSensorFrequencyHz) }) t.Run("returns error when IMU replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { lidarFrequencyHz := 10 movementSensorFrequencyHz := 10 - imuEnabled := true - odometerEnabled := false invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, - lidarFrequencyHz, s.InvalidReplayIMU, movementSensorFrequencyHz, imuEnabled, odometerEnabled) + lidarFrequencyHz, s.InvalidReplayIMU, movementSensorFrequencyHz) + }) + + t.Run("returns error when odometer replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { + lidarFrequencyHz := 10 + movementSensorFrequencyHz := 10 + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, + lidarFrequencyHz, s.InvalidReplayOdometer, movementSensorFrequencyHz) }) t.Run("online replay IMU adds sensor reading once and ignores errors", func(t *testing.T) { - imuEnabled := true - odometerEnabled := false - validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.ReplayIMU, imuEnabled, odometerEnabled) + validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.ReplayIMU) }) t.Run("online IMU adds sensor reading once and ignores errors", func(t *testing.T) { - imuEnabled := true - odometerEnabled := false - validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodIMU, imuEnabled, odometerEnabled) + validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodIMU) }) } @@ -144,15 +150,24 @@ func TestTryAddMovementSensorReadingUntilSuccess(t *testing.T) { config.MovementSensor = replayIMU config.Lidar = &injectLidar - config.tryAddMovementSensorReadingUntilSuccess(ctx, expectedMovementSensorReading) + now := time.Now() + movementSensorReading := s.TimedMovementSensorReadingResponse{ + TimedIMUResponse: &s.TimedIMUReadingResponse{ + AngularVelocity: s.TestAngVel, + LinearAcceleration: s.TestLinAcc, + ReadingTime: now, + }, + } + + config.tryAddMovementSensorReadingUntilSuccess(ctx, movementSensorReading) test.That(t, len(calls), test.ShouldEqual, 3) firstTimestamp := calls[0].currentReading.ReadingTime for i, call := range calls { t.Logf("call %d", i) test.That(t, call.sensorName, test.ShouldResemble, string(imu)) - test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, expectedIMUReading.LinearAcceleration) - test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, expectedIMUReading.AngularVelocity) + test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, movementSensorReading.TimedIMUResponse.LinearAcceleration) + test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, movementSensorReading.TimedIMUResponse.AngularVelocity) test.That(t, call.timeout, test.ShouldEqual, config.Timeout) test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) } diff --git a/sensorprocess/testhelper.go b/sensorprocess/testhelper.go index 527815fc..c9cdbed5 100644 --- a/sensorprocess/testhelper.go +++ b/sensorprocess/testhelper.go @@ -6,7 +6,6 @@ import ( "testing" "time" - "github.com/golang/geo/r3" "go.viam.com/rdk/logging" "go.viam.com/rdk/spatialmath" "go.viam.com/test" @@ -14,6 +13,7 @@ import ( "github.com/viamrobotics/viam-cartographer/cartofacade" s "github.com/viamrobotics/viam-cartographer/sensors" "github.com/viamrobotics/viam-cartographer/sensors/inject" + rdkutils "go.viam.com/rdk/utils" ) type addLidarReadingArgs struct { @@ -47,13 +47,7 @@ VIEWPOINT 0 0 0 1 0 0 0 POINTS 0 DATA binary `) - expectedIMUReading = s.TimedIMUReadingResponse{ - LinearAcceleration: r3.Vector{X: 1, Y: 1, Z: 1}, - AngularVelocity: spatialmath.AngularVelocity{X: 0.017453292519943295, Y: 0.008726646259971648, Z: 0}, - } - expectedMovementSensorReading = s.TimedMovementSensorReadingResponse{ - TimedIMUResponse: &expectedIMUReading, - } + errUnknown = errors.New("unknown error") ) @@ -129,14 +123,47 @@ func validAddLidarReadingInOnlineTestHelper( } } +func invalidAddLidarReadingInOnlineTestHelper( + ctx context.Context, + t *testing.T, + cartoFacadeMock cartofacade.Mock, + config Config, + testLidar s.TestSensor, + lidarDataFrequencyHz int, +) { + logger := logging.NewTestLogger(t) + lidar, err := s.NewLidar(context.Background(), s.SetupDeps(testLidar, s.NoMovementSensor), string(testLidar), lidarDataFrequencyHz, logger) + test.That(t, err, test.ShouldBeNil) + + var calls []addLidarReadingArgs + cartoFacadeMock.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + args := addLidarReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + calls = append(calls, args) + return nil + } + config.Lidar = lidar + config.CartoFacade = &cartoFacadeMock + + err = config.addLidarReadingInOnline(ctx) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, len(calls), test.ShouldEqual, 0) +} + func validAddMovementSensorReadingInOnlineTestHelper( ctx context.Context, t *testing.T, config Config, cf cartofacade.Mock, testMovementSensor s.TestSensor, - imuEnabled bool, - odometerEnabled bool, ) { logger := logging.NewTestLogger(t) dataFrequencyHz := 100 @@ -192,26 +219,57 @@ func validAddMovementSensorReadingInOnlineTestHelper( config.MovementSensor = movementSensor config.IsOnline = true + testNumberCalls := func(movementSensor s.TimedMovementSensor, expectedNumberCalls int) { + if movementSensor.Properties().IMUSupported { + test.That(t, len(imuCalls), test.ShouldEqual, expectedNumberCalls) + } else { + test.That(t, len(imuCalls), test.ShouldEqual, 0) + } + if movementSensor.Properties().OdometerSupported { + test.That(t, len(odometerCalls), test.ShouldEqual, expectedNumberCalls) + } else { + test.That(t, len(odometerCalls), test.ShouldEqual, 0) + } + } + err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldBeNil) - test.That(t, len(imuCalls), test.ShouldEqual, 1) + testNumberCalls(movementSensor, 1) err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldBeNil) - test.That(t, len(imuCalls), test.ShouldEqual, 2) + testNumberCalls(movementSensor, 2) err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldBeNil) - test.That(t, len(imuCalls), test.ShouldEqual, 3) + testNumberCalls(movementSensor, 3) + + if movementSensor.Properties().IMUSupported { + for i, call := range imuCalls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, string(testMovementSensor)) + // the IMU test fixture happens to always return the same readings currently + // in reality they are likely different every time + test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, s.TestLinAcc) + test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, spatialmath.AngularVelocity{ + X: rdkutils.DegToRad(s.TestAngVel.X), + Y: rdkutils.DegToRad(s.TestAngVel.Y), + Z: rdkutils.DegToRad(s.TestAngVel.Z), + }) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + } + } - for i, call := range imuCalls { - t.Logf("call %d", i) - test.That(t, call.sensorName, test.ShouldResemble, string(testMovementSensor)) - // the IMU test fixture happens to always return the same readings currently - // in reality they are likely different every time - test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, expectedIMUReading.LinearAcceleration) - test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, expectedIMUReading.AngularVelocity) - test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + if movementSensor.Properties().OdometerSupported { + for i, call := range odometerCalls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, string(testMovementSensor)) + // the odometer test fixture happens to always return the same readings currently + // in reality they are likely different every time + test.That(t, call.currentReading.Position, test.ShouldResemble, s.TestPosition) + test.That(t, call.currentReading.Orientation, test.ShouldResemble, s.TestOrientation) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + } } if testMovementSensor == s.GoodIMU { @@ -226,41 +284,6 @@ func validAddMovementSensorReadingInOnlineTestHelper( } } -func invalidAddLidarReadingInOnlineTestHelper( - ctx context.Context, - t *testing.T, - cartoFacadeMock cartofacade.Mock, - config Config, - testLidar s.TestSensor, - lidarDataFrequencyHz int, -) { - logger := logging.NewTestLogger(t) - lidar, err := s.NewLidar(context.Background(), s.SetupDeps(testLidar, s.NoMovementSensor), string(testLidar), lidarDataFrequencyHz, logger) - test.That(t, err, test.ShouldBeNil) - - var calls []addLidarReadingArgs - cartoFacadeMock.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - args := addLidarReadingArgs{ - timeout: timeout, - sensorName: sensorName, - currentReading: currentReading, - } - calls = append(calls, args) - return nil - } - config.Lidar = lidar - config.CartoFacade = &cartoFacadeMock - - err = config.addLidarReadingInOnline(ctx) - test.That(t, err, test.ShouldNotBeNil) - test.That(t, len(calls), test.ShouldEqual, 0) -} - func invalidAddMovementSensorReadingInOnlineTestHelper( ctx context.Context, t *testing.T, @@ -269,8 +292,6 @@ func invalidAddMovementSensorReadingInOnlineTestHelper( lidarDataFrequencyHz int, testMovementSensor s.TestSensor, movementSensorDataFrequencyHz int, - imuEnabled bool, - odometerEnabled bool, ) { logger := logging.NewTestLogger(t) movementSensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(s.NoLidar, testMovementSensor), diff --git a/sensors/movementsensor.go b/sensors/movementsensor.go index f4c660a3..5a7b2009 100644 --- a/sensors/movementsensor.go +++ b/sensors/movementsensor.go @@ -58,6 +58,7 @@ type TimedMovementSensorReadingResponse struct { // TimedIMUReadingResponse represents an IMU sensor reading with a time. type TimedIMUReadingResponse struct { + // TODO[kat]: AngularVelocity is currently in radians. Either leave a comment, or change this. AngularVelocity spatialmath.AngularVelocity LinearAcceleration r3.Vector ReadingTime time.Time diff --git a/sensors/movementsensor_test.go b/sensors/movementsensor_test.go index eb579bd3..d7ab3303 100644 --- a/sensors/movementsensor_test.go +++ b/sensors/movementsensor_test.go @@ -57,12 +57,12 @@ func TestNewMovementSensor(t *testing.T) { actualReading, err := actualMs.TimedMovementSensorReading(context.Background()) test.That(t, err, test.ShouldBeNil) - test.That(t, actualReading.TimedIMUResponse.LinearAcceleration, test.ShouldResemble, s.LinAcc) + test.That(t, actualReading.TimedIMUResponse.LinearAcceleration, test.ShouldResemble, s.TestLinAcc) test.That(t, actualReading.TimedIMUResponse.AngularVelocity, test.ShouldResemble, spatialmath.AngularVelocity{ - X: rdkutils.DegToRad(s.AngVel.X), - Y: rdkutils.DegToRad(s.AngVel.Y), - Z: rdkutils.DegToRad(s.AngVel.Z), + X: rdkutils.DegToRad(s.TestAngVel.X), + Y: rdkutils.DegToRad(s.TestAngVel.Y), + Z: rdkutils.DegToRad(s.TestAngVel.Z), }) test.That(t, actualReading.TimedOdometerResponse, test.ShouldBeNil) }) @@ -76,8 +76,8 @@ func TestNewMovementSensor(t *testing.T) { actualReading, err := actualMs.TimedMovementSensorReading(context.Background()) test.That(t, err, test.ShouldBeNil) - test.That(t, actualReading.TimedOdometerResponse.Position, test.ShouldResemble, s.Position) - test.That(t, actualReading.TimedOdometerResponse.Orientation, test.ShouldResemble, s.Orientation) + test.That(t, actualReading.TimedOdometerResponse.Position, test.ShouldResemble, s.TestPosition) + test.That(t, actualReading.TimedOdometerResponse.Orientation, test.ShouldResemble, s.TestOrientation) test.That(t, actualReading.TimedIMUResponse, test.ShouldBeNil) }) } @@ -160,12 +160,12 @@ func TestTimedMovementSensorReading(t *testing.T) { afterReading := time.Now().UTC() test.That(t, err, test.ShouldBeNil) - test.That(t, actualReading.TimedIMUResponse.LinearAcceleration, test.ShouldResemble, s.LinAcc) + test.That(t, actualReading.TimedIMUResponse.LinearAcceleration, test.ShouldResemble, s.TestLinAcc) test.That(t, actualReading.TimedIMUResponse.AngularVelocity, test.ShouldResemble, spatialmath.AngularVelocity{ - X: rdkutils.DegToRad(s.AngVel.X), - Y: rdkutils.DegToRad(s.AngVel.Y), - Z: rdkutils.DegToRad(s.AngVel.Z), + X: rdkutils.DegToRad(s.TestAngVel.X), + Y: rdkutils.DegToRad(s.TestAngVel.Y), + Z: rdkutils.DegToRad(s.TestAngVel.Z), }) test.That(t, actualReading.TimedIMUResponse.ReadingTime.After(beforeReading), test.ShouldBeTrue) test.That(t, actualReading.TimedIMUResponse.ReadingTime.Before(afterReading), test.ShouldBeTrue) @@ -189,8 +189,8 @@ func TestTimedMovementSensorReading(t *testing.T) { afterReading := time.Now().UTC() test.That(t, err, test.ShouldBeNil) - test.That(t, actualReading.TimedOdometerResponse.Position, test.ShouldResemble, s.Position) - test.That(t, actualReading.TimedOdometerResponse.Orientation, test.ShouldResemble, s.Orientation) + test.That(t, actualReading.TimedOdometerResponse.Position, test.ShouldResemble, s.TestPosition) + test.That(t, actualReading.TimedOdometerResponse.Orientation, test.ShouldResemble, s.TestOrientation) test.That(t, actualReading.TimedOdometerResponse.ReadingTime.After(beforeReading), test.ShouldBeTrue) test.That(t, actualReading.TimedOdometerResponse.ReadingTime.Before(afterReading), test.ShouldBeTrue) test.That(t, actualReading.TimedOdometerResponse.ReadingTime.Location(), test.ShouldEqual, time.UTC) @@ -214,8 +214,8 @@ func TestTimedMovementSensorReading(t *testing.T) { afterReading := time.Now().UTC() test.That(t, err, test.ShouldBeNil) - test.That(t, actualReading.TimedOdometerResponse.Position, test.ShouldResemble, s.Position) - test.That(t, actualReading.TimedOdometerResponse.Orientation, test.ShouldResemble, s.Orientation) + test.That(t, actualReading.TimedOdometerResponse.Position, test.ShouldResemble, s.TestPosition) + test.That(t, actualReading.TimedOdometerResponse.Orientation, test.ShouldResemble, s.TestOrientation) test.That(t, actualReading.TimedOdometerResponse.ReadingTime.After(beforeReading), test.ShouldBeTrue) test.That(t, actualReading.TimedOdometerResponse.ReadingTime.Before(afterReading), test.ShouldBeTrue) test.That(t, actualReading.TimedOdometerResponse.ReadingTime.Location(), test.ShouldEqual, time.UTC) diff --git a/sensors/test_deps.go b/sensors/test_deps.go index 77c0d32f..8809c1d1 100644 --- a/sensors/test_deps.go +++ b/sensors/test_deps.go @@ -26,14 +26,14 @@ const BadTime = "NOT A TIME" var ( // TestTimestamp can be used to test specific timestamps provided by a replay sensor. TestTimestamp = time.Now().UTC().Format("2006-01-02T15:04:05.999999Z") - // LinAcc is the successful mock linear acceleration result used for testing. - LinAcc = r3.Vector{X: 1, Y: 1, Z: 1} - // AngVel is the successful mock angular velocity result used for testing. - AngVel = spatialmath.AngularVelocity{X: 1, Y: .5, Z: 0} - // Position is the successful mock position result used for testing. - Position = geo.NewPoint(1, 2) - // Orientation is the successful mock orientation result used for testing. - Orientation = spatialmath.NewZeroOrientation() + // TestLinAcc is the successful mock linear acceleration result used for testing. + TestLinAcc = r3.Vector{X: 1, Y: 2, Z: 3} + // TestAngVel is the successful mock angular velocity result used for testing. + TestAngVel = spatialmath.AngularVelocity{X: 1.1, Y: .5, Z: 0} + // TestPosition is the successful mock position result used for testing. + TestPosition = geo.NewPoint(5, 4) + // TestOrientation is the successful mock orientation result used for testing. + TestOrientation = &spatialmath.Quaternion{Real: 0.1, Imag: -0.2, Jmag: 2.5, Kmag: -9.1} ) // TestSensor represents sensors used for testing. @@ -260,10 +260,10 @@ func getFinishedReplayLidar() *inject.Camera { func getGoodIMU() *inject.MovementSensor { imu := &inject.MovementSensor{} imu.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - return LinAcc, nil + return TestLinAcc, nil } imu.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { - return AngVel, nil + return TestAngVel, nil } imu.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -298,14 +298,14 @@ func getReplayIMU(testTime string) *inject.MovementSensor { if mdMap, ok := md.(map[string][]string); ok { mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} } - return LinAcc, nil + return TestLinAcc, nil } imu.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { md := ctx.Value(contextutils.MetadataContextKey) if mdMap, ok := md.(map[string][]string); ok { mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} } - return AngVel, nil + return TestAngVel, nil } imu.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -336,10 +336,10 @@ func getFinishedReplayIMU() *inject.MovementSensor { func getGoodOdometer() *inject.MovementSensor { odometer := &inject.MovementSensor{} odometer.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return Position, 10, nil + return TestPosition, 10, nil } odometer.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - return Orientation, nil + return TestOrientation, nil } odometer.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -374,14 +374,14 @@ func getReplayOdometer(testTime string) *inject.MovementSensor { if mdMap, ok := md.(map[string][]string); ok { mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} } - return Position, 1.2, nil + return TestPosition, 1.2, nil } odometer.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { md := ctx.Value(contextutils.MetadataContextKey) if mdMap, ok := md.(map[string][]string); ok { mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} } - return Orientation, nil + return TestOrientation, nil } odometer.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -420,16 +420,16 @@ func getMovementSensorNotIMUAndNotOdometer() *inject.MovementSensor { func getMovementSensorBothIMUAndOdometer() *inject.MovementSensor { movementSensor := &inject.MovementSensor{} movementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return Position, 10, nil + return TestPosition, 10, nil } movementSensor.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - return Orientation, nil + return TestOrientation, nil } movementSensor.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - return LinAcc, nil + return TestLinAcc, nil } movementSensor.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { - return AngVel, nil + return TestAngVel, nil } movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -453,10 +453,10 @@ func getMovementSensorWithErroringPropertiesFunc() *inject.MovementSensor { func getMovementSensorWithInvalidProperties() *inject.MovementSensor { movementSensor := &inject.MovementSensor{} movementSensor.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - return LinAcc, nil + return TestLinAcc, nil } movementSensor.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { - return AngVel, nil + return TestAngVel, nil } movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ From f0edf3900311e20d12ddd8500f6c31c3e47a934f Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Tue, 12 Dec 2023 14:25:31 -0500 Subject: [PATCH 12/21] Improve tests --- sensorprocess/movementsensorprocess_test.go | 128 +++++++++--------- sensorprocess/testhelper.go | 118 +++++++++++++++-- sensors/movementsensor_test.go | 4 +- sensors/test_deps.go | 139 +++++++++++++++++--- 4 files changed, 297 insertions(+), 92 deletions(-) diff --git a/sensorprocess/movementsensorprocess_test.go b/sensorprocess/movementsensorprocess_test.go index b8833dda..c97b366a 100644 --- a/sensorprocess/movementsensorprocess_test.go +++ b/sensorprocess/movementsensorprocess_test.go @@ -77,6 +77,13 @@ func TestAddMovementSensorReadingInOnline(t *testing.T) { lidarFrequencyHz, s.OdometerWithErroringFunctions, movementSensorFrequencyHz) }) + t.Run("returns error when LinearAcceleration, AngularVelocity, Position, and Orientation return an error, doesn't try to add movement sensor data", func(t *testing.T) { + lidarFrequencyHz := 10 + movementSensorFrequencyHz := 10 + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, + lidarFrequencyHz, s.MovementSensorBothIMUAndOdometerWithErroringFunctions, movementSensorFrequencyHz) + }) + t.Run("returns error when IMU replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { lidarFrequencyHz := 10 movementSensorFrequencyHz := 10 @@ -91,12 +98,57 @@ func TestAddMovementSensorReadingInOnline(t *testing.T) { lidarFrequencyHz, s.InvalidReplayOdometer, movementSensorFrequencyHz) }) + t.Run("returns error when replay movement sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { + lidarFrequencyHz := 10 + movementSensorFrequencyHz := 10 + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, + lidarFrequencyHz, s.InvalidReplayMovementSensorBothIMUAndOdometer, movementSensorFrequencyHz) + }) + t.Run("online replay IMU adds sensor reading once and ignores errors", func(t *testing.T) { - validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.ReplayIMU) + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.ReplayIMU) + test.That(t, len(odometerCalls), test.ShouldBeZeroValue) + readingTime, err := time.Parse(time.RFC3339Nano, s.TestTimestamp) + test.That(t, err, test.ShouldBeNil) + test.That(t, imuCalls[0].currentReading.ReadingTime.Equal(readingTime), test.ShouldBeTrue) + }) + + t.Run("online replay odometer adds sensor reading once and ignores errors", func(t *testing.T) { + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.ReplayOdometer) + test.That(t, len(imuCalls), test.ShouldBeZeroValue) + readingTime, err := time.Parse(time.RFC3339Nano, s.TestTimestamp) + test.That(t, err, test.ShouldBeNil) + test.That(t, odometerCalls[0].currentReading.ReadingTime.Equal(readingTime), test.ShouldBeTrue) + }) + + t.Run("online replay movement sensor adds sensor reading once and ignores errors", func(t *testing.T) { + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.ReplayMovementSensorBothIMUAndOdometer) + readingTime, err := time.Parse(time.RFC3339Nano, s.TestTimestamp) + test.That(t, err, test.ShouldBeNil) + test.That(t, imuCalls[0].currentReading.ReadingTime.Equal(readingTime), test.ShouldBeTrue) + test.That(t, odometerCalls[0].currentReading.ReadingTime.Equal(readingTime), test.ShouldBeTrue) }) t.Run("online IMU adds sensor reading once and ignores errors", func(t *testing.T) { - validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodIMU) + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodIMU) + test.That(t, len(odometerCalls), test.ShouldBeZeroValue) + test.That(t, imuCalls[0].currentReading.ReadingTime.Before(imuCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) + test.That(t, imuCalls[1].currentReading.ReadingTime.Before(imuCalls[2].currentReading.ReadingTime), test.ShouldBeTrue) + }) + + t.Run("online odometer adds sensor reading once and ignores errors", func(t *testing.T) { + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodOdometer) + test.That(t, len(imuCalls), test.ShouldBeZeroValue) + test.That(t, odometerCalls[0].currentReading.ReadingTime.Before(odometerCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) + test.That(t, odometerCalls[1].currentReading.ReadingTime.Before(odometerCalls[2].currentReading.ReadingTime), test.ShouldBeTrue) + }) + + t.Run("online movement sensor adds sensor reading once and ignores errors", func(t *testing.T) { + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodMovementSensorBothIMUAndOdometer) + test.That(t, imuCalls[0].currentReading.ReadingTime.Before(imuCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) + test.That(t, imuCalls[1].currentReading.ReadingTime.Before(imuCalls[2].currentReading.ReadingTime), test.ShouldBeTrue) + test.That(t, odometerCalls[0].currentReading.ReadingTime.Before(odometerCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) + test.That(t, odometerCalls[1].currentReading.ReadingTime.Before(odometerCalls[2].currentReading.ReadingTime), test.ShouldBeTrue) }) } @@ -106,71 +158,23 @@ func TestTryAddMovementSensorReadingUntilSuccess(t *testing.T) { cf := cartofacade.Mock{} - dataFrequencyHz := 0 - injectImu := inject.TimedMovementSensor{} - injectImu.DataFrequencyHzFunc = func() int { return dataFrequencyHz } - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: false, - MovementSensor: &injectImu, - Timeout: 10 * time.Second, + Logger: logger, + CartoFacade: &cf, + IsOnline: false, + Timeout: 10 * time.Second, } - t.Run("replay IMU adds sensor data until success", func(t *testing.T) { - lidar, imu := s.NoLidar, s.ReplayIMU - replayIMU, err := s.NewMovementSensor(context.Background(), s.SetupDeps(lidar, imu), string(imu), dataFrequencyHz, logger) - test.That(t, err, test.ShouldBeNil) - - injectLidar := inject.TimedLidar{} - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + t.Run("replay IMU attempts to add sensor data until success", func(t *testing.T) { + validAddMovementSensorReadingUntilSuccessTestHelper(ctx, t, config, cf, s.ReplayIMU) + }) - var calls []addIMUReadingArgs - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - args := addIMUReadingArgs{ - timeout: timeout, - sensorName: sensorName, - currentReading: currentReading, - } - calls = append(calls, args) - if len(calls) == 1 { - return errUnknown - } - if len(calls) == 2 { - return cartofacade.ErrUnableToAcquireLock - } - return nil - } - config.MovementSensor = replayIMU - config.Lidar = &injectLidar - - now := time.Now() - movementSensorReading := s.TimedMovementSensorReadingResponse{ - TimedIMUResponse: &s.TimedIMUReadingResponse{ - AngularVelocity: s.TestAngVel, - LinearAcceleration: s.TestLinAcc, - ReadingTime: now, - }, - } + t.Run("replay odometer attempts to add sensor data until success", func(t *testing.T) { + validAddMovementSensorReadingUntilSuccessTestHelper(ctx, t, config, cf, s.ReplayOdometer) + }) - config.tryAddMovementSensorReadingUntilSuccess(ctx, movementSensorReading) - test.That(t, len(calls), test.ShouldEqual, 3) - - firstTimestamp := calls[0].currentReading.ReadingTime - for i, call := range calls { - t.Logf("call %d", i) - test.That(t, call.sensorName, test.ShouldResemble, string(imu)) - test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, movementSensorReading.TimedIMUResponse.LinearAcceleration) - test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, movementSensorReading.TimedIMUResponse.AngularVelocity) - test.That(t, call.timeout, test.ShouldEqual, config.Timeout) - test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) - } + t.Run("replay movement sensor attempts to add sensor data until success", func(t *testing.T) { + validAddMovementSensorReadingUntilSuccessTestHelper(ctx, t, config, cf, s.ReplayMovementSensorBothIMUAndOdometer) }) } diff --git a/sensorprocess/testhelper.go b/sensorprocess/testhelper.go index c9cdbed5..ddba3629 100644 --- a/sensorprocess/testhelper.go +++ b/sensorprocess/testhelper.go @@ -164,7 +164,7 @@ func validAddMovementSensorReadingInOnlineTestHelper( config Config, cf cartofacade.Mock, testMovementSensor s.TestSensor, -) { +) ([]addIMUReadingArgs, []addOdometerReadingArgs) { logger := logging.NewTestLogger(t) dataFrequencyHz := 100 movementSensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(s.NoLidar, testMovementSensor), @@ -272,16 +272,7 @@ func validAddMovementSensorReadingInOnlineTestHelper( } } - if testMovementSensor == s.GoodIMU { - test.That(t, imuCalls[0].currentReading.ReadingTime.Before(imuCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) - test.That(t, imuCalls[1].currentReading.ReadingTime.Before(imuCalls[2].currentReading.ReadingTime), test.ShouldBeTrue) - } else if testMovementSensor == s.ReplayIMU { - readingTime, err := time.Parse(time.RFC3339Nano, s.TestTimestamp) - test.That(t, err, test.ShouldBeNil) - test.That(t, imuCalls[0].currentReading.ReadingTime.Equal(readingTime), test.ShouldBeTrue) - } else { - t.Errorf("no timestamp tests provided for %v", string(testMovementSensor)) - } + return imuCalls, odometerCalls } func invalidAddMovementSensorReadingInOnlineTestHelper( @@ -342,3 +333,108 @@ func invalidAddMovementSensorReadingInOnlineTestHelper( test.That(t, len(imuCalls), test.ShouldEqual, 0) test.That(t, len(odometerCalls), test.ShouldEqual, 0) } + +func validAddMovementSensorReadingUntilSuccessTestHelper( + ctx context.Context, + t *testing.T, + config Config, + cf cartofacade.Mock, + testMovementSensor s.TestSensor, +) { + logger := logging.NewTestLogger(t) + dataFrequencyHz := 0 + movementSensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(s.NoLidar, testMovementSensor), + string(testMovementSensor), dataFrequencyHz, logger) + test.That(t, err, test.ShouldBeNil) + + var imuCalls []addIMUReadingArgs + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + args := addIMUReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + imuCalls = append(imuCalls, args) + if len(imuCalls) == 1 { + return errUnknown + } + if len(imuCalls) == 2 { + return cartofacade.ErrUnableToAcquireLock + } + return nil + } + + var odometerCalls []addOdometerReadingArgs + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + args := addOdometerReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + odometerCalls = append(odometerCalls, args) + if len(odometerCalls) == 1 { + return errUnknown + } + if len(odometerCalls) == 2 { + return cartofacade.ErrUnableToAcquireLock + } + return nil + } + + config.CartoFacade = &cf + config.MovementSensor = movementSensor + config.IsOnline = true + + now := time.Now() + var movementSensorReading s.TimedMovementSensorReadingResponse + if movementSensor.Properties().IMUSupported { + movementSensorReading.TimedIMUResponse = &s.TimedIMUReadingResponse{ + AngularVelocity: s.TestAngVel, + LinearAcceleration: s.TestLinAcc, + ReadingTime: now, + } + } + if movementSensor.Properties().OdometerSupported { + movementSensorReading.TimedOdometerResponse = &s.TimedOdometerReadingResponse{ + Position: s.TestPosition, + Orientation: s.TestOrientation, + ReadingTime: now, + } + } + + config.tryAddMovementSensorReadingUntilSuccess(ctx, movementSensorReading) + if movementSensor.Properties().IMUSupported { + test.That(t, len(imuCalls), test.ShouldEqual, 3) + firstTimestamp := imuCalls[0].currentReading.ReadingTime + for i, call := range imuCalls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, string(testMovementSensor)) + test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, movementSensorReading.TimedIMUResponse.LinearAcceleration) + test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, movementSensorReading.TimedIMUResponse.AngularVelocity) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + } + } + if movementSensor.Properties().OdometerSupported { + test.That(t, len(odometerCalls), test.ShouldEqual, 3) + firstTimestamp := odometerCalls[0].currentReading.ReadingTime + for i, call := range odometerCalls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, string(testMovementSensor)) + test.That(t, call.currentReading.Position, test.ShouldResemble, movementSensorReading.TimedOdometerResponse.Position) + test.That(t, call.currentReading.Orientation, test.ShouldResemble, movementSensorReading.TimedOdometerResponse.Orientation) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + } + } +} diff --git a/sensors/movementsensor_test.go b/sensors/movementsensor_test.go index d7ab3303..54989385 100644 --- a/sensors/movementsensor_test.go +++ b/sensors/movementsensor_test.go @@ -107,7 +107,7 @@ func TestProperties(t *testing.T) { }) t.Run("both IMU and odometer supported", func(t *testing.T) { - lidar, movementSensor := s.GoodLidar, s.MovementSensorBothIMUAndOdometer + lidar, movementSensor := s.GoodLidar, s.GoodMovementSensorBothIMUAndOdometer deps := s.SetupDeps(lidar, movementSensor) actualMovementSensor, err := s.NewMovementSensor(ctx, deps, string(movementSensor), testDataFrequencyHz, logger) test.That(t, err, test.ShouldBeNil) @@ -200,7 +200,7 @@ func TestTimedMovementSensorReading(t *testing.T) { t.Run("when a movemement sensor that supports both an odometer and an IMU succeeds,"+ " returns current time in UTC and the reading", func(t *testing.T) { - lidar, odometer := s.GoodLidar, s.MovementSensorBothIMUAndOdometer + lidar, odometer := s.GoodLidar, s.GoodMovementSensorBothIMUAndOdometer deps := s.SetupDeps(lidar, odometer) actualOdometer, err := s.NewMovementSensor(ctx, deps, string(odometer), testDataFrequencyHz, logger) test.That(t, err, test.ShouldBeNil) diff --git a/sensors/test_deps.go b/sensors/test_deps.go index 8809c1d1..fb04fb9c 100644 --- a/sensors/test_deps.go +++ b/sensors/test_deps.go @@ -91,8 +91,10 @@ const ( // MovementSensorNotIMUNotOdometer is a movement sensor that does neither support an IMU nor an odometer. MovementSensorNotIMUNotOdometer TestSensor = "movement_sensor_not_imu_not_odometer" - // MovementSensorBothIMUAndOdometer is a movement sensor that dsupports both an IMU nor an odometer. - MovementSensorBothIMUAndOdometer TestSensor = "movement_sensor_imu_and_odometer" + // GoodMovementSensorBothIMUAndOdometer is a movement sensor that supports both an IMU nor an odometer. + GoodMovementSensorBothIMUAndOdometer TestSensor = "good_movement_sensor_imu_and_odometer" + // MovementSensorBothIMUAndOdometerWithErroringFunctions is a movement sensor whose functions return errors. + MovementSensorBothIMUAndOdometerWithErroringFunctions TestSensor = "movement_sensor_imu_and_odometer_with_erroring_functions" // MovementSensorWithErroringPropertiesFunc is a movement sensor whose Properties function returns an error. MovementSensorWithErroringPropertiesFunc TestSensor = "movement_sensor_with_erroring_properties_function" // MovementSensorWithInvalidProperties is a movement sensor whose properties are invalid. @@ -101,6 +103,14 @@ const ( GibberishMovementSensor TestSensor = "gibberish_movement_sensor" // NoMovementSensor is a movement sensor that represents that no movement sensor is set up or added. NoMovementSensor TestSensor = "" + + // ReplayMovementSensorBothIMUAndOdometer is a replay movement sensor that supports both an IMU nor an odometer. + ReplayMovementSensorBothIMUAndOdometer TestSensor = "replay_movement_sensor_imu_and_odometer" + // InvalidReplayMovementSensorBothIMUAndOdometer is a replay movement sensor that supports both an IMU nor an odometer. + InvalidReplayMovementSensorBothIMUAndOdometer TestSensor = "invalid_replay_movement_sensor_imu_and_odometer" + // FinishedReplayMovementSensor is a movement sensor whose LinearAcceleration, AngularVelocity, Position, and Orientation + // functions return an end of dataset error. + FinishedReplayMovementSensor TestSensor = "finished_replay_movement_sensor" ) var ( @@ -115,20 +125,24 @@ var ( } testMovementSensors = map[TestSensor]func() *inject.MovementSensor{ - GoodIMU: getGoodIMU, - IMUWithErroringFunctions: getIMUWithErroringFunctions, - ReplayIMU: func() *inject.MovementSensor { return getReplayIMU(TestTimestamp) }, - InvalidReplayIMU: func() *inject.MovementSensor { return getReplayIMU(BadTime) }, - FinishedReplayIMU: func() *inject.MovementSensor { return getFinishedReplayIMU() }, - GoodOdometer: getGoodOdometer, - OdometerWithErroringFunctions: getOdometerWithErroringFunctions, - ReplayOdometer: func() *inject.MovementSensor { return getReplayOdometer(TestTimestamp) }, - InvalidReplayOdometer: func() *inject.MovementSensor { return getReplayOdometer(BadTime) }, - FinishedReplayOdometer: func() *inject.MovementSensor { return getFinishedReplayOdometer() }, - MovementSensorNotIMUNotOdometer: getMovementSensorNotIMUAndNotOdometer, - MovementSensorBothIMUAndOdometer: getMovementSensorBothIMUAndOdometer, - MovementSensorWithErroringPropertiesFunc: getMovementSensorWithErroringPropertiesFunc, - MovementSensorWithInvalidProperties: getMovementSensorWithInvalidProperties, + GoodIMU: getGoodIMU, + IMUWithErroringFunctions: getIMUWithErroringFunctions, + ReplayIMU: func() *inject.MovementSensor { return getReplayIMU(TestTimestamp) }, + InvalidReplayIMU: func() *inject.MovementSensor { return getReplayIMU(BadTime) }, + FinishedReplayIMU: func() *inject.MovementSensor { return getFinishedReplayIMU() }, + GoodOdometer: getGoodOdometer, + OdometerWithErroringFunctions: getOdometerWithErroringFunctions, + ReplayOdometer: func() *inject.MovementSensor { return getReplayOdometer(TestTimestamp) }, + InvalidReplayOdometer: func() *inject.MovementSensor { return getReplayOdometer(BadTime) }, + FinishedReplayOdometer: func() *inject.MovementSensor { return getFinishedReplayOdometer() }, + MovementSensorNotIMUNotOdometer: getMovementSensorNotIMUAndNotOdometer, + GoodMovementSensorBothIMUAndOdometer: getGoodMovementSensorBothIMUAndOdometer, + MovementSensorBothIMUAndOdometerWithErroringFunctions: getMovementSensorBothIMUAndOdometerWithErroringFunctions, + MovementSensorWithErroringPropertiesFunc: getMovementSensorWithErroringPropertiesFunc, + MovementSensorWithInvalidProperties: getMovementSensorWithInvalidProperties, + ReplayMovementSensorBothIMUAndOdometer: func() *inject.MovementSensor { return getReplayMovementSensor(TestTimestamp) }, + InvalidReplayMovementSensorBothIMUAndOdometer: func() *inject.MovementSensor { return getReplayMovementSensor(BadTime) }, + FinishedReplayMovementSensor: func() *inject.MovementSensor { return getFinishedReplayMovementSensor() }, } ) @@ -417,7 +431,7 @@ func getMovementSensorNotIMUAndNotOdometer() *inject.MovementSensor { return movementSensor } -func getMovementSensorBothIMUAndOdometer() *inject.MovementSensor { +func getGoodMovementSensorBothIMUAndOdometer() *inject.MovementSensor { movementSensor := &inject.MovementSensor{} movementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { return TestPosition, 10, nil @@ -442,6 +456,31 @@ func getMovementSensorBothIMUAndOdometer() *inject.MovementSensor { return movementSensor } +func getMovementSensorBothIMUAndOdometerWithErroringFunctions() *inject.MovementSensor { + movementSensor := &inject.MovementSensor{} + movementSensor.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + return r3.Vector{}, errors.New(InvalidSensorTestErrMsg) + } + movementSensor.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { + return spatialmath.AngularVelocity{}, errors.New(InvalidSensorTestErrMsg) + } + movementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return &geo.Point{}, 0.0, errors.New(InvalidSensorTestErrMsg) + } + movementSensor.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + return &spatialmath.Quaternion{}, errors.New(InvalidSensorTestErrMsg) + } + movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + AngularVelocitySupported: true, + LinearAccelerationSupported: true, + PositionSupported: true, + OrientationSupported: true, + }, nil + } + return movementSensor +} + func getMovementSensorWithErroringPropertiesFunc() *inject.MovementSensor { movementSensor := &inject.MovementSensor{} movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { @@ -468,3 +507,69 @@ func getMovementSensorWithInvalidProperties() *inject.MovementSensor { } return movementSensor } + +func getReplayMovementSensor(testTime string) *inject.MovementSensor { + movementSensor := &inject.MovementSensor{} + movementSensor.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + md := ctx.Value(contextutils.MetadataContextKey) + if mdMap, ok := md.(map[string][]string); ok { + mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} + } + return TestLinAcc, nil + } + movementSensor.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { + md := ctx.Value(contextutils.MetadataContextKey) + if mdMap, ok := md.(map[string][]string); ok { + mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} + } + return TestAngVel, nil + } + movementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + md := ctx.Value(contextutils.MetadataContextKey) + if mdMap, ok := md.(map[string][]string); ok { + mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} + } + return TestPosition, 1.2, nil + } + movementSensor.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + md := ctx.Value(contextutils.MetadataContextKey) + if mdMap, ok := md.(map[string][]string); ok { + mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} + } + return TestOrientation, nil + } + movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + AngularVelocitySupported: true, + LinearAccelerationSupported: true, + PositionSupported: true, + OrientationSupported: true, + }, nil + } + return movementSensor +} + +func getFinishedReplayMovementSensor() *inject.MovementSensor { + movementSensor := &inject.MovementSensor{} + movementSensor.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + return r3.Vector{}, replay.ErrEndOfDataset + } + movementSensor.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { + return spatialmath.AngularVelocity{}, replay.ErrEndOfDataset + } + movementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return geo.NewPoint(0, 0), 0.0, replay.ErrEndOfDataset + } + movementSensor.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + return &spatialmath.Quaternion{}, replay.ErrEndOfDataset + } + movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + AngularVelocitySupported: true, + LinearAccelerationSupported: true, + PositionSupported: true, + OrientationSupported: true, + }, nil + } + return movementSensor +} From 27cbb3644fe3a731adbd9577bbe7ffad4c406a00 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Tue, 12 Dec 2023 15:04:28 -0500 Subject: [PATCH 13/21] Finish tests --- sensorprocess/movementsensorprocess_test.go | 129 ++------- sensorprocess/testhelper.go | 277 ++++++++++++++++++++ 2 files changed, 306 insertions(+), 100 deletions(-) diff --git a/sensorprocess/movementsensorprocess_test.go b/sensorprocess/movementsensorprocess_test.go index c97b366a..937bf339 100644 --- a/sensorprocess/movementsensorprocess_test.go +++ b/sensorprocess/movementsensorprocess_test.go @@ -181,126 +181,55 @@ func TestTryAddMovementSensorReadingUntilSuccess(t *testing.T) { func TestTryAddMovementSensorReadingOnce(t *testing.T) { logger := logging.NewTestLogger(t) cf := cartofacade.Mock{} - imuReading := s.TimedIMUReadingResponse{ - LinearAcceleration: r3.Vector{X: 1, Y: 1, Z: 1}, - AngularVelocity: spatialmath.AngularVelocity{X: 1, Y: 1, Z: 1}, - ReadingTime: time.Now().UTC(), - } - - reading := s.TimedMovementSensorReadingResponse{ - TimedIMUResponse: &imuReading, - } injectLidar := inject.TimedLidar{} injectLidar.DataFrequencyHzFunc = func() int { return 5 } - injectImu := inject.TimedMovementSensor{} - injectImu.NameFunc = func() string { return "good_imu" } - injectImu.DataFrequencyHzFunc = func() int { return 20 } - injectImu.PropertiesFunc = func() s.MovementSensorProperties { - return s.MovementSensorProperties{ - IMUSupported: true, - } - } + movementSensorName := "good_movement_sensor" + injectMovementSensor := inject.TimedMovementSensor{} + injectMovementSensor.NameFunc = func() string { return movementSensorName } + injectMovementSensor.DataFrequencyHzFunc = func() int { return 20 } config := Config{ Logger: logger, CartoFacade: &cf, IsOnline: injectLidar.DataFrequencyHzFunc() != 0, Lidar: &injectLidar, - MovementSensor: &injectImu, + MovementSensor: &injectMovementSensor, Timeout: 10 * time.Second, } - t.Run("when AddIMUReading blocks for more than the date rate and succeeds, time to sleep is 0", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return nil - } - - timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - - t.Run("when AddIMUReading is slower than date rate and returns a lock error, time to sleep is 0", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return cartofacade.ErrUnableToAcquireLock - } - - timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - - t.Run("when AddIMUReading blocks for more than the date rate and returns an unexpected error, time to sleep is 0", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return errUnknown - } - - timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - - t.Run("when AddIMUReading is faster than the date rate and succeeds, time to sleep is <= date rate", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - return nil + t.Run("imu only supported", func(t *testing.T) { + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + OdometerSupported: false, + } } - - timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + config.MovementSensor = &injectMovementSensor + TestTryAddMovementSensorReadingOnceTestHelper(context.Background(), t, config, cf) }) - t.Run("when AddIMUReading is faster than the date rate and returns a lock error, time to sleep is <= date rate", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - return cartofacade.ErrUnableToAcquireLock + t.Run("odometer only supported", func(t *testing.T) { + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + OdometerSupported: false, + } } - - timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + config.MovementSensor = &injectMovementSensor + TestTryAddMovementSensorReadingOnceTestHelper(context.Background(), t, config, cf) }) - t.Run("when AddIMUReading is faster than date rate "+ - "and returns an unexpected error, time to sleep is <= date rate", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - return errUnknown + t.Run("both imu and odometer supported", func(t *testing.T) { + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + OdometerSupported: false, + } } - - timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + config.MovementSensor = &injectMovementSensor + TestTryAddMovementSensorReadingOnceTestHelper(context.Background(), t, config, cf) }) } diff --git a/sensorprocess/testhelper.go b/sensorprocess/testhelper.go index ddba3629..f035cd8e 100644 --- a/sensorprocess/testhelper.go +++ b/sensorprocess/testhelper.go @@ -438,3 +438,280 @@ func validAddMovementSensorReadingUntilSuccessTestHelper( } } } + +func TestTryAddMovementSensorReadingOnceTestHelper( + ctx context.Context, + t *testing.T, + config Config, + cf cartofacade.Mock, +) { + config.CartoFacade = &cf + + // Set up movement sensor reading + now := time.Now().UTC() + var movementSensorReading s.TimedMovementSensorReadingResponse + if config.MovementSensor.Properties().IMUSupported { + movementSensorReading.TimedIMUResponse = &s.TimedIMUReadingResponse{ + AngularVelocity: s.TestAngVel, + LinearAcceleration: s.TestLinAcc, + ReadingTime: now, + } + } + if config.MovementSensor.Properties().OdometerSupported { + movementSensorReading.TimedOdometerResponse = &s.TimedOdometerReadingResponse{ + Position: s.TestPosition, + Orientation: s.TestOrientation, + ReadingTime: now, + } + } + + if config.MovementSensor.Properties().IMUSupported { + + // In case that the odometer is also supported, let's assume it works fast and efficiently for all the + // following test cases + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + return nil + } + + t.Run("when AddIMUReading blocks for more than the date rate and succeeds, time to sleep is 0", func(t *testing.T) { + var imuCalls []addIMUReadingArgs + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + time.Sleep(1 * time.Second) + args := addIMUReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + imuCalls = append(imuCalls, args) + return nil + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + + if config.MovementSensor.Properties().IMUSupported { + test.That(t, len(imuCalls), test.ShouldEqual, 1) + for i, call := range imuCalls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, config.MovementSensor.Name()) + test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, movementSensorReading.TimedIMUResponse.LinearAcceleration) + test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, movementSensorReading.TimedIMUResponse.AngularVelocity) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + } + } + }) + + t.Run("when AddIMUReading is slower than date rate and returns a lock error, time to sleep is 0", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddIMUReading blocks for more than the date rate and returns an unexpected error, time to sleep is 0", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return errUnknown + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddIMUReading is faster than the date rate and succeeds, time to sleep is <= date rate", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return nil + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + + t.Run("when AddIMUReading is faster than the date rate and returns a lock error, time to sleep is <= date rate", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + + t.Run("when AddIMUReading or AddOdometerReading are faster than date rate "+ + "and returns an unexpected error, time to sleep is <= date rate", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return errUnknown + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + + } + + if config.MovementSensor.Properties().OdometerSupported { + + // In case that the IMU is also supported, let's assume it works fast and efficiently for all the + // following test cases + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return errUnknown + } + + t.Run("when AddOdometerReading blocks for more than the date rate and succeeds, time to sleep is 0", func(t *testing.T) { + var odometerCalls []addOdometerReadingArgs + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + time.Sleep(1 * time.Second) + args := addOdometerReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + odometerCalls = append(odometerCalls, args) + return nil + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + + if config.MovementSensor.Properties().OdometerSupported { + test.That(t, len(odometerCalls), test.ShouldEqual, 1) + firstTimestamp := odometerCalls[0].currentReading.ReadingTime + for i, call := range odometerCalls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, config.MovementSensor.Name()) + test.That(t, call.currentReading.Position, test.ShouldResemble, movementSensorReading.TimedOdometerResponse.Position) + test.That(t, call.currentReading.Orientation, test.ShouldResemble, movementSensorReading.TimedOdometerResponse.Orientation) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + } + } + }) + + t.Run("when AddOdometerReading is slower than date rate and returns a lock error, time to sleep is 0", func(t *testing.T) { + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddOdometerReading blocks for more than the date rate and returns an unexpected error, time to sleep is 0", func(t *testing.T) { + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return errUnknown + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddOdometerReading are faster than the date rate and succeeds, time to sleep is <= date rate", func(t *testing.T) { + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + return nil + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + + t.Run("when AddOdometerReading are faster than the date rate and returns a lock error, time to sleep is <= date rate", func(t *testing.T) { + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + + t.Run("when AddOdometerReading are faster than date rate "+ + "and returns an unexpected error, time to sleep is <= date rate", func(t *testing.T) { + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + return errUnknown + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + + } +} From 45f715ff4f978839cb6bdd5ead11808e3df7eb1b Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Tue, 12 Dec 2023 15:15:15 -0500 Subject: [PATCH 14/21] Appease linter --- sensorprocess/movementsensorprocess_test.go | 50 ++++++++------------- sensorprocess/sensorprocess_test.go | 20 ++++++--- sensorprocess/testhelper.go | 26 +++++------ 3 files changed, 45 insertions(+), 51 deletions(-) diff --git a/sensorprocess/movementsensorprocess_test.go b/sensorprocess/movementsensorprocess_test.go index 937bf339..3bfca446 100644 --- a/sensorprocess/movementsensorprocess_test.go +++ b/sensorprocess/movementsensorprocess_test.go @@ -64,45 +64,29 @@ func TestAddMovementSensorReadingInOnline(t *testing.T) { } t.Run("returns error when LinearAcceleration or AngularVelocity return an error, doesn't try to add IMU data", func(t *testing.T) { - lidarFrequencyHz := 10 - movementSensorFrequencyHz := 10 - invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, - lidarFrequencyHz, s.IMUWithErroringFunctions, movementSensorFrequencyHz) + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, s.IMUWithErroringFunctions) }) t.Run("returns error when Position or Orientation return an error, doesn't try to add odometer data", func(t *testing.T) { - lidarFrequencyHz := 10 - movementSensorFrequencyHz := 10 - invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, - lidarFrequencyHz, s.OdometerWithErroringFunctions, movementSensorFrequencyHz) + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, s.OdometerWithErroringFunctions) }) - t.Run("returns error when LinearAcceleration, AngularVelocity, Position, and Orientation return an error, doesn't try to add movement sensor data", func(t *testing.T) { - lidarFrequencyHz := 10 - movementSensorFrequencyHz := 10 + t.Run("returns error when LinearAcceleration, AngularVelocity, Position, and Orientation return an error, "+ + "doesn't try to add movement sensor data", func(t *testing.T) { invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, - lidarFrequencyHz, s.MovementSensorBothIMUAndOdometerWithErroringFunctions, movementSensorFrequencyHz) + s.MovementSensorBothIMUAndOdometerWithErroringFunctions) }) t.Run("returns error when IMU replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { - lidarFrequencyHz := 10 - movementSensorFrequencyHz := 10 - invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, - lidarFrequencyHz, s.InvalidReplayIMU, movementSensorFrequencyHz) + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, s.InvalidReplayIMU) }) t.Run("returns error when odometer replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { - lidarFrequencyHz := 10 - movementSensorFrequencyHz := 10 - invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, - lidarFrequencyHz, s.InvalidReplayOdometer, movementSensorFrequencyHz) + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, s.InvalidReplayOdometer) }) t.Run("returns error when replay movement sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { - lidarFrequencyHz := 10 - movementSensorFrequencyHz := 10 - invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, - lidarFrequencyHz, s.InvalidReplayMovementSensorBothIMUAndOdometer, movementSensorFrequencyHz) + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, s.InvalidReplayMovementSensorBothIMUAndOdometer) }) t.Run("online replay IMU adds sensor reading once and ignores errors", func(t *testing.T) { @@ -122,7 +106,8 @@ func TestAddMovementSensorReadingInOnline(t *testing.T) { }) t.Run("online replay movement sensor adds sensor reading once and ignores errors", func(t *testing.T) { - imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.ReplayMovementSensorBothIMUAndOdometer) + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, + s.ReplayMovementSensorBothIMUAndOdometer) readingTime, err := time.Parse(time.RFC3339Nano, s.TestTimestamp) test.That(t, err, test.ShouldBeNil) test.That(t, imuCalls[0].currentReading.ReadingTime.Equal(readingTime), test.ShouldBeTrue) @@ -130,21 +115,24 @@ func TestAddMovementSensorReadingInOnline(t *testing.T) { }) t.Run("online IMU adds sensor reading once and ignores errors", func(t *testing.T) { - imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodIMU) + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, + s.GoodIMU) test.That(t, len(odometerCalls), test.ShouldBeZeroValue) test.That(t, imuCalls[0].currentReading.ReadingTime.Before(imuCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) test.That(t, imuCalls[1].currentReading.ReadingTime.Before(imuCalls[2].currentReading.ReadingTime), test.ShouldBeTrue) }) t.Run("online odometer adds sensor reading once and ignores errors", func(t *testing.T) { - imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodOdometer) + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, + s.GoodOdometer) test.That(t, len(imuCalls), test.ShouldBeZeroValue) test.That(t, odometerCalls[0].currentReading.ReadingTime.Before(odometerCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) test.That(t, odometerCalls[1].currentReading.ReadingTime.Before(odometerCalls[2].currentReading.ReadingTime), test.ShouldBeTrue) }) t.Run("online movement sensor adds sensor reading once and ignores errors", func(t *testing.T) { - imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodMovementSensorBothIMUAndOdometer) + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, + s.GoodMovementSensorBothIMUAndOdometer) test.That(t, imuCalls[0].currentReading.ReadingTime.Before(imuCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) test.That(t, imuCalls[1].currentReading.ReadingTime.Before(imuCalls[2].currentReading.ReadingTime), test.ShouldBeTrue) test.That(t, odometerCalls[0].currentReading.ReadingTime.Before(odometerCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) @@ -207,7 +195,7 @@ func TestTryAddMovementSensorReadingOnce(t *testing.T) { } } config.MovementSensor = &injectMovementSensor - TestTryAddMovementSensorReadingOnceTestHelper(context.Background(), t, config, cf) + tryAddMovementSensorReadingOnceTestHelper(t, config, cf) }) t.Run("odometer only supported", func(t *testing.T) { @@ -218,7 +206,7 @@ func TestTryAddMovementSensorReadingOnce(t *testing.T) { } } config.MovementSensor = &injectMovementSensor - TestTryAddMovementSensorReadingOnceTestHelper(context.Background(), t, config, cf) + tryAddMovementSensorReadingOnceTestHelper(t, config, cf) }) t.Run("both imu and odometer supported", func(t *testing.T) { @@ -229,7 +217,7 @@ func TestTryAddMovementSensorReadingOnce(t *testing.T) { } } config.MovementSensor = &injectMovementSensor - TestTryAddMovementSensorReadingOnceTestHelper(context.Background(), t, config, cf) + tryAddMovementSensorReadingOnceTestHelper(t, config, cf) }) } diff --git a/sensorprocess/sensorprocess_test.go b/sensorprocess/sensorprocess_test.go index 0c38ef25..0809b61b 100644 --- a/sensorprocess/sensorprocess_test.go +++ b/sensorprocess/sensorprocess_test.go @@ -175,7 +175,8 @@ func TestStartOfflineSensorProcess(t *testing.T) { expectedDataInsertions: []string{"lidar: 2", "odometer: 3", "lidar: 4", "odometer: 5"}, }, { - description: "if movement sensor data ends before lidar data ends, stop adding data once end of movement sensor dataset is reached", + description: "if movement sensor data ends before lidar data ends, stop adding data once end " + + "of movement sensor dataset is reached", imuEnabled: true, odometerEnabled: true, lidarReadingTimeAddedMs: []int{2, 4, 6, 8, 10, 12}, @@ -204,7 +205,10 @@ func TestStartOfflineSensorProcess(t *testing.T) { odometerEnabled: true, lidarReadingTimeAddedMs: []int{1, 3, 5}, msReadingTimeAddedMs: []int{2, 3, 4, 6, 8, 10, 12}, - expectedDataInsertions: []string{"lidar: 1", "odometer: 2", "imu: 2", "lidar: 3", "odometer: 3", "imu: 3", "odometer: 4", "imu: 4", "lidar: 5"}, + expectedDataInsertions: []string{ + "lidar: 1", "odometer: 2", "imu: 2", "lidar: 3", "odometer: 3", + "imu: 3", "odometer: 4", "imu: 4", "lidar: 5", + }, }, } @@ -234,11 +238,13 @@ func TestStartOfflineSensorProcess(t *testing.T) { if numMovementSensorData < len(tt.msReadingTimeAddedMs) { if tt.odometerEnabled { movementSensorReading.TimedOdometerResponse = &odometerReading - movementSensorReading.TimedOdometerResponse.ReadingTime = now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) + odometerReadingTime := now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) + movementSensorReading.TimedOdometerResponse.ReadingTime = odometerReadingTime } if tt.imuEnabled { movementSensorReading.TimedIMUResponse = &imuReading - movementSensorReading.TimedIMUResponse.ReadingTime = now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) + imuReadingTime := now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) + movementSensorReading.TimedIMUResponse.ReadingTime = imuReadingTime } numMovementSensorData++ return movementSensorReading, nil @@ -433,11 +439,13 @@ func TestGetInitialMovementSensorReading(t *testing.T) { if numMovementSensorData < len(tt.msReadingTimeAddedMs) { if tt.odometerEnabled { movementSensorReading.TimedOdometerResponse = &odometerReading - movementSensorReading.TimedOdometerResponse.ReadingTime = now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) + odometerReadingTime := now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) + movementSensorReading.TimedOdometerResponse.ReadingTime = odometerReadingTime } if tt.imuEnabled { movementSensorReading.TimedIMUResponse = &imuReading - movementSensorReading.TimedIMUResponse.ReadingTime = now.Add(time.Duration(float64(tt.msReadingTimeAddedMs[numMovementSensorData])+0.1) * time.Millisecond) + imuReadingTime := now.Add(time.Duration(float64(tt.msReadingTimeAddedMs[numMovementSensorData])+0.1) * time.Millisecond) + movementSensorReading.TimedIMUResponse.ReadingTime = imuReadingTime } numMovementSensorData++ return movementSensorReading, nil diff --git a/sensorprocess/testhelper.go b/sensorprocess/testhelper.go index f035cd8e..f735d142 100644 --- a/sensorprocess/testhelper.go +++ b/sensorprocess/testhelper.go @@ -8,12 +8,12 @@ import ( "go.viam.com/rdk/logging" "go.viam.com/rdk/spatialmath" + rdkutils "go.viam.com/rdk/utils" "go.viam.com/test" "github.com/viamrobotics/viam-cartographer/cartofacade" s "github.com/viamrobotics/viam-cartographer/sensors" "github.com/viamrobotics/viam-cartographer/sensors/inject" - rdkutils "go.viam.com/rdk/utils" ) type addLidarReadingArgs struct { @@ -280,13 +280,13 @@ func invalidAddMovementSensorReadingInOnlineTestHelper( t *testing.T, cartoFacadeMock cartofacade.Mock, config Config, - lidarDataFrequencyHz int, testMovementSensor s.TestSensor, - movementSensorDataFrequencyHz int, ) { logger := logging.NewTestLogger(t) + lidarFrequencyHz := 10 + movementSensorFrequencyHz := 10 movementSensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(s.NoLidar, testMovementSensor), - string(testMovementSensor), movementSensorDataFrequencyHz, logger) + string(testMovementSensor), movementSensorFrequencyHz, logger) test.That(t, err, test.ShouldBeNil) var imuCalls []addIMUReadingArgs @@ -323,7 +323,7 @@ func invalidAddMovementSensorReadingInOnlineTestHelper( config.CartoFacade = &cartoFacadeMock injectLidar := inject.TimedLidar{} - injectLidar.DataFrequencyHzFunc = func() int { return lidarDataFrequencyHz } + injectLidar.DataFrequencyHzFunc = func() int { return lidarFrequencyHz } config.Lidar = &injectLidar config.MovementSensor = movementSensor @@ -412,7 +412,8 @@ func validAddMovementSensorReadingUntilSuccessTestHelper( } } - config.tryAddMovementSensorReadingUntilSuccess(ctx, movementSensorReading) + err = config.tryAddMovementSensorReadingUntilSuccess(ctx, movementSensorReading) + test.That(t, err, test.ShouldBeNil) if movementSensor.Properties().IMUSupported { test.That(t, len(imuCalls), test.ShouldEqual, 3) firstTimestamp := imuCalls[0].currentReading.ReadingTime @@ -439,8 +440,7 @@ func validAddMovementSensorReadingUntilSuccessTestHelper( } } -func TestTryAddMovementSensorReadingOnceTestHelper( - ctx context.Context, +func tryAddMovementSensorReadingOnceTestHelper( t *testing.T, config Config, cf cartofacade.Mock, @@ -466,7 +466,6 @@ func TestTryAddMovementSensorReadingOnceTestHelper( } if config.MovementSensor.Properties().IMUSupported { - // In case that the odometer is also supported, let's assume it works fast and efficiently for all the // following test cases cf.AddOdometerReadingFunc = func( @@ -586,11 +585,9 @@ func TestTryAddMovementSensorReadingOnceTestHelper( test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) }) - } if config.MovementSensor.Properties().OdometerSupported { - // In case that the IMU is also supported, let's assume it works fast and efficiently for all the // following test cases cf.AddIMUReadingFunc = func( @@ -652,7 +649,8 @@ func TestTryAddMovementSensorReadingOnceTestHelper( test.That(t, timeToSleep, test.ShouldEqual, 0) }) - t.Run("when AddOdometerReading blocks for more than the date rate and returns an unexpected error, time to sleep is 0", func(t *testing.T) { + t.Run("when AddOdometerReading blocks for more than the date rate and returns an unexpected error, "+ + "time to sleep is 0", func(t *testing.T) { cf.AddOdometerReadingFunc = func( ctx context.Context, timeout time.Duration, @@ -682,7 +680,8 @@ func TestTryAddMovementSensorReadingOnceTestHelper( test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) }) - t.Run("when AddOdometerReading are faster than the date rate and returns a lock error, time to sleep is <= date rate", func(t *testing.T) { + t.Run("when AddOdometerReading are faster than the date rate and returns a lock error, "+ + "time to sleep is <= date rate", func(t *testing.T) { cf.AddOdometerReadingFunc = func( ctx context.Context, timeout time.Duration, @@ -712,6 +711,5 @@ func TestTryAddMovementSensorReadingOnceTestHelper( test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) }) - } } From 6419e70f5f61271cf55c18cfa31b9b4677523e9d Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Tue, 19 Dec 2023 15:10:43 -0500 Subject: [PATCH 15/21] Fix comment --- sensors/movementsensor.go | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/sensors/movementsensor.go b/sensors/movementsensor.go index 5a7b2009..39aee587 100644 --- a/sensors/movementsensor.go +++ b/sensors/movementsensor.go @@ -58,8 +58,7 @@ type TimedMovementSensorReadingResponse struct { // TimedIMUReadingResponse represents an IMU sensor reading with a time. type TimedIMUReadingResponse struct { - // TODO[kat]: AngularVelocity is currently in radians. Either leave a comment, or change this. - AngularVelocity spatialmath.AngularVelocity + AngularVelocity spatialmath.AngularVelocity // We set the values in radians/s instead of deg/s LinearAcceleration r3.Vector ReadingTime time.Time } From 0af8396cb92a32e009d38f5eb248beb6a2990497 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Wed, 3 Jan 2024 11:49:25 +0100 Subject: [PATCH 16/21] Fix PR comments --- config/config.go | 6 +++-- sensorprocess/movementsensorprocess.go | 2 ++ sensorprocess/sensorprocess.go | 37 +++++++++++++++----------- sensorprocess/sensorprocess_test.go | 4 +-- sensorprocess/testhelper.go | 4 +-- sensors/test_deps.go | 29 +++++++++++--------- viam_cartographer.go | 10 +++---- 7 files changed, 54 insertions(+), 38 deletions(-) diff --git a/config/config.go b/config/config.go index 9840391d..1347d636 100644 --- a/config/config.go +++ b/config/config.go @@ -38,7 +38,8 @@ type OptionalConfigParams struct { var ( errCameraMustHaveName = errors.New("\"camera[name]\" is required") - errLocalizationInOfflineMode = newError("camera[data_freq_hz] and enable_mapping = false. localization in offline mode not supported.") + errModeHasToBeProvided = errors.New("\"mode\" config parameter is required") + errLocalizationInOfflineMode = newError("\"camera[data_freq_hz]\" and enable_mapping = false. localization in offline mode not supported.") ) // Validate creates the list of implicit dependencies. @@ -65,7 +66,8 @@ func (config *Config) Validate(path string) ([]string, error) { deps = append(deps, movementSensorName) } - if config.ConfigParams["mode"] == "" { + mode, ok := config.ConfigParams["mode"] + if !ok || mode == "" { return nil, utils.NewConfigValidationFieldRequiredError(path, "config_params[mode]") } diff --git a/sensorprocess/movementsensorprocess.go b/sensorprocess/movementsensorprocess.go index de9d3d69..c731b12c 100644 --- a/sensorprocess/movementsensorprocess.go +++ b/sensorprocess/movementsensorprocess.go @@ -56,9 +56,11 @@ func (config *Config) addMovementSensorReadingInOnline(ctx context.Context) erro // process each reading so if we cannot acquire the lock we should try again. func (config *Config) tryAddMovementSensorReadingUntilSuccess(ctx context.Context, reading s.TimedMovementSensorReadingResponse) error { var imuDone, odometerDone bool + // set IMU as done since it is not supported: we won't attempt to add IMU data to cartographer if !config.MovementSensor.Properties().IMUSupported { imuDone = true } + // set odometer as done since it is not supported: we won't attempt to add odometer data to cartographer if !config.MovementSensor.Properties().OdometerSupported { odometerDone = true } diff --git a/sensorprocess/sensorprocess.go b/sensorprocess/sensorprocess.go index c09c794c..90a46479 100644 --- a/sensorprocess/sensorprocess.go +++ b/sensorprocess/sensorprocess.go @@ -16,6 +16,18 @@ import ( s "github.com/viamrobotics/viam-cartographer/sensors" ) +type sensorType int64 + +const ( + lidar sensorType = iota + movementSensor +) + +type offlineSensorReadingTime struct { + sensorType sensorType + readingTime time.Time +} + // Config holds config needed throughout the process of adding a sensor reading to the cartofacade. type Config struct { CartoFacade cartofacade.Interface @@ -82,11 +94,6 @@ func (config *Config) StartOfflineSensorProcess(ctx context.Context) bool { } } - type readingTime struct { - sensorType string - readingTime time.Time - } - // loop over all the data until one of the datasets has reached its end for { select { @@ -94,23 +101,23 @@ func (config *Config) StartOfflineSensorProcess(ctx context.Context) bool { return false default: // create a map of supported sensors and their reading time stamps - readingTimes := []readingTime{ - {sensorType: "lidar", readingTime: lidarReading.ReadingTime}, + readingTimes := []offlineSensorReadingTime{ + {sensorType: lidar, readingTime: lidarReading.ReadingTime}, } - // default to the slightly later imu timestamp, in case that the odometer time stamp was + // default to the slightly later imu timestamp: in case that the odometer time stamp was // taken before the lidar time stamp, but the imu time stamp was taken after the lidar time // stamp, we'll want to prioritize adding the lidar measurement before adding the movement // sensor measurement if config.MovementSensor != nil && config.MovementSensor.Properties().IMUSupported { readingTimes = append(readingTimes, - readingTime{ - sensorType: "movement-sensor", + offlineSensorReadingTime{ + sensorType: movementSensor, readingTime: movementSensorReading.TimedIMUResponse.ReadingTime, }) } else if config.MovementSensor != nil && config.MovementSensor.Properties().OdometerSupported { readingTimes = append(readingTimes, - readingTime{ - sensorType: "movement-sensor", + offlineSensorReadingTime{ + sensorType: movementSensor, readingTime: movementSensorReading.TimedOdometerResponse.ReadingTime, }) } @@ -121,14 +128,14 @@ func (config *Config) StartOfflineSensorProcess(ctx context.Context) bool { // if the timestamps are the same, we want to prioritize the lidar measurement before // the movement sensor measurement if readingTimes[i].readingTime.Equal(readingTimes[j].readingTime) { - return readingTimes[i].sensorType == "lidar" + return readingTimes[i].sensorType == lidar } return readingTimes[i].readingTime.Before(readingTimes[j].readingTime) }) // insert the reading with the earliest time stamp switch readingTimes[0].sensorType { - case "lidar": + case lidar: if err := config.tryAddLidarReadingUntilSuccess(ctx, lidarReading); err != nil { return false } @@ -142,7 +149,7 @@ func (config *Config) StartOfflineSensorProcess(ctx context.Context) bool { } return lidarEndOfDataSetReached } - case "movement-sensor": + case movementSensor: if err := config.tryAddMovementSensorReadingUntilSuccess(ctx, movementSensorReading); err != nil { return false } diff --git a/sensorprocess/sensorprocess_test.go b/sensorprocess/sensorprocess_test.go index 0809b61b..966cd2b7 100644 --- a/sensorprocess/sensorprocess_test.go +++ b/sensorprocess/sensorprocess_test.go @@ -144,8 +144,8 @@ func TestStartOfflineSensorProcess(t *testing.T) { }{ { description: "no movement sensor data is added if movement sensor is not enabled", - odometerEnabled: false, imuEnabled: false, + odometerEnabled: false, lidarReadingTimeAddedMs: []int{0, 2, 4, 6, 8}, msReadingTimeAddedMs: []int{1, 3, 5}, expectedDataInsertions: []string{"lidar: 0", "lidar: 2", "lidar: 4", "lidar: 6", "lidar: 8"}, @@ -249,7 +249,7 @@ func TestStartOfflineSensorProcess(t *testing.T) { numMovementSensorData++ return movementSensorReading, nil } - return movementSensorReading, replay.ErrEndOfDataset + return s.TimedMovementSensorReadingResponse{}, replay.ErrEndOfDataset } injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { return s.MovementSensorProperties{ diff --git a/sensorprocess/testhelper.go b/sensorprocess/testhelper.go index f735d142..43ea2ab9 100644 --- a/sensorprocess/testhelper.go +++ b/sensorprocess/testhelper.go @@ -248,7 +248,7 @@ func validAddMovementSensorReadingInOnlineTestHelper( for i, call := range imuCalls { t.Logf("call %d", i) test.That(t, call.sensorName, test.ShouldResemble, string(testMovementSensor)) - // the IMU test fixture happens to always return the same readings currently + // the IMU test fixture happens to always return the same readings currently; // in reality they are likely different every time test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, s.TestLinAcc) test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, spatialmath.AngularVelocity{ @@ -264,7 +264,7 @@ func validAddMovementSensorReadingInOnlineTestHelper( for i, call := range odometerCalls { t.Logf("call %d", i) test.That(t, call.sensorName, test.ShouldResemble, string(testMovementSensor)) - // the odometer test fixture happens to always return the same readings currently + // the odometer test fixture happens to always return the same readings currently; // in reality they are likely different every time test.That(t, call.currentReading.Position, test.ShouldResemble, s.TestPosition) test.That(t, call.currentReading.Orientation, test.ShouldResemble, s.TestOrientation) diff --git a/sensors/test_deps.go b/sensors/test_deps.go index fb04fb9c..14466378 100644 --- a/sensors/test_deps.go +++ b/sensors/test_deps.go @@ -2,6 +2,7 @@ package sensors import ( "context" + "math" "time" "github.com/golang/geo/r3" @@ -43,6 +44,7 @@ const ( // InvalidSensorTestErrMsg represents an error message that indicates that the sensor is invalid. InvalidSensorTestErrMsg = "invalid test sensor" + // ---------- LIDAR Test Sensors -------------- // GoodLidar is a lidar that works as expected and returns a pointcloud. GoodLidar TestSensor = "good_lidar" // WarmingUpLidar is a lidar whose NextPointCloud function returns a "warming up" error. @@ -63,6 +65,7 @@ const ( // FinishedReplayLidar is a lidar whose NextPointCloud function returns an end of dataset error. FinishedReplayLidar TestSensor = "finished_replay_lidar" + // ------------- IMU Test Sensors --------------- // GoodIMU is an IMU that works as expected and returns linear acceleration and angular velocity values. GoodIMU TestSensor = "good_imu" // IMUWithErroringFunctions is an IMU whose functions return errors. @@ -76,6 +79,7 @@ const ( // dataset error. FinishedReplayIMU TestSensor = "finished_replay_imu" + // -------------- ODOMETER Test Sensors ------------ // GoodOdometer is an odometer that works as expected and returns position and orientation values. GoodOdometer TestSensor = "good_odometer" // OdometerWithErroringFunctions is an Odometer whose functions return errors. @@ -89,6 +93,7 @@ const ( // dataset error. FinishedReplayOdometer TestSensor = "finished_replay_odometer" + // ------------- IMU + ODOMETER Test Sensors ---------- // MovementSensorNotIMUNotOdometer is a movement sensor that does neither support an IMU nor an odometer. MovementSensorNotIMUNotOdometer TestSensor = "movement_sensor_not_imu_not_odometer" // GoodMovementSensorBothIMUAndOdometer is a movement sensor that supports both an IMU nor an odometer. @@ -350,7 +355,7 @@ func getFinishedReplayIMU() *inject.MovementSensor { func getGoodOdometer() *inject.MovementSensor { odometer := &inject.MovementSensor{} odometer.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return TestPosition, 10, nil + return TestPosition, 0.0, nil } odometer.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { return TestOrientation, nil @@ -367,10 +372,10 @@ func getGoodOdometer() *inject.MovementSensor { func getOdometerWithErroringFunctions() *inject.MovementSensor { odometer := &inject.MovementSensor{} odometer.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return &geo.Point{}, 0.0, errors.New(InvalidSensorTestErrMsg) + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), errors.New(InvalidSensorTestErrMsg) } odometer.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - return &spatialmath.Quaternion{}, errors.New(InvalidSensorTestErrMsg) + return nil, errors.New(InvalidSensorTestErrMsg) } odometer.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -388,7 +393,7 @@ func getReplayOdometer(testTime string) *inject.MovementSensor { if mdMap, ok := md.(map[string][]string); ok { mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} } - return TestPosition, 1.2, nil + return TestPosition, 0.0, nil } odometer.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { md := ctx.Value(contextutils.MetadataContextKey) @@ -409,10 +414,10 @@ func getReplayOdometer(testTime string) *inject.MovementSensor { func getFinishedReplayOdometer() *inject.MovementSensor { odometer := &inject.MovementSensor{} odometer.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return geo.NewPoint(0, 0), 0.0, replay.ErrEndOfDataset + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), replay.ErrEndOfDataset } odometer.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - return &spatialmath.Quaternion{}, replay.ErrEndOfDataset + return nil, replay.ErrEndOfDataset } odometer.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -434,7 +439,7 @@ func getMovementSensorNotIMUAndNotOdometer() *inject.MovementSensor { func getGoodMovementSensorBothIMUAndOdometer() *inject.MovementSensor { movementSensor := &inject.MovementSensor{} movementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return TestPosition, 10, nil + return TestPosition, 0.0, nil } movementSensor.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { return TestOrientation, nil @@ -465,10 +470,10 @@ func getMovementSensorBothIMUAndOdometerWithErroringFunctions() *inject.Movement return spatialmath.AngularVelocity{}, errors.New(InvalidSensorTestErrMsg) } movementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return &geo.Point{}, 0.0, errors.New(InvalidSensorTestErrMsg) + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), errors.New(InvalidSensorTestErrMsg) } movementSensor.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - return &spatialmath.Quaternion{}, errors.New(InvalidSensorTestErrMsg) + return nil, errors.New(InvalidSensorTestErrMsg) } movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -529,7 +534,7 @@ func getReplayMovementSensor(testTime string) *inject.MovementSensor { if mdMap, ok := md.(map[string][]string); ok { mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} } - return TestPosition, 1.2, nil + return TestPosition, 0.0, nil } movementSensor.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { md := ctx.Value(contextutils.MetadataContextKey) @@ -558,10 +563,10 @@ func getFinishedReplayMovementSensor() *inject.MovementSensor { return spatialmath.AngularVelocity{}, replay.ErrEndOfDataset } movementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return geo.NewPoint(0, 0), 0.0, replay.ErrEndOfDataset + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), replay.ErrEndOfDataset } movementSensor.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - return &spatialmath.Quaternion{}, replay.ErrEndOfDataset + return nil, replay.ErrEndOfDataset } movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ diff --git a/viam_cartographer.go b/viam_cartographer.go index 608ec630..6c2cff8d 100644 --- a/viam_cartographer.go +++ b/viam_cartographer.go @@ -508,7 +508,7 @@ func (cartoSvc *CartographerService) Position(ctx context.Context) (spatialmath. cartoSvcClosed := cartoSvc.closed cartoSvc.muClose.Unlock() if cartoSvcClosed { - cartoSvc.logger.Warn("Position called after closed") + cartoSvc.logger.Warn("Position called after shutting down of cartographer has been initiated") return nil, "", ErrClosed } @@ -543,7 +543,7 @@ func (cartoSvc *CartographerService) PointCloudMap(ctx context.Context) (func() cartoSvcClosed := cartoSvc.closed cartoSvc.muClose.Unlock() if cartoSvcClosed { - cartoSvc.logger.Warn("PointCloudMap called after closed") + cartoSvc.logger.Warn("PointCloudMap called after shutting down of cartographer has been initiated") return nil, ErrClosed } @@ -568,7 +568,7 @@ func (cartoSvc *CartographerService) InternalState(ctx context.Context) (func() cartoSvcClosed := cartoSvc.closed cartoSvc.muClose.Unlock() if cartoSvcClosed { - cartoSvc.logger.Warn("InternalState called after closed") + cartoSvc.logger.Warn("InternalState called after shutting down of cartographer has been initiated") return nil, ErrClosed } @@ -609,7 +609,7 @@ func (cartoSvc *CartographerService) LatestMapInfo(ctx context.Context) (time.Ti cartoSvcClosed := cartoSvc.closed cartoSvc.muClose.Unlock() if cartoSvcClosed { - cartoSvc.logger.Warn("LatestMapInfo called after closed") + cartoSvc.logger.Warn("LatestMapInfo called after shutting down of cartographer has been initiated") return time.Time{}, ErrClosed } @@ -631,7 +631,7 @@ func (cartoSvc *CartographerService) DoCommand(ctx context.Context, req map[stri cartoSvcClosed := cartoSvc.closed cartoSvc.muClose.Unlock() if cartoSvcClosed { - cartoSvc.logger.Warn("DoCommand called after closed") + cartoSvc.logger.Warn("DoCommand called after shutting down of cartographer has been initiated") return nil, ErrClosed } From 3878c1ddfea4d74196b3a9959f37d927b9ca93a8 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Wed, 3 Jan 2024 11:50:51 +0100 Subject: [PATCH 17/21] Appease linter --- config/config.go | 1 - sensors/test_deps.go | 12 ++++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/config/config.go b/config/config.go index 1347d636..cdd5f648 100644 --- a/config/config.go +++ b/config/config.go @@ -38,7 +38,6 @@ type OptionalConfigParams struct { var ( errCameraMustHaveName = errors.New("\"camera[name]\" is required") - errModeHasToBeProvided = errors.New("\"mode\" config parameter is required") errLocalizationInOfflineMode = newError("\"camera[data_freq_hz]\" and enable_mapping = false. localization in offline mode not supported.") ) diff --git a/sensors/test_deps.go b/sensors/test_deps.go index 14466378..6948da8d 100644 --- a/sensors/test_deps.go +++ b/sensors/test_deps.go @@ -44,7 +44,8 @@ const ( // InvalidSensorTestErrMsg represents an error message that indicates that the sensor is invalid. InvalidSensorTestErrMsg = "invalid test sensor" - // ---------- LIDAR Test Sensors -------------- + // ---------- LIDAR Test Sensors --------------. + // GoodLidar is a lidar that works as expected and returns a pointcloud. GoodLidar TestSensor = "good_lidar" // WarmingUpLidar is a lidar whose NextPointCloud function returns a "warming up" error. @@ -65,7 +66,8 @@ const ( // FinishedReplayLidar is a lidar whose NextPointCloud function returns an end of dataset error. FinishedReplayLidar TestSensor = "finished_replay_lidar" - // ------------- IMU Test Sensors --------------- + // ------------- IMU Test Sensors ---------------. + // GoodIMU is an IMU that works as expected and returns linear acceleration and angular velocity values. GoodIMU TestSensor = "good_imu" // IMUWithErroringFunctions is an IMU whose functions return errors. @@ -79,7 +81,8 @@ const ( // dataset error. FinishedReplayIMU TestSensor = "finished_replay_imu" - // -------------- ODOMETER Test Sensors ------------ + // -------------- ODOMETER Test Sensors ------------. + // GoodOdometer is an odometer that works as expected and returns position and orientation values. GoodOdometer TestSensor = "good_odometer" // OdometerWithErroringFunctions is an Odometer whose functions return errors. @@ -93,7 +96,8 @@ const ( // dataset error. FinishedReplayOdometer TestSensor = "finished_replay_odometer" - // ------------- IMU + ODOMETER Test Sensors ---------- + // ------------- IMU + ODOMETER Test Sensors ----------. + // MovementSensorNotIMUNotOdometer is a movement sensor that does neither support an IMU nor an odometer. MovementSensorNotIMUNotOdometer TestSensor = "movement_sensor_not_imu_not_odometer" // GoodMovementSensorBothIMUAndOdometer is a movement sensor that supports both an IMU nor an odometer. From 043ff6772070c4f1510b527adffe3de5ae8d578d Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Fri, 5 Jan 2024 11:13:34 -0500 Subject: [PATCH 18/21] Update config/config.go Co-authored-by: jeremyrhyde <34897732+jeremyrhyde@users.noreply.github.com> --- config/config.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/config.go b/config/config.go index cdd5f648..218b6ac3 100644 --- a/config/config.go +++ b/config/config.go @@ -38,7 +38,7 @@ type OptionalConfigParams struct { var ( errCameraMustHaveName = errors.New("\"camera[name]\" is required") - errLocalizationInOfflineMode = newError("\"camera[data_freq_hz]\" and enable_mapping = false. localization in offline mode not supported.") + errLocalizationInOfflineMode = newError("\"camera[data_freq_hz]\" and enable_mapping = false. Localization in offline mode is not supported.") ) // Validate creates the list of implicit dependencies. From eb12c24ede49543840ce226c9db75c3e78692cc7 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Fri, 5 Jan 2024 17:37:36 +0100 Subject: [PATCH 19/21] Appease linter --- config/config.go | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/config/config.go b/config/config.go index 218b6ac3..24fb985f 100644 --- a/config/config.go +++ b/config/config.go @@ -38,7 +38,8 @@ type OptionalConfigParams struct { var ( errCameraMustHaveName = errors.New("\"camera[name]\" is required") - errLocalizationInOfflineMode = newError("\"camera[data_freq_hz]\" and enable_mapping = false. Localization in offline mode is not supported.") + errLocalizationInOfflineMode = newError("\"camera[data_freq_hz]\" and enable_mapping = false." + + " Localization in offline mode is not supported.") ) // Validate creates the list of implicit dependencies. From dc9d8fd22814fd7f4c5b10ca43546caef78e44a7 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Mon, 8 Jan 2024 12:05:20 +0100 Subject: [PATCH 20/21] Revert mutex changes --- viam_cartographer.go | 33 +++++++++------------------------ 1 file changed, 9 insertions(+), 24 deletions(-) diff --git a/viam_cartographer.go b/viam_cartographer.go index 6c2cff8d..27d4f2a7 100644 --- a/viam_cartographer.go +++ b/viam_cartographer.go @@ -471,8 +471,8 @@ type CartographerService struct { movementSensor s.TimedMovementSensor subAlgo SubAlgo - muClose sync.Mutex - closed bool + mu sync.Mutex + closed bool configParams map[string]string @@ -504,10 +504,7 @@ func (cartoSvc *CartographerService) Position(ctx context.Context) (spatialmath. return nil, "", ErrUseCloudSlamEnabled } - cartoSvc.muClose.Lock() - cartoSvcClosed := cartoSvc.closed - cartoSvc.muClose.Unlock() - if cartoSvcClosed { + if cartoSvc.closed { cartoSvc.logger.Warn("Position called after shutting down of cartographer has been initiated") return nil, "", ErrClosed } @@ -539,10 +536,7 @@ func (cartoSvc *CartographerService) PointCloudMap(ctx context.Context) (func() return nil, ErrUseCloudSlamEnabled } - cartoSvc.muClose.Lock() - cartoSvcClosed := cartoSvc.closed - cartoSvc.muClose.Unlock() - if cartoSvcClosed { + if cartoSvc.closed { cartoSvc.logger.Warn("PointCloudMap called after shutting down of cartographer has been initiated") return nil, ErrClosed } @@ -564,10 +558,7 @@ func (cartoSvc *CartographerService) InternalState(ctx context.Context) (func() return nil, ErrUseCloudSlamEnabled } - cartoSvc.muClose.Lock() - cartoSvcClosed := cartoSvc.closed - cartoSvc.muClose.Unlock() - if cartoSvcClosed { + if cartoSvc.closed { cartoSvc.logger.Warn("InternalState called after shutting down of cartographer has been initiated") return nil, ErrClosed } @@ -605,10 +596,7 @@ func (cartoSvc *CartographerService) LatestMapInfo(ctx context.Context) (time.Ti return time.Time{}, ErrUseCloudSlamEnabled } - cartoSvc.muClose.Lock() - cartoSvcClosed := cartoSvc.closed - cartoSvc.muClose.Unlock() - if cartoSvcClosed { + if cartoSvc.closed { cartoSvc.logger.Warn("LatestMapInfo called after shutting down of cartographer has been initiated") return time.Time{}, ErrClosed } @@ -627,10 +615,7 @@ func (cartoSvc *CartographerService) DoCommand(ctx context.Context, req map[stri return nil, ErrUseCloudSlamEnabled } - cartoSvc.muClose.Lock() - cartoSvcClosed := cartoSvc.closed - cartoSvc.muClose.Unlock() - if cartoSvcClosed { + if cartoSvc.closed { cartoSvc.logger.Warn("DoCommand called after shutting down of cartographer has been initiated") return nil, ErrClosed } @@ -644,8 +629,8 @@ func (cartoSvc *CartographerService) DoCommand(ctx context.Context, req map[stri // Close out of all slam related processes. func (cartoSvc *CartographerService) Close(ctx context.Context) error { - cartoSvc.muClose.Lock() - defer cartoSvc.muClose.Unlock() + cartoSvc.mu.Lock() + defer cartoSvc.mu.Unlock() if cartoSvc.useCloudSlam { return nil } From c00990af3101aaa35fb74283093cb7dd05362c77 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Mon, 8 Jan 2024 12:14:30 +0100 Subject: [PATCH 21/21] Fix order of vars --- viam_cartographer.go | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/viam_cartographer.go b/viam_cartographer.go index 27d4f2a7..acc664c1 100644 --- a/viam_cartographer.go +++ b/viam_cartographer.go @@ -466,14 +466,13 @@ func terminateCartoFacade(ctx context.Context, cartoSvc *CartographerService) er type CartographerService struct { resource.Named resource.AlwaysRebuild + mu sync.Mutex SlamMode cartofacade.SlamMode + closed bool lidar s.TimedLidar movementSensor s.TimedMovementSensor subAlgo SubAlgo - mu sync.Mutex - closed bool - configParams map[string]string cartofacade cartofacade.Interface