From 7c69c3a8a42f7606c2a24da8d8a81932a181cf91 Mon Sep 17 00:00:00 2001 From: jeremyrhyde <34897732+jeremyrhyde@users.noreply.github.com> Date: Tue, 15 Aug 2023 14:58:59 -0400 Subject: [PATCH] RSDK-4306 Add Properties Check of LiDAR during Initialization (#245) --- sensorprocess/sensorprocess_test.go | 2 +- sensors/sensors.go | 20 ++++++------ sensors/sensors_test.go | 31 ++++++------------- sensors/test_deps.go | 47 ++++++++++++++++++++--------- testhelper/testhelper.go | 5 +++ viam_cartographer_test.go | 33 +++++++++++++++++--- 6 files changed, 87 insertions(+), 51 deletions(-) diff --git a/sensorprocess/sensorprocess_test.go b/sensorprocess/sensorprocess_test.go index 0868bb31..954fefd9 100644 --- a/sensorprocess/sensorprocess_test.go +++ b/sensorprocess/sensorprocess_test.go @@ -366,7 +366,7 @@ func TestAddSensorReading(t *testing.T) { ctx := context.Background() t.Run("returns error in online mode when lidar GetData returns error, doesn't try to add lidar data", func(t *testing.T) { - cam := "invalid_lidar" + cam := "lidar_with_erroring_functions" invalidSensorTestHelper( ctx, t, diff --git a/sensors/sensors.go b/sensors/sensors.go index e2f8d8a3..d75d14a8 100644 --- a/sensors/sensors.go +++ b/sensors/sensors.go @@ -72,18 +72,16 @@ func NewLidar( return Lidar{}, errors.Wrapf(err, "error getting lidar camera %v for slam service", cameraName) } - // https://viam.atlassian.net/browse/RSDK-4306 - // To be implemented once replay camera supports Properties - // // If there is a camera provided in the 'camera' field, we enforce that it supports PCD. - // properties, err := newLidar.Properties(ctx) - // if err != nil { - // return Lidar{}, errors.Wrapf(err, "error getting lidar camera properties %v for slam service", cameraName) - // } + // If there is a camera provided in the 'camera' field, we enforce that it supports PCD. + properties, err := newLidar.Properties(ctx) + if err != nil { + return Lidar{}, errors.Wrapf(err, "error getting lidar camera properties %v for slam service", cameraName) + } - // if !properties.SupportsPCD { - // return Lidar{}, errors.New("configuring lidar camera error: " + - // "'camera' must support PCD") - // } + if !properties.SupportsPCD { + return Lidar{}, errors.New("configuring lidar camera error: " + + "'camera' must support PCD") + } return Lidar{ Name: cameraName, diff --git a/sensors/sensors_test.go b/sensors/sensors_test.go index 946e80e9..6ca3c1ca 100644 --- a/sensors/sensors_test.go +++ b/sensors/sensors_test.go @@ -23,15 +23,9 @@ func TestValidateGetLidarData(t *testing.T) { goodLidar, err := s.NewLidar(ctx, s.SetupDeps(lidar, ""), lidar, logger) test.That(t, err, test.ShouldBeNil) - lidar = "invalid_lidar" - invalidLidar, err := s.NewLidar(ctx, s.SetupDeps(lidar, ""), lidar, logger) - test.That(t, err, test.ShouldBeNil) - - // https://viam.atlassian.net/browse/RSDK-4306 - // test not relevant until replay camera supports Properties - // lidar = map[string]string{"name": "no_pcd_camera"} - // _, err = s.NewLidar(ctx, s.SetupDeps(lidar), lidar, logger) - // test.That(t, err, test.ShouldBeError, errors.New("configuring lidar camera error: 'camera' must support PCD")) + lidar = "lidar_with_invalid_properties" + _, err = s.NewLidar(ctx, s.SetupDeps(lidar, ""), lidar, logger) + test.That(t, err, test.ShouldBeError, errors.New("configuring lidar camera error: 'camera' must support PCD")) sensorValidationMaxTimeout := time.Duration(50) * time.Millisecond sensorValidationInterval := time.Duration(10) * time.Millisecond @@ -49,11 +43,6 @@ func TestValidateGetLidarData(t *testing.T) { test.That(t, err, test.ShouldBeNil) }) - t.Run("returns error if no lidar reading succeeds within the timeout", func(t *testing.T) { - err := s.ValidateGetLidarData(ctx, invalidLidar, sensorValidationMaxTimeout, sensorValidationInterval, logger) - test.That(t, err, test.ShouldBeError, errors.New("ValidateGetLidarData timeout: NextPointCloud error: invalid sensor")) - }) - t.Run("returns error if no lidar reading succeeds by the time the context is cancelled", func(t *testing.T) { cancelledCtx, cancelFunc := context.WithCancel(context.Background()) cancelFunc() @@ -129,7 +118,7 @@ func TestNewIMU(t *testing.T) { t.Run("Failed IMU creation with sensor that does not support AngularVelocity", func(t *testing.T) { lidar := "good_lidar" - imu := "bad_imu" + imu := "imu_with_invalid_properties" deps := s.SetupDeps(lidar, imu) actualIMU, err := s.NewIMU(context.Background(), deps, imu, logger) test.That(t, err, test.ShouldBeError, @@ -160,8 +149,8 @@ func TestTimedLidarSensorReading(t *testing.T) { logger := golog.NewTestLogger(t) ctx := context.Background() - lidar := "invalid_lidar" - invalidLidar, err := s.NewLidar(ctx, s.SetupDeps(lidar, ""), lidar, logger) + lidar := "lidar_with_erroring_functions" + lidarWithErroringFunctions, err := s.NewLidar(ctx, s.SetupDeps(lidar, ""), lidar, logger) test.That(t, err, test.ShouldBeNil) lidar = "invalid_replay_lidar" @@ -177,7 +166,7 @@ func TestTimedLidarSensorReading(t *testing.T) { test.That(t, err, test.ShouldBeNil) t.Run("when the lidar returns an error, returns that error", func(t *testing.T) { - tsr, err := invalidLidar.TimedLidarSensorReading(ctx) + tsr, err := lidarWithErroringFunctions.TimedLidarSensorReading(ctx) msg := "invalid sensor" test.That(t, err, test.ShouldBeError) test.That(t, err.Error(), test.ShouldContainSubstring, msg) @@ -222,8 +211,8 @@ func TestTimedIMUSensorReading(t *testing.T) { ctx := context.Background() lidar := "good_lidar" - imu := "invalid_imu" - invalidIMU, err := s.NewIMU(ctx, s.SetupDeps(lidar, imu), imu, logger) + imu := "imu_with_erroring_functions" + imuWithErroringFunctions, err := s.NewIMU(ctx, s.SetupDeps(lidar, imu), imu, logger) test.That(t, err, test.ShouldBeNil) imu = "good_imu" @@ -231,7 +220,7 @@ func TestTimedIMUSensorReading(t *testing.T) { test.That(t, err, test.ShouldBeNil) t.Run("when the IMU returns an error, returns that error", func(t *testing.T) { - tsr, err := invalidIMU.TimedIMUSensorReading(ctx) + tsr, err := imuWithErroringFunctions.TimedIMUSensorReading(ctx) msg := "invalid sensor" test.That(t, err, test.ShouldBeError) test.That(t, err.Error(), test.ShouldContainSubstring, msg) diff --git a/sensors/test_deps.go b/sensors/test_deps.go index f5e6d1f2..c520581a 100644 --- a/sensors/test_deps.go +++ b/sensors/test_deps.go @@ -36,8 +36,10 @@ func SetupDeps(lidarName, imuName string) resource.Dependencies { deps[camera.Named(lidarName)] = getReplayLidar(TestTime) case "invalid_replay_lidar": deps[camera.Named(lidarName)] = getReplayLidar(BadTime) - case "invalid_lidar": - deps[camera.Named(lidarName)] = getInvalidLidar() + case "lidar_with_erroring_functions": + deps[camera.Named(lidarName)] = getLidarWithErroringFunctions() + case "lidar_with_invalid_properties": + deps[camera.Named(lidarName)] = getLidarWithInvalidProperties() case "gibberish_lidar": return deps case "finished_replay_lidar": @@ -47,10 +49,10 @@ func SetupDeps(lidarName, imuName string) resource.Dependencies { switch imuName { case "good_imu": deps[movementsensor.Named(imuName)] = getGoodIMU() - case "invalid_imu": - deps[movementsensor.Named(imuName)] = getInvalidIMU() - case "bad_imu": - deps[movementsensor.Named(imuName)] = getBadIMU() + case "imu_with_erroring_functions": + deps[movementsensor.Named(imuName)] = getIMUWithErroringFunctions() + case "imu_with_invalid_properties": + deps[movementsensor.Named(imuName)] = getIMUWithInvalidProperties() case "gibberish_imu": return deps } @@ -75,7 +77,7 @@ func getWarmingUpLidar() *inject.Camera { return nil, transform.NewNoIntrinsicsError("") } cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { - return camera.Properties{}, nil + return camera.Properties{SupportsPCD: true}, nil } return cam } @@ -92,7 +94,7 @@ func getGoodLidar() *inject.Camera { return nil, transform.NewNoIntrinsicsError("") } cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { - return camera.Properties{}, nil + return camera.Properties{SupportsPCD: true}, nil } return cam } @@ -113,12 +115,29 @@ func getReplayLidar(testTime string) *inject.Camera { return nil, transform.NewNoIntrinsicsError("") } cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { - return camera.Properties{}, nil + return camera.Properties{SupportsPCD: true}, nil } return cam } -func getInvalidLidar() *inject.Camera { +func getLidarWithInvalidProperties() *inject.Camera { + cam := &inject.Camera{} + cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { + return pointcloud.New(), nil + } + cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { + return nil, errors.New("lidar not camera") + } + cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { + return nil, transform.NewNoIntrinsicsError("") + } + cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { + return camera.Properties{SupportsPCD: false}, nil + } + return cam +} + +func getLidarWithErroringFunctions() *inject.Camera { cam := &inject.Camera{} cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { return nil, errors.New("invalid sensor") @@ -130,7 +149,7 @@ func getInvalidLidar() *inject.Camera { return nil, transform.NewNoIntrinsicsError("") } cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { - return camera.Properties{}, nil + return camera.Properties{SupportsPCD: true}, nil } return cam } @@ -147,7 +166,7 @@ func getFinishedReplayLidar() *inject.Camera { return nil, transform.NewNoIntrinsicsError("") } cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { - return camera.Properties{}, nil + return camera.Properties{SupportsPCD: true}, nil } return cam } @@ -171,7 +190,7 @@ func getGoodIMU() *inject.MovementSensor { return imu } -func getInvalidIMU() *inject.MovementSensor { +func getIMUWithErroringFunctions() *inject.MovementSensor { imu := &inject.MovementSensor{} imu.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { return r3.Vector{}, errors.New("invalid sensor") @@ -188,7 +207,7 @@ func getInvalidIMU() *inject.MovementSensor { return imu } -func getBadIMU() *inject.MovementSensor { +func getIMUWithInvalidProperties() *inject.MovementSensor { imu := &inject.MovementSensor{} vec := r3.NewPreciseVector(1, 1, 1).Vector() imu.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { diff --git a/testhelper/testhelper.go b/testhelper/testhelper.go index b25526c7..36f07ea4 100644 --- a/testhelper/testhelper.go +++ b/testhelper/testhelper.go @@ -87,6 +87,11 @@ func getStubLidar(t *testing.T) *inject.Camera { t.Error("TEST FAILED stub lidar Projector called") return nil, transform.NewNoIntrinsicsError("") } + cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { + return camera.Properties{ + SupportsPCD: true, + }, nil + } return cam } diff --git a/viam_cartographer_test.go b/viam_cartographer_test.go index ae8a1e5b..748d9333 100644 --- a/viam_cartographer_test.go +++ b/viam_cartographer_test.go @@ -170,6 +170,31 @@ func TestNew(t *testing.T) { }) t.Run("Failed creation of cartographer slam service with invalid sensor "+ + "that errors during call to Properties", func(t *testing.T) { + termFunc := testhelper.InitTestCL(t, logger) + defer termFunc() + + dataDirectory, err := os.MkdirTemp("", "*") + test.That(t, err, test.ShouldBeNil) + defer func() { + err := os.RemoveAll(dataDirectory) + test.That(t, err, test.ShouldBeNil) + }() + + attrCfg := &vcConfig.Config{ + Sensors: []string{"lidar_with_invalid_properties"}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + DataRateMsec: &testDataRateMsec, + } + + _, err = testhelper.CreateSLAMService(t, attrCfg, logger) + test.That(t, err, test.ShouldBeError, + + errors.New("configuring lidar camera error: 'camera' must support PCD")) + }) + + t.Run("Failed creation of cartographer slam service with bad sensor "+ "that errors during call to NextPointCloud", func(t *testing.T) { termFunc := testhelper.InitTestCL(t, logger) defer termFunc() @@ -182,7 +207,7 @@ func TestNew(t *testing.T) { }() attrCfg := &vcConfig.Config{ - Sensors: []string{"invalid_lidar"}, + Sensors: []string{"lidar_with_erroring_functions"}, ConfigParams: map[string]string{"mode": "2d"}, DataDirectory: dataDirectory, DataRateMsec: &testDataRateMsec, @@ -379,7 +404,7 @@ func TestNewFeatureFlag(t *testing.T) { }() attrCfg := &vcConfig.Config{ - Camera: map[string]string{"name": "invalid_lidar", "data_frequency_hz": testLidarDataFreqHz}, + Camera: map[string]string{"name": "lidar_with_erroring_functions", "data_frequency_hz": testLidarDataFreqHz}, ConfigParams: map[string]string{"mode": "2d"}, DataDirectory: dataDirectory, IMUIntegrationEnabled: true, @@ -408,13 +433,13 @@ func TestNewFeatureFlag(t *testing.T) { ConfigParams: map[string]string{"mode": "2d"}, DataDirectory: dataDirectory, IMUIntegrationEnabled: true, - MovementSensor: map[string]string{"name": "invalid_imu", "data_frequency_hz": testIMUDataFreqHz}, + MovementSensor: map[string]string{"name": "imu_with_invalid_properties", "data_frequency_hz": testIMUDataFreqHz}, } _, err = testhelper.CreateSLAMService(t, attrCfg, logger) test.That(t, err, test.ShouldBeError, - errors.New("failed to get data from IMU: ValidateGetIMUData timeout: LinearAcceleration error: invalid sensor")) + errors.New("configuring IMU movement sensor error: 'movement_sensor' must support both LinearAcceleration and AngularVelocity")) }) t.Run("Successful creation of cartographer slam service in localization mode with feature flag enabled", func(t *testing.T) {