Skip to content

Commit

Permalink
RSDK-4306 Add Properties Check of LiDAR during Initialization (#245)
Browse files Browse the repository at this point in the history
  • Loading branch information
@jeremyrhyde
jeremyrhyde authored Aug 15, 2023
1 parent 8b3ba83 commit 7c69c3a
Show file tree
Hide file tree
Showing 6 changed files with 87 additions and 51 deletions.
2 changes: 1 addition & 1 deletion sensorprocess/sensorprocess_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
20 changes: 9 additions & 11 deletions sensors/sensors.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
31 changes: 10 additions & 21 deletions sensors/sensors_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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()
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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"
Expand All @@ -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)
Expand Down Expand Up @@ -222,16 +211,16 @@ 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"
goodIMU, err := s.NewIMU(ctx, s.SetupDeps(lidar, imu), imu, logger)
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)
Expand Down
47 changes: 33 additions & 14 deletions sensors/test_deps.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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":
Expand All @@ -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
}
Expand All @@ -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
}
Expand All @@ -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
}
Expand All @@ -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")
Expand All @@ -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
}
Expand All @@ -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
}
Expand All @@ -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")
Expand All @@ -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) {
Expand Down
5 changes: 5 additions & 0 deletions testhelper/testhelper.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
}

Expand Down
33 changes: 29 additions & 4 deletions viam_cartographer_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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) {
Expand Down

0 comments on commit 7c69c3a

Please sign in to comment.