diff --git a/cartofacade/capi.go b/cartofacade/capi.go index 52770afd..7c59138f 100644 --- a/cartofacade/capi.go +++ b/cartofacade/capi.go @@ -10,19 +10,6 @@ package cartofacade #cgo LDFLAGS: -L../viam-cartographer/build -L../viam-cartographer/build/cartographer -lviam-cartographer -lcartographer -ldl -lm -labsl_hash -labsl_city -labsl_bad_optional_access -labsl_strerror -labsl_str_format_internal -labsl_synchronization -labsl_strings -labsl_throw_delegate -lcairo -llua5.3 -lstdc++ -lceres -lprotobuf -lglog -lboost_filesystem -lboost_iostreams -lpcl_io -lpcl_common -labsl_raw_hash_set #include "../viam-cartographer/src/carto_facade/carto_facade.h" - - bstring* alloc_bstring_array(size_t len) { return (bstring*) malloc(len * sizeof(bstring)); } - int free_bstring_array(bstring* p, size_t len) { - int result; - for (size_t i = 0; i < len; i++) { - result = bdestroy(p[i]); - if (result != BSTR_OK) { - return result; - }; - } - free(p); - return BSTR_OK; - } */ import "C" @@ -99,7 +86,7 @@ const ( // CartoConfig contains config values from app type CartoConfig struct { - Sensors []string + Camera string MapRateSecond int DataDir string ComponentReference string @@ -166,25 +153,19 @@ func NewCarto(cfg CartoConfig, acfg CartoAlgoConfig, vcl CartoLibInterface) (Car if err != nil { return Carto{}, err } - vcac := toAlgoConfig(acfg) - cl, ok := vcl.(*CartoLib) if !ok { return Carto{}, errors.New("cannot cast provided library to a CartoLib") } - status := C.viam_carto_init(&pVc, cl.value, vcc, vcac) + if err := toError(status); err != nil { return Carto{}, err } - status = C.free_bstring_array(vcc.sensors, C.size_t(len(cfg.Sensors))) - if status != C.BSTR_OK { - return Carto{}, errors.New("unable to free memory for sensor list") - } - carto := Carto{value: pVc, SlamMode: toSlamMode(pVc.slam_mode)} + return carto, nil } @@ -299,17 +280,6 @@ func (vc *Carto) getInternalState() ([]byte, error) { return interalState, nil } -// this function is only used for testing purposes, but needs to be in this file as CGo is not supported in go test files -func bStringToGoStringSlice(bstr *C.bstring, length int) []string { - bStrings := (*[1 << 28]C.bstring)(unsafe.Pointer(bstr))[:length:length] - - goStrings := []string{} - for _, bString := range bStrings { - goStrings = append(goStrings, bstringToGoString(bString)) - } - return goStrings -} - // this function is only used for testing purposes, but needs to be in this file as CGo is not supported in go test files func getTestGetPositionResponse() C.viam_carto_get_position_response { gpr := C.viam_carto_get_position_response{} @@ -352,24 +322,13 @@ func toLidarConfig(lidarConfig LidarConfig) (C.viam_carto_LIDAR_CONFIG, error) { func getConfig(cfg CartoConfig) (C.viam_carto_config, error) { vcc := C.viam_carto_config{} + vcc.camera = goStringToBstring(cfg.Camera) - // create pointer to bstring which can represent a list of sensors - sz := len(cfg.Sensors) - pSensor := C.alloc_bstring_array(C.size_t(sz)) - if pSensor == nil { - return C.viam_carto_config{}, errors.New("unable to allocate memory for sensor list") - } - sensorSlice := unsafe.Slice(pSensor, sz) - for i, sensor := range cfg.Sensors { - sensorSlice[i] = goStringToBstring(sensor) - } lidarCfg, err := toLidarConfig(cfg.LidarConfig) if err != nil { return C.viam_carto_config{}, err } - vcc.sensors = pSensor - vcc.sensors_len = C.int(sz) vcc.map_rate_sec = C.int(cfg.MapRateSecond) vcc.data_dir = goStringToBstring(cfg.DataDir) vcc.lidar_config = lidarCfg @@ -426,12 +385,6 @@ func bstringToByteSlice(bstr C.bstring) []byte { return C.GoBytes(unsafe.Pointer(bstr.data), bstr.slen) } -// freeBstringArray used to cleanup a bstring array (needs to be a go func so it can be used in tests) -func freeBstringArray(arr *C.bstring, length C.int) { - lengthSizeT := C.size_t(length) - C.free_bstring_array(arr, lengthSizeT) -} - func toError(status C.int) error { switch int(status) { case C.VIAM_CARTO_SUCCESS: @@ -450,8 +403,6 @@ func toError(status C.int) error { return errors.New("VIAM_CARTO_LIB_INVALID") case C.VIAM_CARTO_LIB_NOT_INITIALIZED: return errors.New("VIAM_CARTO_LIB_NOT_INITIALIZED") - case C.VIAM_CARTO_SENSORS_LIST_EMPTY: - return errors.New("VIAM_CARTO_SENSORS_LIST_EMPTY") case C.VIAM_CARTO_UNKNOWN_ERROR: return errors.New("VIAM_CARTO_UNKNOWN_ERROR") case C.VIAM_CARTO_DATA_DIR_NOT_PROVIDED: diff --git a/cartofacade/capi_test.go b/cartofacade/capi_test.go index 94ba955b..70574b89 100644 --- a/cartofacade/capi_test.go +++ b/cartofacade/capi_test.go @@ -62,16 +62,12 @@ func TestGetConfig(t *testing.T) { vcc, err := getConfig(cfg) test.That(t, err, test.ShouldBeNil) - sensors := bStringToGoStringSlice(vcc.sensors, int(vcc.sensors_len)) - test.That(t, sensors[0], test.ShouldResemble, "mysensor") - test.That(t, sensors[1], test.ShouldResemble, "imu") - test.That(t, vcc.sensors_len, test.ShouldEqual, 2) + camera := bstringToGoString(vcc.camera) + test.That(t, camera, test.ShouldResemble, "mysensor") dataDir := bstringToGoString(vcc.data_dir) test.That(t, dataDir, test.ShouldResemble, dir) - freeBstringArray(vcc.sensors, vcc.sensors_len) - test.That(t, vcc.lidar_config, test.ShouldEqual, TwoD) }) } diff --git a/cartofacade/testhelpers.go b/cartofacade/testhelpers.go index 366d906c..ff58106b 100644 --- a/cartofacade/testhelpers.go +++ b/cartofacade/testhelpers.go @@ -5,14 +5,14 @@ import ( ) // GetTestConfig gets a sample config for testing purposes. -func GetTestConfig(sensor string) (CartoConfig, string, error) { +func GetTestConfig(cameraName string) (CartoConfig, string, error) { dir, err := os.MkdirTemp("", "slam-test") if err != nil { return CartoConfig{}, "", err } return CartoConfig{ - Sensors: []string{sensor, "imu"}, + Camera: cameraName, MapRateSecond: 5, DataDir: dir, ComponentReference: "component", @@ -23,7 +23,7 @@ func GetTestConfig(sensor string) (CartoConfig, string, error) { // GetBadTestConfig gets a sample config for testing purposes that will cause a failure. func GetBadTestConfig() CartoConfig { return CartoConfig{ - Sensors: []string{"rplidar", "imu"}, + Camera: "rplidar", LidarConfig: TwoD, } } diff --git a/config/config.go b/config/config.go index 21097c20..e90dcdd9 100644 --- a/config/config.go +++ b/config/config.go @@ -2,6 +2,8 @@ package config import ( + "strconv" + "github.com/edaniels/golog" "github.com/pkg/errors" "go.viam.com/utils" @@ -14,19 +16,48 @@ func newError(configError string) error { // Config describes how to configure the SLAM service. type Config struct { - Sensors []string `json:"sensors"` - ConfigParams map[string]string `json:"config_params"` - DataDirectory string `json:"data_dir"` - DataRateMsec int `json:"data_rate_msec"` - MapRateSec *int `json:"map_rate_sec"` + Camera map[string]string `json:"camera"` + ConfigParams map[string]string `json:"config_params"` + DataDirectory string `json:"data_dir"` + MapRateSec *int `json:"map_rate_sec"` + IMUIntegrationEnabled bool `json:"imu_integration_enabled"` + Sensors []string `json:"sensors"` + DataRateMsec int `json:"data_rate_msec"` } -var errSensorsMustNotBeEmpty = errors.New("\"sensors\" must not be empty") +var ( + errCameraMustHaveName = errors.New("\"camera[name]\" is required") + errSensorsMustNotBeEmpty = errors.New("\"sensors\" must not be empty") +) // Validate creates the list of implicit dependencies. func (config *Config) Validate(path string) ([]string, error) { - if config.Sensors == nil || len(config.Sensors) < 1 { - return nil, utils.NewConfigValidationError(path, errSensorsMustNotBeEmpty) + cameraName := "" + if config.IMUIntegrationEnabled { + var ok bool + cameraName, ok = config.Camera["name"] + if !ok { + return nil, utils.NewConfigValidationError(path, errCameraMustHaveName) + } + dataFreqHz, ok := config.Camera["data_frequency_hz"] + if ok { + dataFreqHz, err := strconv.Atoi(dataFreqHz) + if err != nil { + return nil, errors.New("camera[data_frequency_hz] must only contain digits") + } + if dataFreqHz < 0 { + return nil, errors.New("cannot specify camera[data_frequency_hz] less than zero") + } + } + } else { + if config.Sensors == nil || len(config.Sensors) < 1 { + return nil, utils.NewConfigValidationError(path, errors.New("\"sensors\" must not be empty")) + } + cameraName = config.Sensors[0] + + if config.DataRateMsec < 0 { + return nil, errors.New("cannot specify data_rate_msec less than zero") + } } if config.ConfigParams["mode"] == "" { @@ -37,28 +68,39 @@ func (config *Config) Validate(path string) ([]string, error) { return nil, utils.NewConfigValidationFieldRequiredError(path, "data_dir") } - if config.DataRateMsec < 0 { - return nil, errors.New("cannot specify data_rate_msec less than zero") - } - if config.MapRateSec != nil && *config.MapRateSec < 0 { return nil, errors.New("cannot specify map_rate_sec less than zero") } - deps := config.Sensors + deps := []string{cameraName} return deps, nil } // GetOptionalParameters sets any unset optional config parameters to the values passed to this function, // and returns them. -func GetOptionalParameters(config *Config, - defaultDataRateMsec, defaultMapRateSec int, logger golog.Logger, -) (int, int) { - dataRateMsec := config.DataRateMsec - if config.DataRateMsec == 0 { - dataRateMsec = defaultDataRateMsec - logger.Debugf("no data_rate_msec given, setting to default value of %d", defaultDataRateMsec) +func GetOptionalParameters(config *Config, defaultLidarDataRateMsec, defaultMapRateSec int, logger golog.Logger, +) (int, int, error) { + lidarDataRateMsec := defaultLidarDataRateMsec + + // feature flag for new config + if config.IMUIntegrationEnabled { + strCameraDataFreqHz, ok := config.Camera["data_frequency_hz"] + if !ok { + logger.Debugf("config did not provide camera[data_frequency_hz], setting to default value of %d", 1000/defaultLidarDataRateMsec) + } else { + lidarDataFreqHz, err := strconv.Atoi(strCameraDataFreqHz) + if err != nil { + return 0, 0, newError("camera[data_frequency_hz] must only contain digits") + } + lidarDataRateMsec = 1000 / lidarDataFreqHz + } + } else { + lidarDataRateMsec = config.DataRateMsec + if config.DataRateMsec == 0 { + lidarDataRateMsec = defaultLidarDataRateMsec + logger.Debugf("no data_rate_msec given, setting to default value of %d", defaultLidarDataRateMsec) + } } mapRateSec := 0 @@ -69,5 +111,5 @@ func GetOptionalParameters(config *Config, mapRateSec = *config.MapRateSec } - return dataRateMsec, mapRateSec + return lidarDataRateMsec, mapRateSec, nil } diff --git a/config/config_test.go b/config/config_test.go index e056d95b..3e92e75f 100644 --- a/config/config_test.go +++ b/config/config_test.go @@ -22,7 +22,7 @@ func TestValidate(t *testing.T) { }) t.Run("Simplest valid config", func(t *testing.T) { - cfgService := makeCfgService() + cfgService := makeCfgService(false) _, err := newConfig(cfgService) test.That(t, err, test.ShouldBeNil) }) @@ -30,15 +30,15 @@ func TestValidate(t *testing.T) { t.Run("Config without required fields", func(t *testing.T) { requiredFields := []string{"data_dir", "sensors"} dataDirErr := utils.NewConfigValidationFieldRequiredError(testCfgPath, requiredFields[0]) - sensorsErr := utils.NewConfigValidationError(testCfgPath, errSensorsMustNotBeEmpty) + cameraErr := utils.NewConfigValidationError(testCfgPath, errSensorsMustNotBeEmpty) expectedErrors := []error{ newError(dataDirErr.Error()), - newError(sensorsErr.Error()), + newError(cameraErr.Error()), } for i, requiredField := range requiredFields { logger.Debugf("Testing SLAM config without %s\n", requiredField) - cfgService := makeCfgService() + cfgService := makeCfgService(false) delete(cfgService.Attributes, requiredField) _, err := newConfig(cfgService) @@ -46,14 +46,14 @@ func TestValidate(t *testing.T) { } // Test for missing config_params attributes logger.Debug("Testing SLAM config without config_params[mode]") - cfgService := makeCfgService() + cfgService := makeCfgService(false) delete(cfgService.Attributes["config_params"].(map[string]string), "mode") _, err := newConfig(cfgService) test.That(t, err, test.ShouldBeError, newError(utils.NewConfigValidationFieldRequiredError(testCfgPath, "config_params[mode]").Error())) }) t.Run("Config with invalid parameter type", func(t *testing.T) { - cfgService := makeCfgService() + cfgService := makeCfgService(false) cfgService.Attributes["data_dir"] = true _, err := newConfig(cfgService) expE := newError("1 error(s) decoding:\n\n* 'data_dir' expected type 'string', got unconvertible type 'bool', value: 'true'") @@ -64,7 +64,7 @@ func TestValidate(t *testing.T) { }) t.Run("Config with out of range values", func(t *testing.T) { - cfgService := makeCfgService() + cfgService := makeCfgService(false) cfgService.Attributes["data_rate_msec"] = -1 _, err := newConfig(cfgService) test.That(t, err, test.ShouldBeError, newError("cannot specify data_rate_msec less than zero")) @@ -75,7 +75,7 @@ func TestValidate(t *testing.T) { }) t.Run("All parameters e2e", func(t *testing.T) { - cfgService := makeCfgService() + cfgService := makeCfgService(false) cfgService.Attributes["sensors"] = []string{"a", "b"} cfgService.Attributes["data_rate_msec"] = 1001 cfgService.Attributes["map_rate_sec"] = 1002 @@ -96,7 +96,7 @@ func TestValidate(t *testing.T) { } // makeCfgService creates the simplest possible config that can pass validation. -func makeCfgService() resource.Config { +func makeCfgService(IMUIntegrationEnabled bool) resource.Config { model := resource.DefaultModelFamily.WithModel("test") cfgService := resource.Config{Name: "test", API: slam.API, Model: model} cfgService.Attributes = make(map[string]interface{}) @@ -104,7 +104,15 @@ func makeCfgService() resource.Config { "mode": "test mode", } cfgService.Attributes["data_dir"] = "path" - cfgService.Attributes["sensors"] = []string{"a"} + if IMUIntegrationEnabled { + cfgService.Attributes["imu_integration_enabled"] = true + cfgService.Attributes["camera"] = map[string]string{ + "name": "a", + } + } else { + cfgService.Attributes["sensors"] = []string{"a"} + } + return cfgService } @@ -112,34 +120,39 @@ func TestGetOptionalParameters(t *testing.T) { logger := golog.NewTestLogger(t) t.Run("Pass default parameters", func(t *testing.T) { - cfgService := makeCfgService() + cfgService := makeCfgService(false) cfgService.Attributes["sensors"] = []string{"a"} cfg, err := newConfig(cfgService) test.That(t, err, test.ShouldBeNil) - dataRateMsec, mapRateSec := GetOptionalParameters( + lidarDataRateMsec, mapRateSec, err := GetOptionalParameters( cfg, - 1001, + 1000, 1002, logger) - test.That(t, dataRateMsec, test.ShouldEqual, 1001) + test.That(t, err, test.ShouldBeNil) test.That(t, mapRateSec, test.ShouldEqual, 1002) + test.That(t, lidarDataRateMsec, test.ShouldEqual, 1000) }) t.Run("Return overrides", func(t *testing.T) { - cfgService := makeCfgService() - cfgService.Attributes["sensors"] = []string{"a"} + cfgService := makeCfgService(false) + cfgService.Attributes["camera"] = map[string]string{ + "name": "a", + "data_frequency_hz": "1", + } cfg, err := newConfig(cfgService) two := 2 - cfg.DataRateMsec = 1 cfg.MapRateSec = &two + cfg.DataRateMsec = 50 test.That(t, err, test.ShouldBeNil) - dataRateMsec, mapRateSec := GetOptionalParameters( + lidarDataRateMsec, mapRateSec, err := GetOptionalParameters( cfg, - 1001, + 1000, 1002, logger) - test.That(t, dataRateMsec, test.ShouldEqual, 1) + test.That(t, err, test.ShouldBeNil) test.That(t, mapRateSec, test.ShouldEqual, 2) + test.That(t, lidarDataRateMsec, test.ShouldEqual, 50) }) } @@ -155,3 +168,156 @@ func newConfig(conf resource.Config) (*Config, error) { return slamConf, nil } + +func TestValidateFeatureFlag(t *testing.T) { + testCfgPath := "services.slam.attributes.fake" + logger := golog.NewTestLogger(t) + + t.Run("Empty config with feature flag enabled", func(t *testing.T) { + model := resource.DefaultModelFamily.WithModel("test") + cfgService := resource.Config{Name: "test", API: slam.API, Model: model} + cfgService.Attributes = make(map[string]interface{}) + cfgService.Attributes["imu_integration_enabled"] = true + _, err := newConfig(cfgService) + test.That(t, err, test.ShouldBeError, newError("error validating \"services.slam.attributes.fake\": \"camera[name]\" is required")) + }) + + t.Run("Simplest valid config with feature flag enabled", func(t *testing.T) { + cfgService := makeCfgService(true) + _, err := newConfig(cfgService) + test.That(t, err, test.ShouldBeNil) + }) + + t.Run("Config without camera name with feature flag enabled", func(t *testing.T) { + // Test for missing camera name attribute + logger.Debug("Testing SLAM config without camera[name]") + cfgService := makeCfgService(true) + delete(cfgService.Attributes["camera"].(map[string]string), "name") + _, err := newConfig(cfgService) + test.That(t, err, test.ShouldBeError, newError(utils.NewConfigValidationFieldRequiredError(testCfgPath, "camera[name]").Error())) + }) + + t.Run("Config with invalid parameter type with feature flag enabled", func(t *testing.T) { + cfgService := makeCfgService(true) + cfgService.Attributes["data_dir"] = true + _, err := newConfig(cfgService) + expE := newError("1 error(s) decoding:\n\n* 'data_dir' expected type 'string', got unconvertible type 'bool', value: 'true'") + test.That(t, err, test.ShouldBeError, expE) + cfgService.Attributes["data_dir"] = "true" + _, err = newConfig(cfgService) + test.That(t, err, test.ShouldBeNil) + }) + + t.Run("Config with invalid camera data_frequency_hz with feature flag enabled", func(t *testing.T) { + cfgService := makeCfgService(true) + cfgService.Attributes["camera"] = map[string]string{ + "name": "a", + "data_frequency_hz": "twenty", + } + _, err := newConfig(cfgService) + test.That(t, err, test.ShouldBeError, newError("camera[data_frequency_hz] must only contain digits")) + }) + + t.Run("Config with out of range values with feature flag enabled", func(t *testing.T) { + cfgService := makeCfgService(true) + cfgService.Attributes["camera"] = map[string]string{ + "name": "a", + "data_frequency_hz": "-1", + } + _, err := newConfig(cfgService) + test.That(t, err, test.ShouldBeError, newError("cannot specify camera[data_frequency_hz] less than zero")) + cfgService.Attributes["camera"] = map[string]string{ + "name": "a", + "data_frequency_hz": "1", + } + cfgService.Attributes["map_rate_sec"] = -1 + _, err = newConfig(cfgService) + test.That(t, err, test.ShouldBeError, newError("cannot specify map_rate_sec less than zero")) + }) + + t.Run("All parameters e2e with feature flag enabled", func(t *testing.T) { + cfgService := makeCfgService(true) + cfgService.Attributes["camera"] = map[string]string{ + "name": "a", + "data_frequency_hz": "20", + } + cfgService.Attributes["map_rate_sec"] = 1002 + + cfgService.Attributes["config_params"] = map[string]string{ + "mode": "test mode", + "value": "0", + "value_2": "test", + } + cfg, err := newConfig(cfgService) + test.That(t, err, test.ShouldBeNil) + test.That(t, cfg.DataDirectory, test.ShouldEqual, cfgService.Attributes["data_dir"]) + test.That(t, cfg.Camera, test.ShouldResemble, cfgService.Attributes["camera"]) + test.That(t, *cfg.MapRateSec, test.ShouldEqual, cfgService.Attributes["map_rate_sec"]) + test.That(t, cfg.ConfigParams, test.ShouldResemble, cfgService.Attributes["config_params"]) + }) +} + +func TestGetOptionalParametersFeatureFlag(t *testing.T) { + logger := golog.NewTestLogger(t) + + t.Run("Pass default parameters with feature flag enabled", func(t *testing.T) { + cfgService := makeCfgService(true) + cfgService.Attributes["camera"] = map[string]string{"name": "a"} + cfg, err := newConfig(cfgService) + test.That(t, err, test.ShouldBeNil) + lidarDataRateMsec, mapRateSec, err := GetOptionalParameters( + cfg, + 1000, + 1002, + logger) + test.That(t, err, test.ShouldBeNil) + test.That(t, mapRateSec, test.ShouldEqual, 1002) + test.That(t, lidarDataRateMsec, test.ShouldEqual, 1000) + }) + + t.Run("Return overrides with feature flag enabled", func(t *testing.T) { + cfgService := makeCfgService(true) + cfgService.Attributes["camera"] = map[string]string{ + "name": "a", + "data_frequency_hz": "1", + } + cfg, err := newConfig(cfgService) + two := 2 + cfg.MapRateSec = &two + cfg.Camera["data_frequency_hz"] = "20" + test.That(t, err, test.ShouldBeNil) + lidarDataRateMsec, mapRateSec, err := GetOptionalParameters( + cfg, + 1000, + 1002, + logger) + test.That(t, err, test.ShouldBeNil) + test.That(t, mapRateSec, test.ShouldEqual, 2) + test.That(t, lidarDataRateMsec, test.ShouldEqual, 50) + }) + + t.Run("Unit test return error if lidar data frequency is invalid", func(t *testing.T) { + cfgService := makeCfgService(true) + cfgService.Attributes["camera"] = map[string]string{ + "name": "a", + "data_frequency_hz": "b", + } + cfg, err := newConfigWithoutValidate(cfgService) + test.That(t, err, test.ShouldBeNil) + _, _, err = GetOptionalParameters( + cfg, + 1000, + 1002, + logger) + test.That(t, err, test.ShouldBeError, newError("camera[data_frequency_hz] must only contain digits")) + }) +} + +func newConfigWithoutValidate(conf resource.Config) (*Config, error) { + slamConf, err := resource.TransformAttributeMap[*Config](conf.Attributes) + if err != nil { + return &Config{}, newError(err.Error()) + } + + return slamConf, nil +} diff --git a/integration_test.go b/integration_test.go index a4550a31..04f39d3b 100644 --- a/integration_test.go +++ b/integration_test.go @@ -76,7 +76,7 @@ func testCartographerPosition(t *testing.T, svc slam.Service, expectedComponentR test.That(t, actualPos.Y, test.ShouldBeBetween, expectedPosLinux.Y-tolerancePos, expectedPosLinux.Y+tolerancePos) test.That(t, actualPos.Z, test.ShouldBeBetween, expectedPosLinux.Z-tolerancePos, expectedPosLinux.Z+tolerancePos) } else { - t.Error("Position is outside of expected platform range") + t.Error("TEST FAILED Position is outside of expected platform range") } actualOri := position.Orientation().AxisAngles() @@ -91,7 +91,7 @@ func testCartographerPosition(t *testing.T, svc slam.Service, expectedComponentR test.That(t, actualOri.RY, test.ShouldBeBetween, expectedOriLinux.RY-toleranceOri, expectedOriLinux.RY+toleranceOri) test.That(t, actualOri.RZ, test.ShouldBeBetween, expectedOriLinux.RZ-toleranceOri, expectedOriLinux.RZ+toleranceOri) } else { - t.Error("Orientation is outside of expected platform range") + t.Error("TEST FAILED Orientation is outside of expected platform range") } } @@ -99,11 +99,11 @@ func saveInternalState(t *testing.T, internalState []byte, dataDir string) { timeStamp := time.Now() internalStateDir := filepath.Join(dataDir, "internal_state") if err := os.Mkdir(internalStateDir, 0o755); err != nil { - t.Error("failed to create test internal state directory") + t.Error("TEST FAILED failed to create test internal state directory") } filename := filepath.Join(internalStateDir, "map_data_"+timeStamp.UTC().Format(testhelper.SlamTimeFormat)+".pbstream") if err := os.WriteFile(filename, internalState, 0o644); err != nil { - t.Error("failed to write test internal state") + t.Error("TEST FAILED failed to write test internal state") } } @@ -120,27 +120,26 @@ func testHelperCartographer( defer termFunc() attrCfg := &vcConfig.Config{ - Sensors: []string{"stub_lidar"}, + Camera: map[string]string{"name": "stub_lidar"}, ConfigParams: map[string]string{ "mode": reflect.ValueOf(subAlgo).String(), }, - MapRateSec: &mapRateSec, - DataDirectory: dataDirectory, + MapRateSec: &mapRateSec, + DataDirectory: dataDirectory, + IMUIntegrationEnabled: true, } done := make(chan struct{}) sensorReadingInterval := time.Millisecond * 200 - timedSensor, err := testhelper.IntegrationLidarTimedSensor(t, attrCfg.Sensors[0], replaySensor, sensorReadingInterval, done) + timedSensor, err := testhelper.IntegrationLidarTimedSensor(t, attrCfg.Camera["name"], replaySensor, sensorReadingInterval, done) test.That(t, err, test.ShouldBeNil) - svc, err := testhelper.CreateIntegrationSLAMService(t, attrCfg, timedSensor, logger) test.That(t, err, test.ShouldBeNil) - start := time.Now() + start := time.Now() cSvc, ok := svc.(*viamcartographer.CartographerService) test.That(t, ok, test.ShouldBeTrue) test.That(t, cSvc.SlamMode, test.ShouldEqual, expectedMode) - ctx, cancelFunc := context.WithTimeout(context.Background(), time.Second*5) defer cancelFunc() @@ -150,7 +149,7 @@ func testHelperCartographer( test.That(t, errors.New("test timeout"), test.ShouldBeNil) } - testCartographerPosition(t, svc, attrCfg.Sensors[0]) + testCartographerPosition(t, svc, attrCfg.Camera["name"]) testCartographerMap(t, svc, cSvc.SlamMode == cartofacade.LocalizingMode) internalState, err := slam.GetInternalStateFull(context.Background(), svc) diff --git a/sensorprocess/sensorprocess.go b/sensorprocess/sensorprocess.go index 1677785e..a49a1e3c 100644 --- a/sensorprocess/sensorprocess.go +++ b/sensorprocess/sensorprocess.go @@ -17,12 +17,12 @@ import ( // Config holds config needed throughout the process of adding a sensor reading to the cartofacade. type Config struct { - CartoFacade cartofacade.Interface - Lidar sensors.TimedSensor - LidarName string - DataRateMs int - Timeout time.Duration - Logger golog.Logger + CartoFacade cartofacade.Interface + Lidar sensors.TimedSensor + LidarName string + LidarDataRateMsec int + Timeout time.Duration + Logger golog.Logger } // Start polls the lidar to get the next sensor reading and adds it to the cartofacade. @@ -98,5 +98,5 @@ func addSensorReadingFromLiveReadings(ctx context.Context, reading []byte, readi } } timeElapsedMs := int(time.Since(startTime).Milliseconds()) - return int(math.Max(0, float64(config.DataRateMs-timeElapsedMs))) + return int(math.Max(0, float64(config.LidarDataRateMsec-timeElapsedMs))) } diff --git a/sensorprocess/sensorprocess_test.go b/sensorprocess/sensorprocess_test.go index 3d321e02..693bd654 100644 --- a/sensorprocess/sensorprocess_test.go +++ b/sensorprocess/sensorprocess_test.go @@ -41,11 +41,11 @@ func TestAddSensorReadingReplaySensor(t *testing.T) { readingTimestamp := time.Now().UTC() cf := cartofacade.Mock{} config := Config{ - Logger: logger, - CartoFacade: &cf, - LidarName: "good_lidar", - DataRateMs: 200, - Timeout: 10 * time.Second, + Logger: logger, + CartoFacade: &cf, + LidarName: "good_lidar", + LidarDataRateMsec: 200, + Timeout: 10 * time.Second, } t.Run("When addSensorReading returns successfully, no infinite loop", func(t *testing.T) { cf.AddSensorReadingFunc = func( @@ -138,14 +138,14 @@ func TestAddSensorReadingLiveReadings(t *testing.T) { reading := []byte("12345") readingTimestamp := time.Now().UTC() config := Config{ - Logger: logger, - CartoFacade: &cf, - LidarName: "good_lidar", - DataRateMs: 200, - Timeout: 10 * time.Second, + Logger: logger, + CartoFacade: &cf, + LidarName: "good_lidar", + LidarDataRateMsec: 200, + Timeout: 10 * time.Second, } - t.Run("When AddSensorReading blocks for more than the DataRateMs and succeeds, time to sleep is 0", func(t *testing.T) { + t.Run("When AddSensorReading blocks for more than the DataFreqHz and succeeds, time to sleep is 0", func(t *testing.T) { cf.AddSensorReadingFunc = func( ctx context.Context, timeout time.Duration, @@ -161,7 +161,7 @@ func TestAddSensorReadingLiveReadings(t *testing.T) { test.That(t, timeToSleep, test.ShouldEqual, 0) }) - t.Run("AddSensorReading slower than DataRateMs and returns lock error, time to sleep is 0", func(t *testing.T) { + t.Run("AddSensorReading slower than DataFreqHz and returns lock error, time to sleep is 0", func(t *testing.T) { cf.AddSensorReadingFunc = func( ctx context.Context, timeout time.Duration, @@ -177,7 +177,7 @@ func TestAddSensorReadingLiveReadings(t *testing.T) { test.That(t, timeToSleep, test.ShouldEqual, 0) }) - t.Run("When AddSensorReading blocks for more than the DataRateMs and returns an unexpected error, time to sleep is 0", func(t *testing.T) { + t.Run("When AddSensorReading blocks for more than the DataFreqHz and returns an unexpected error, time to sleep is 0", func(t *testing.T) { cf.AddSensorReadingFunc = func( ctx context.Context, timeout time.Duration, @@ -193,7 +193,7 @@ func TestAddSensorReadingLiveReadings(t *testing.T) { test.That(t, timeToSleep, test.ShouldEqual, 0) }) - t.Run("AddSensorReading faster than the DataRateMs and succeeds, time to sleep is <= DataRateMs", func(t *testing.T) { + t.Run("AddSensorReading faster than the DataFreqHz and succeeds, time to sleep is <= DataFreqHz", func(t *testing.T) { cf.AddSensorReadingFunc = func( ctx context.Context, timeout time.Duration, @@ -206,10 +206,10 @@ func TestAddSensorReadingLiveReadings(t *testing.T) { timeToSleep := addSensorReadingFromLiveReadings(context.Background(), reading, readingTimestamp, config) test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, config.DataRateMs) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, config.LidarDataRateMsec) }) - t.Run("AddSensorReading faster than the DataRateMs and returns lock error, time to sleep is <= DataRateMs", func(t *testing.T) { + t.Run("AddSensorReading faster than the DataFreqHz and returns lock error, time to sleep is <= DataFreqHz", func(t *testing.T) { cf.AddSensorReadingFunc = func( ctx context.Context, timeout time.Duration, @@ -222,10 +222,10 @@ func TestAddSensorReadingLiveReadings(t *testing.T) { timeToSleep := addSensorReadingFromLiveReadings(context.Background(), reading, readingTimestamp, config) test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, config.DataRateMs) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, config.LidarDataRateMsec) }) - t.Run("AddSensorReading faster than DataRateMs and returns unexpected error, time to sleep is <= DataRateMs", func(t *testing.T) { + t.Run("AddSensorReading faster than DataFreqHz and returns unexpected error, time to sleep is <= DataFreqHz", func(t *testing.T) { cf.AddSensorReadingFunc = func( ctx context.Context, timeout time.Duration, @@ -238,7 +238,7 @@ func TestAddSensorReadingLiveReadings(t *testing.T) { timeToSleep := addSensorReadingFromLiveReadings(context.Background(), reading, readingTimestamp, config) test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, config.DataRateMs) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, config.LidarDataRateMsec) }) } @@ -247,10 +247,10 @@ func invalidSensorTestHelper( t *testing.T, cartoFacadeMock cartofacade.Mock, config Config, - sensors []string, + cameraName string, ) { logger := golog.NewTestLogger(t) - sensor, err := s.NewLidar(context.Background(), s.SetupDeps(sensors), sensors, logger) + sensor, err := s.NewLidar(context.Background(), s.SetupDeps(cameraName), cameraName, logger) test.That(t, err, test.ShouldBeNil) var calls []addSensorReadingArgs @@ -283,39 +283,39 @@ func TestAddSensorReading(t *testing.T) { cf := cartofacade.Mock{} config := Config{ - Logger: logger, - CartoFacade: &cf, - DataRateMs: 200, - Timeout: 10 * time.Second, + Logger: logger, + CartoFacade: &cf, + LidarDataRateMsec: 200, + Timeout: 10 * time.Second, } ctx := context.Background() t.Run("returns error when lidar GetData returns error, doesn't try to add sensor data", func(t *testing.T) { - sensors := []string{"invalid_sensor"} + cam := "invalid_lidar" invalidSensorTestHelper( ctx, t, cf, config, - sensors, + cam, ) }) t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { - sensors := []string{"invalid_replay_sensor"} + cam := "invalid_replay_lidar" invalidSensorTestHelper( ctx, t, cf, config, - sensors, + cam, ) }) t.Run("replay sensor adds sensor data until success", func(t *testing.T) { - sensors := []string{"replay_sensor"} + cam := "replay_lidar" logger := golog.NewTestLogger(t) - replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(sensors), sensors, logger) + replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(cam), cam, logger) test.That(t, err, test.ShouldBeNil) var calls []addSensorReadingArgs @@ -351,7 +351,7 @@ func TestAddSensorReading(t *testing.T) { firstTimestamp := calls[0].readingTimestamp for i, call := range calls { t.Logf("call %d", i) - test.That(t, call.sensorName, test.ShouldResemble, "replay_sensor") + test.That(t, call.sensorName, test.ShouldResemble, "replay_lidar") test.That(t, call.currentReading, test.ShouldResemble, expectedPCD) test.That(t, call.timeout, test.ShouldEqual, config.Timeout) test.That(t, call.readingTimestamp, test.ShouldEqual, firstTimestamp) @@ -359,9 +359,9 @@ func TestAddSensorReading(t *testing.T) { }) t.Run("live sensor adds sensor reading once and ignores errors", func(t *testing.T) { - sensors := []string{"good_lidar"} + cam := "good_lidar" logger := golog.NewTestLogger(t) - liveSensor, err := s.NewLidar(context.Background(), s.SetupDeps(sensors), sensors, logger) + liveSensor, err := s.NewLidar(context.Background(), s.SetupDeps(cam), cam, logger) test.That(t, err, test.ShouldBeNil) var calls []addSensorReadingArgs @@ -415,9 +415,9 @@ func TestAddSensorReading(t *testing.T) { }) t.Run("returns true when lidar returns an error that it reached end of dataset", func(t *testing.T) { - sensors := []string{"finished_replay_sensor"} + cam := "finished_replay_lidar" logger := golog.NewTestLogger(t) - replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(sensors), sensors, logger) + replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(cam), cam, logger) test.That(t, err, test.ShouldBeNil) config.Lidar = replaySensor @@ -432,17 +432,17 @@ func TestStart(t *testing.T) { cf := cartofacade.Mock{} config := Config{ - Logger: logger, - CartoFacade: &cf, - DataRateMs: 200, - Timeout: 10 * time.Second, + Logger: logger, + CartoFacade: &cf, + LidarDataRateMsec: 200, + Timeout: 10 * time.Second, } cancelCtx, cancelFunc := context.WithCancel(context.Background()) t.Run("returns true when lidar returns an error that it reached end of dataset but the context is valid", func(t *testing.T) { - sensors := []string{"finished_replay_sensor"} + cam := "finished_replay_lidar" logger := golog.NewTestLogger(t) - replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(sensors), sensors, logger) + replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(cam), cam, logger) test.That(t, err, test.ShouldBeNil) config.Lidar = replaySensor @@ -452,9 +452,9 @@ func TestStart(t *testing.T) { }) t.Run("returns false when lidar returns an error that it reached end of dataset but the context was cancelled", func(t *testing.T) { - sensors := []string{"finished_replay_sensor"} + cam := "finished_replay_lidar" logger := golog.NewTestLogger(t) - replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(sensors), sensors, logger) + replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(cam), cam, logger) test.That(t, err, test.ShouldBeNil) config.Lidar = replaySensor diff --git a/sensors/sensors.go b/sensors/sensors.go index 5cf7b6d9..729c44bd 100644 --- a/sensors/sensors.go +++ b/sensors/sensors.go @@ -4,7 +4,6 @@ package sensors import ( "bytes" "context" - "strings" "time" "github.com/edaniels/golog" @@ -17,13 +16,6 @@ import ( goutils "go.viam.com/utils" ) -const ( - // The Lidar is expected to be located at the first - // index in the provided `sensors` array in the slam - // service configuration. - lidarIndex = 0 -) - // Lidar represents a LIDAR sensor. type Lidar struct { Name string @@ -46,37 +38,31 @@ type TimedSensor interface { func NewLidar( ctx context.Context, deps resource.Dependencies, - sensors []string, + cameraName string, logger golog.Logger, ) (Lidar, error) { _, span := trace.StartSpan(ctx, "viamcartographer::sensors::NewLidar") defer span.End() - - // An empty `sensors: []` array is allowed in offline mode. - if len(sensors) == 0 { - logger.Debug("no sensor provided in 'sensors' config parameter") - return Lidar{}, nil - } - // If there is a sensor provided in the 'sensors' array, we enforce that only one - // sensor has to be provided. - if len(sensors) != 1 { - return Lidar{}, errors.Errorf("configuring lidar camera error: "+ - "'sensors' must contain only one lidar camera, but is 'sensors: [%v]'", - strings.Join(sensors, ", ")) - } - - name, err := getName(sensors, lidarIndex) + newLidar, err := camera.FromDependencies(deps, cameraName) if err != nil { - return Lidar{}, err + return Lidar{}, errors.Wrapf(err, "error getting lidar camera %v for slam service", cameraName) } - newLidar, err := camera.FromDependencies(deps, name) - if err != nil { - return Lidar{}, errors.Wrapf(err, "error getting lidar camera %v for slam service", name) - } + // 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 !properties.SupportsPCD { + // return Lidar{}, errors.New("configuring lidar camera error: " + + // "'camera' must support PCD") + // } return Lidar{ - Name: name, + Name: cameraName, lidar: newLidar, }, nil } @@ -144,11 +130,3 @@ func (lidar Lidar) TimedSensorReading(ctx context.Context) (TimedSensorReadingRe } return TimedSensorReadingResponse{Reading: buf.Bytes(), ReadingTime: readingTime, Replay: replay}, nil } - -// getName returns the name of the sensor based on its index in the sensor array. -func getName(sensors []string, index int) (string, error) { - if index < 0 || index >= len(sensors) { - return "", errors.New("index out of bounds") - } - return sensors[index], nil -} diff --git a/sensors/sensors_test.go b/sensors/sensors_test.go index f8899b8e..df2dcf5a 100644 --- a/sensors/sensors_test.go +++ b/sensors/sensors_test.go @@ -17,14 +17,20 @@ func TestValidateGetData(t *testing.T) { logger := golog.NewTestLogger(t) ctx := context.Background() - sensors := []string{"good_lidar"} - goodLidar, err := s.NewLidar(ctx, s.SetupDeps(sensors), sensors, logger) + lidar := "good_lidar" + goodLidar, err := s.NewLidar(ctx, s.SetupDeps(lidar), lidar, logger) test.That(t, err, test.ShouldBeNil) - sensors = []string{"invalid_sensor"} - invalidLidar, err := s.NewLidar(ctx, s.SetupDeps(sensors), sensors, logger) + 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")) + sensorValidationMaxTimeout := time.Duration(50) * time.Millisecond sensorValidationInterval := time.Duration(10) * time.Millisecond @@ -34,10 +40,9 @@ func TestValidateGetData(t *testing.T) { }) t.Run("returns nil if a lidar reading succeeds within the timeout", func(t *testing.T) { - sensors = []string{"warming_up_lidar"} - warmingUpLidar, err := s.NewLidar(ctx, s.SetupDeps(sensors), sensors, logger) + lidar = "warming_up_lidar" + warmingUpLidar, err := s.NewLidar(ctx, s.SetupDeps(lidar), lidar, logger) test.That(t, err, test.ShouldBeNil) - err = s.ValidateGetData(ctx, warmingUpLidar, sensorValidationMaxTimeout, sensorValidationInterval, logger) test.That(t, err, test.ShouldBeNil) }) @@ -51,8 +56,8 @@ func TestValidateGetData(t *testing.T) { cancelledCtx, cancelFunc := context.WithCancel(context.Background()) cancelFunc() - sensors = []string{"warming_up_lidar"} - warmingUpLidar, err := s.NewLidar(ctx, s.SetupDeps(sensors), sensors, logger) + lidar = "warming_up_lidar" + warmingUpLidar, err := s.NewLidar(ctx, s.SetupDeps(lidar), lidar, logger) test.That(t, err, test.ShouldBeNil) err = s.ValidateGetData(cancelledCtx, warmingUpLidar, sensorValidationMaxTimeout, sensorValidationInterval, logger) @@ -64,28 +69,18 @@ func TestNewLidar(t *testing.T) { logger := golog.NewTestLogger(t) t.Run("No sensor provided", func(t *testing.T) { - sensors := []string{} - deps := s.SetupDeps(sensors) - actualLidar, err := s.NewLidar(context.Background(), deps, sensors, logger) - expectedLidar := s.Lidar{} - test.That(t, actualLidar, test.ShouldResemble, expectedLidar) - test.That(t, err, test.ShouldBeNil) - }) - - t.Run("Failed lidar creation due to more than one sensor provided", func(t *testing.T) { - sensors := []string{"lidar", "one-too-many"} - deps := s.SetupDeps(sensors) - actualLidar, err := s.NewLidar(context.Background(), deps, sensors, logger) - expectedLidar := s.Lidar{} - test.That(t, actualLidar, test.ShouldResemble, expectedLidar) + lidar := "" + deps := s.SetupDeps(lidar) + _, err := s.NewLidar(context.Background(), deps, lidar, logger) test.That(t, err, test.ShouldBeError, - errors.New("configuring lidar camera error: 'sensors' must contain only one lidar camera, but is 'sensors: [lidar, one-too-many]'")) + errors.New("error getting lidar camera "+ + " for slam service: \"rdk:component:camera/\" missing from dependencies")) }) t.Run("Failed lidar creation with non-existing sensor", func(t *testing.T) { - sensors := []string{"gibberish"} - deps := s.SetupDeps(sensors) - actualLidar, err := s.NewLidar(context.Background(), deps, sensors, logger) + lidar := "gibberish" + deps := s.SetupDeps(lidar) + actualLidar, err := s.NewLidar(context.Background(), deps, lidar, logger) expectedLidar := s.Lidar{} test.That(t, actualLidar, test.ShouldResemble, expectedLidar) test.That(t, err, test.ShouldBeError, @@ -94,11 +89,11 @@ func TestNewLidar(t *testing.T) { }) t.Run("Successful lidar creation", func(t *testing.T) { - sensors := []string{"good_lidar"} + lidar := "good_lidar" ctx := context.Background() - deps := s.SetupDeps(sensors) - actualLidar, err := s.NewLidar(ctx, deps, sensors, logger) - test.That(t, actualLidar.Name, test.ShouldEqual, sensors[0]) + deps := s.SetupDeps(lidar) + actualLidar, err := s.NewLidar(ctx, deps, lidar, logger) + test.That(t, actualLidar.Name, test.ShouldEqual, lidar) test.That(t, err, test.ShouldBeNil) tsr, err := actualLidar.TimedSensorReading(ctx) @@ -111,20 +106,20 @@ func TestTimedSensorReading(t *testing.T) { logger := golog.NewTestLogger(t) ctx := context.Background() - sensors := []string{"invalid_sensor"} - invalidLidar, err := s.NewLidar(ctx, s.SetupDeps(sensors), sensors, logger) + lidar := "invalid_lidar" + invalidLidar, err := s.NewLidar(ctx, s.SetupDeps(lidar), lidar, logger) test.That(t, err, test.ShouldBeNil) - sensors = []string{"invalid_replay_sensor"} - invalidReplayLidar, err := s.NewLidar(ctx, s.SetupDeps(sensors), sensors, logger) + lidar = "invalid_replay_lidar" + invalidReplayLidar, err := s.NewLidar(ctx, s.SetupDeps(lidar), lidar, logger) test.That(t, err, test.ShouldBeNil) - sensors = []string{"good_lidar"} - goodLidar, err := s.NewLidar(ctx, s.SetupDeps(sensors), sensors, logger) + lidar = "good_lidar" + goodLidar, err := s.NewLidar(ctx, s.SetupDeps(lidar), lidar, logger) test.That(t, err, test.ShouldBeNil) - sensors = []string{"replay_sensor"} - goodReplayLidar, err := s.NewLidar(ctx, s.SetupDeps(sensors), sensors, logger) + lidar = "replay_lidar" + goodReplayLidar, err := s.NewLidar(ctx, s.SetupDeps(lidar), lidar, logger) test.That(t, err, test.ShouldBeNil) t.Run("when the lidar returns an error, returns that error", func(t *testing.T) { diff --git a/sensors/test_deps.go b/sensors/test_deps.go index adc3d919..2d32935f 100644 --- a/sensors/test_deps.go +++ b/sensors/test_deps.go @@ -21,30 +21,26 @@ const ( BadTime = "NOT A TIME" ) -// SetupDeps returns the dependencies based on the sensors passed as arguments. -func SetupDeps(sensors []string) resource.Dependencies { +// SetupDeps returns the dependencies based on the lidar passed as argument. +func SetupDeps(lidarName string) resource.Dependencies { deps := make(resource.Dependencies) - - for _, sensor := range sensors { - switch sensor { - case "good_lidar": - deps[camera.Named(sensor)] = getGoodLidar() - case "warming_up_lidar": - deps[camera.Named(sensor)] = getWarmingUpLidar() - case "replay_sensor": - deps[camera.Named(sensor)] = getReplaySensor(TestTime) - case "invalid_replay_sensor": - deps[camera.Named(sensor)] = getReplaySensor(BadTime) - case "invalid_sensor": - deps[camera.Named(sensor)] = getInvalidSensor() - case "gibberish": - return deps - case "finished_replay_sensor": - deps[camera.Named(sensor)] = getFinishedReplaySensor() - default: - continue - } + switch lidarName { + case "good_lidar": + deps[camera.Named(lidarName)] = getGoodLidar() + case "warming_up_lidar": + deps[camera.Named(lidarName)] = getWarmingUpLidar() + case "replay_lidar": + 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 "gibberish_lidar": + return deps + case "finished_replay_lidar": + deps[camera.Named(lidarName)] = getFinishedReplayLidar() } + return deps } @@ -87,7 +83,7 @@ func getGoodLidar() *inject.Camera { return cam } -func getReplaySensor(testTime string) *inject.Camera { +func getReplayLidar(testTime string) *inject.Camera { cam := &inject.Camera{} cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { md := ctx.Value(contextutils.MetadataContextKey) @@ -108,7 +104,7 @@ func getReplaySensor(testTime string) *inject.Camera { return cam } -func getInvalidSensor() *inject.Camera { +func getInvalidLidar() *inject.Camera { cam := &inject.Camera{} cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { return nil, errors.New("invalid sensor") @@ -119,10 +115,13 @@ func getInvalidSensor() *inject.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{}, nil + } return cam } -func getFinishedReplaySensor() *inject.Camera { +func getFinishedReplayLidar() *inject.Camera { cam := &inject.Camera{} cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { return nil, replaypcd.ErrEndOfDataset diff --git a/testhelper/testhelper.go b/testhelper/testhelper.go index f7b1065c..c31fd6c5 100644 --- a/testhelper/testhelper.go +++ b/testhelper/testhelper.go @@ -9,6 +9,7 @@ import ( "path" "path/filepath" "strconv" + "strings" "testing" "time" @@ -48,34 +49,32 @@ const ( var mockLidarPath = artifact.MustPath("viam-cartographer/mock_lidar") -// SetupStubDeps returns stubbed dependencies based on the sensors +// SetupStubDeps returns stubbed dependencies based on the camera // the stubs fail tests if called. -func SetupStubDeps(sensors []string, t *testing.T) resource.Dependencies { +func SetupStubDeps(cameraName string, t *testing.T) resource.Dependencies { deps := make(resource.Dependencies) - - for _, sensor := range sensors { - switch sensor { - case "stub_lidar": - deps[camera.Named(sensor)] = getStubLidar(t) - default: - t.Errorf("SetupStubDeps calld with unhandled sensor sensors: %s, %v", sensor, sensors) - } + switch cameraName { + case "stub_lidar": + deps[camera.Named(cameraName)] = getStubLidar(t) + default: + t.Errorf("SetupStubDeps called with unhandled camera: %s", cameraName) } + return deps } func getStubLidar(t *testing.T) *inject.Camera { cam := &inject.Camera{} cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { - t.Error("stub lidar NextPointCloud called") + t.Error("TEST FAILED stub lidar NextPointCloud called") return nil, errors.New("invalid sensor") } cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { - t.Error("stub lidar Stream called") + t.Error("TEST FAILED stub lidar Stream called") return nil, errors.New("invalid sensor") } cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { - t.Error("stub lidar Projector called") + t.Error("TEST FAILED stub lidar Projector called") return nil, transform.NewNoIntrinsicsError("") } return cam @@ -127,7 +126,7 @@ func mockLidarReadingsValid() error { // ensure test outputs of cartographer are determanistic. func IntegrationLidarTimedSensor( t *testing.T, - sensor string, + lidar string, replay bool, sensorReadingInterval time.Duration, done chan struct{}, @@ -157,19 +156,19 @@ func IntegrationLidarTimedSensor( file, err := os.Open(artifact.MustPath("viam-cartographer/mock_lidar/" + strconv.FormatUint(i, 10) + ".pcd")) if err != nil { - t.Error("TimedSensorReading Mock failed to open pcd file") + t.Error("TEST FAILED TimedSensorReading Mock failed to open pcd file") return s.TimedSensorReadingResponse{}, err } readingPc, err := pointcloud.ReadPCD(file) if err != nil { - t.Error("TimedSensorReading Mock failed to read pcd") + t.Error("TEST FAILED TimedSensorReading Mock failed to read pcd") return s.TimedSensorReadingResponse{}, err } buf := new(bytes.Buffer) err = pointcloud.ToPCD(readingPc, buf, pointcloud.PCDBinary) if err != nil { - t.Error("TimedSensorReading Mock failed to parse pcd") + t.Error("TEST FAILED TimedSensorReading Mock failed to parse pcd") return s.TimedSensorReadingResponse{}, err } @@ -193,7 +192,7 @@ func ClearDirectory(t *testing.T, path string) { func CreateIntegrationSLAMService( t *testing.T, cfg *vcConfig.Config, - timedSensor s.TimedSensor, + timedLidar s.TimedSensor, logger golog.Logger, ) (slam.Service, error) { ctx := context.Background() @@ -204,8 +203,8 @@ func CreateIntegrationSLAMService( if err != nil { return nil, err } - test.That(t, sensorDeps, test.ShouldResemble, cfg.Sensors) - deps := SetupStubDeps(cfg.Sensors, t) + test.That(t, sensorDeps, test.ShouldResemble, []string{cfg.Camera["name"]}) + deps := SetupStubDeps(cfg.Camera["name"], t) svc, err := viamcartographer.New( ctx, @@ -215,7 +214,7 @@ func CreateIntegrationSLAMService( SensorValidationMaxTimeoutSecForTest, SensorValidationIntervalSecForTest, 5*time.Second, - timedSensor, + timedLidar, ) if err != nil { test.That(t, svc, test.ShouldBeNil) @@ -239,13 +238,26 @@ func CreateSLAMService( cfgService := resource.Config{Name: "test", API: slam.API, Model: viamcartographer.Model} cfgService.ConvertedAttributes = cfg - deps := s.SetupDeps(cfg.Sensors) - sensorDeps, err := cfg.Validate("path") if err != nil { return nil, err } - test.That(t, sensorDeps, test.ShouldResemble, cfg.Sensors) + + // feature flag for IMU Integration + cameraName := "" + if cfg.IMUIntegrationEnabled { + cameraName = cfg.Camera["name"] + } else { + if len(cfg.Sensors) > 1 { + return nil, errors.Errorf("configuring lidar camera error: "+ + "'sensors' must contain only one lidar camera, but is 'sensors: [%v]'", + strings.Join(cfg.Sensors, ", ")) + } + cameraName = cfg.Sensors[0] + } + test.That(t, sensorDeps, test.ShouldResemble, []string{cameraName}) + + deps := s.SetupDeps(cameraName) svc, err := viamcartographer.New( ctx, @@ -322,6 +334,6 @@ func InitInternalState(t *testing.T) (string, func()) { // CreateTimestampFilename creates an absolute filename with a primary sensor name and timestamp written // into the filename. -func CreateTimestampFilename(dataDirectory, primarySensorName, fileType string, timeStamp time.Time) string { - return filepath.Join(dataDirectory, primarySensorName+"_data_"+timeStamp.UTC().Format(SlamTimeFormat)+fileType) +func CreateTimestampFilename(dataDirectory, lidarName, fileType string, timeStamp time.Time) string { + return filepath.Join(dataDirectory, lidarName+"_data_"+timeStamp.UTC().Format(SlamTimeFormat)+fileType) } diff --git a/viam-cartographer/src/carto_facade/carto_facade.cc b/viam-cartographer/src/carto_facade/carto_facade.cc index 698ed8ca..d845a5ae 100644 --- a/viam-cartographer/src/carto_facade/carto_facade.cc +++ b/viam-cartographer/src/carto_facade/carto_facade.cc @@ -111,26 +111,21 @@ void validate_lidar_config(viam_carto_LIDAR_CONFIG lidar_config) { config from_viam_carto_config(viam_carto_config vcc) { struct config c; - for (int i = 0; i < vcc.sensors_len; i++) { - c.sensors.push_back(to_std_string(vcc.sensors[i])); - } + c.camera = to_std_string(vcc.camera); c.data_dir = to_std_string(vcc.data_dir); c.map_rate_sec = std::chrono::seconds(vcc.map_rate_sec); c.lidar_config = vcc.lidar_config; - if (c.sensors.size() == 0) { - throw VIAM_CARTO_SENSORS_LIST_EMPTY; - } if (c.data_dir.size() == 0) { throw VIAM_CARTO_DATA_DIR_NOT_PROVIDED; } if (vcc.map_rate_sec < 0) { throw VIAM_CARTO_MAP_RATE_SEC_INVALID; } - if (c.sensors[0].empty()) { + if (c.camera.empty()) { throw VIAM_CARTO_COMPONENT_REFERENCE_INVALID; } validate_lidar_config(c.lidar_config); - c.component_reference = bstrcpy(vcc.sensors[0]); + c.component_reference = bstrcpy(vcc.camera); return c; }; @@ -886,18 +881,15 @@ extern int viam_carto_init(viam_carto **ppVC, viam_carto_lib *pVCL, if (ppVC == nullptr) { return VIAM_CARTO_VC_INVALID; } - if (pVCL == nullptr) { return VIAM_CARTO_LIB_INVALID; } - // allocate viam_carto struct viam_carto *vc = (viam_carto *)malloc(sizeof(viam_carto)); if (vc == nullptr) { return VIAM_CARTO_OUT_OF_MEMORY; } viam::carto_facade::CartoFacade *cf; - try { cf = new viam::carto_facade::CartoFacade(pVCL, c, ac); } catch (int err) { diff --git a/viam-cartographer/src/carto_facade/carto_facade.h b/viam-cartographer/src/carto_facade/carto_facade.h index fb4fff39..e40ed16a 100644 --- a/viam-cartographer/src/carto_facade/carto_facade.h +++ b/viam-cartographer/src/carto_facade/carto_facade.h @@ -87,7 +87,6 @@ typedef enum viam_carto_LIDAR_CONFIG { #define VIAM_CARTO_LIB_PLATFORM_INVALID 5 #define VIAM_CARTO_LIB_INVALID 6 #define VIAM_CARTO_LIB_NOT_INITIALIZED 7 -#define VIAM_CARTO_SENSORS_LIST_EMPTY 8 #define VIAM_CARTO_UNKNOWN_ERROR 9 #define VIAM_CARTO_DATA_DIR_NOT_PROVIDED 10 #define VIAM_CARTO_SLAM_MODE_INVALID 11 @@ -130,8 +129,7 @@ typedef struct viam_carto_algo_config { } viam_carto_algo_config; typedef struct viam_carto_config { - bstring *sensors; - int sensors_len; + bstring camera; int map_rate_sec; bstring data_dir; viam_carto_LIDAR_CONFIG lidar_config; @@ -292,7 +290,7 @@ static const int checkForShutdownIntervalMicroseconds = 1e5; static const double resolutionMeters = 0.05; typedef struct config { - std::vector sensors; + std::string camera; std::chrono::seconds map_rate_sec; std::string data_dir; bstring component_reference; diff --git a/viam-cartographer/src/carto_facade/carto_facade_test.cc b/viam-cartographer/src/carto_facade/carto_facade_test.cc index 04eea2bd..cc7a04a7 100644 --- a/viam-cartographer/src/carto_facade/carto_facade_test.cc +++ b/viam-cartographer/src/carto_facade/carto_facade_test.cc @@ -28,32 +28,21 @@ const auto tol = tt::tolerance(0.001); namespace viam { namespace carto_facade { -viam_carto_config viam_carto_config_setup( - int map_rate_sec, viam_carto_LIDAR_CONFIG lidar_config, - std::string data_dir, std::vector sensors_vec) { +viam_carto_config viam_carto_config_setup(int map_rate_sec, + viam_carto_LIDAR_CONFIG lidar_config, + std::string data_dir, + std::string camera) { struct viam_carto_config vcc; vcc.map_rate_sec = map_rate_sec; vcc.lidar_config = lidar_config; vcc.data_dir = bfromcstr(data_dir.c_str()); - bstring *sensors = - (bstring *)malloc(sizeof(bstring *) * sensors_vec.size()); - BOOST_TEST(sensors != nullptr); - int i = 0; - for (auto &&s : sensors_vec) { - sensors[i] = bfromcstr(s.c_str()); - i++; - } - vcc.sensors_len = sensors_vec.size(); - vcc.sensors = sensors; + vcc.camera = bfromcstr(camera.c_str()); return vcc; } void viam_carto_config_teardown(viam_carto_config vcc) { BOOST_TEST(bdestroy(vcc.data_dir) == BSTR_OK); - for (int i = 0; i < vcc.sensors_len; i++) { - BOOST_TEST(bdestroy(vcc.sensors[i]) == BSTR_OK); - } - free(vcc.sensors); + BOOST_TEST(bdestroy(vcc.camera) == BSTR_OK); } viam_carto_sensor_reading new_test_sensor_reading( std::string sensor, std::string pcd_path, @@ -119,47 +108,34 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_validate) { BOOST_TEST(viam_carto_lib_init(&lib, 0, 1) == VIAM_CARTO_SUCCESS); viam_carto *vc; - std::vector empty_sensors_vec; fs::path tmp_dir = fs::temp_directory_path() / fs::path(bfs::unique_path().string()); - struct viam_carto_config vcc_no_sensors = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, tmp_dir.string(), empty_sensors_vec); struct viam_carto_algo_config ac = viam_carto_algo_config_setup(); - BOOST_TEST(viam_carto_init(&vc, lib, vcc_no_sensors, ac) == - VIAM_CARTO_SENSORS_LIST_EMPTY); - - std::vector sensors_vec; - sensors_vec.push_back("sensor_1"); - sensors_vec.push_back("sensor_2"); - sensors_vec.push_back("sensor_3"); - sensors_vec.push_back("sensor_4"); - sensors_vec.push_back("sensor_5"); + std::string camera = "lidar"; struct viam_carto_config vcc_empty_data_dir = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, "", sensors_vec); + viam_carto_config_setup(1, VIAM_CARTO_THREE_D, "", camera); BOOST_TEST(viam_carto_init(&vc, lib, vcc_empty_data_dir, ac) == VIAM_CARTO_DATA_DIR_NOT_PROVIDED); - std::vector sensors_vec2; - sensors_vec2.push_back(""); - sensors_vec2.push_back("sensor_2"); + std::string camera2 = ""; + struct viam_carto_config vcc_empty_component_ref = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, tmp_dir.string(), sensors_vec2); + 1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera2); BOOST_TEST(viam_carto_init(&vc, lib, vcc_empty_component_ref, ac) == VIAM_CARTO_COMPONENT_REFERENCE_INVALID); struct viam_carto_config vcc_invalid_map_rate_sec = viam_carto_config_setup( - -1, VIAM_CARTO_THREE_D, tmp_dir.string(), sensors_vec); + -1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera); BOOST_TEST(viam_carto_init(&vc, lib, vcc_invalid_map_rate_sec, ac) == VIAM_CARTO_MAP_RATE_SEC_INVALID); - struct viam_carto_config vcc_invalid_lidar_config = - viam_carto_config_setup(1, static_cast(-1), - tmp_dir.string(), sensors_vec); + struct viam_carto_config vcc_invalid_lidar_config = viam_carto_config_setup( + 1, static_cast(-1), tmp_dir.string(), camera); BOOST_TEST(viam_carto_init(&vc, lib, vcc_invalid_lidar_config, ac) == VIAM_CARTO_LIDAR_CONFIG_INVALID); @@ -168,22 +144,20 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_validate) { fs::create_directories(deprecated_path.string() + "/data"); struct viam_carto_config vcc_deprecated_path = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, deprecated_path.string(), sensors_vec); + 1, VIAM_CARTO_THREE_D, deprecated_path.string(), camera); BOOST_TEST(viam_carto_init(&vc, lib, vcc_deprecated_path, ac) == VIAM_CARTO_DATA_DIR_INVALID_DEPRECATED_STRUCTURE); fs::path invalid_path = tmp_dir / fs::path(bfs::unique_path().string()) / fs::path(bfs::unique_path().string()); - struct viam_carto_config vcc_invalid_path = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, invalid_path.string(), - - sensors_vec); + struct viam_carto_config vcc_invalid_path = viam_carto_config_setup( + 1, VIAM_CARTO_THREE_D, invalid_path.string(), camera); BOOST_TEST(viam_carto_init(&vc, lib, vcc_invalid_path, ac) == VIAM_CARTO_DATA_DIR_FILE_SYSTEM_ERROR); struct viam_carto_config vcc = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, tmp_dir.string(), sensors_vec); + 1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera); BOOST_TEST(viam_carto_init(nullptr, lib, vcc, ac) == VIAM_CARTO_VC_INVALID); BOOST_TEST(viam_carto_init(nullptr, nullptr, vcc, ac) == @@ -203,7 +177,6 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_validate) { // can't terminate a carto instance that has already been terminated BOOST_TEST(viam_carto_terminate(&vc) == VIAM_CARTO_VC_INVALID); - viam_carto_config_teardown(vcc_no_sensors); viam_carto_config_teardown(vcc_empty_data_dir); viam_carto_config_teardown(vcc_empty_component_ref); viam_carto_config_teardown(vcc_invalid_map_rate_sec); @@ -223,15 +196,9 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode) { viam_carto_lib *lib; BOOST_TEST(viam_carto_lib_init(&lib, 0, 1) == VIAM_CARTO_SUCCESS); - std::vector empty_sensors_vec; + std::string camera = "lidar"; fs::path tmp_dir = fs::temp_directory_path() / fs::path(bfs::unique_path().string()); - std::vector sensors_vec; - sensors_vec.push_back("sensor_1"); - sensors_vec.push_back("sensor_2"); - sensors_vec.push_back("sensor_3"); - sensors_vec.push_back("sensor_4"); - sensors_vec.push_back("sensor_5"); struct viam_carto_algo_config ac = viam_carto_algo_config_setup(); fs::create_directory(tmp_dir); @@ -240,7 +207,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode) { viam_carto *vc1; auto mapping_dir = tmp_dir / fs::path("mapping_dir"); struct viam_carto_config vcc_mapping = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, mapping_dir.string(), sensors_vec); + 1, VIAM_CARTO_THREE_D, mapping_dir.string(), camera); BOOST_TEST(viam_carto_init(&vc1, lib, vcc_mapping, ac) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc1->slam_mode == VIAM_CARTO_SLAM_MODE_MAPPING); @@ -300,7 +267,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode) { viam_carto *vc2; struct viam_carto_config vcc_updating = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, updating_dir.string(), sensors_vec); + 1, VIAM_CARTO_THREE_D, updating_dir.string(), camera); BOOST_TEST(viam_carto_init(&vc2, lib, vcc_updating, ac) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc2->slam_mode == VIAM_CARTO_SLAM_MODE_UPDATING); @@ -340,7 +307,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode) { // updating optimize_on_start viam_carto *vc3; struct viam_carto_config vcc_updating = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, updating_dir.string(), sensors_vec); + 1, VIAM_CARTO_THREE_D, updating_dir.string(), camera); BOOST_TEST(viam_carto_init(&vc3, lib, vcc_updating, ac_optimize_on_start) == VIAM_CARTO_SUCCESS); @@ -356,7 +323,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode) { // localizing viam_carto *vc4; struct viam_carto_config vcc_localizing = viam_carto_config_setup( - 0, VIAM_CARTO_THREE_D, updating_dir.string(), sensors_vec); + 0, VIAM_CARTO_THREE_D, updating_dir.string(), camera); BOOST_TEST(viam_carto_init(&vc4, lib, vcc_localizing, ac) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc4->slam_mode == VIAM_CARTO_SLAM_MODE_LOCALIZING); @@ -389,7 +356,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode) { // localizing optimize_on_start viam_carto *vc5; struct viam_carto_config vcc_localizing = viam_carto_config_setup( - 0, VIAM_CARTO_THREE_D, updating_dir.string(), sensors_vec); + 0, VIAM_CARTO_THREE_D, updating_dir.string(), camera); BOOST_TEST(viam_carto_init(&vc5, lib, vcc_localizing, ac_optimize_on_start) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc5->slam_mode == VIAM_CARTO_SLAM_MODE_LOCALIZING); @@ -406,7 +373,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode) { ; viam_carto *vc6; struct viam_carto_config vcc_invalid = viam_carto_config_setup( - 0, VIAM_CARTO_THREE_D, empty_dir.string(), sensors_vec); + 0, VIAM_CARTO_THREE_D, empty_dir.string(), camera); BOOST_TEST(viam_carto_init(&vc6, lib, vcc_invalid, ac) == VIAM_CARTO_SLAM_MODE_INVALID); viam_carto_config_teardown(vcc_invalid); @@ -424,16 +391,11 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_terminate) { BOOST_TEST(viam_carto_lib_init(&lib, 0, 1) == VIAM_CARTO_SUCCESS); viam_carto *vc; - std::vector sensors_vec; - sensors_vec.push_back("sensor_1"); - sensors_vec.push_back("sensor_2"); - sensors_vec.push_back("sensor_3"); - sensors_vec.push_back("sensor_4"); - sensors_vec.push_back("sensor_5"); + std::string camera = "lidar"; fs::path tmp_dir = fs::temp_directory_path() / fs::path(bfs::unique_path().string()); struct viam_carto_config vcc = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, tmp_dir.string(), sensors_vec); + 1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera); struct viam_carto_algo_config ac = viam_carto_algo_config_setup(); BOOST_TEST(viam_carto_init(&vc, lib, vcc, ac) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc->slam_mode == VIAM_CARTO_SLAM_MODE_MAPPING); @@ -457,10 +419,10 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_terminate) { BOOST_TEST((cf->path_to_internal_state) == path_to_internal_state.string()); BOOST_TEST(((cf->state) == CartoFacadeState::IO_INITIALIZED)); - BOOST_TEST((cf->config.sensors) == sensors_vec); + BOOST_TEST((cf->config.camera) == camera); BOOST_TEST((cf->config.map_rate_sec).count() == 1); BOOST_TEST((cf->config.data_dir) == tmp_dir.string()); - BOOST_TEST(to_std_string(cf->config.component_reference) == "sensor_1"); + BOOST_TEST(to_std_string(cf->config.component_reference) == "lidar"); BOOST_TEST((cf->config.lidar_config) == VIAM_CARTO_THREE_D); BOOST_TEST(viam_carto_terminate(&vc) == VIAM_CARTO_SUCCESS); @@ -478,18 +440,13 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { // Setup viam_carto *vc; - std::vector sensors_vec; - sensors_vec.push_back("sensor_1"); - sensors_vec.push_back("sensor_2"); - sensors_vec.push_back("sensor_3"); - sensors_vec.push_back("sensor_4"); - sensors_vec.push_back("sensor_5"); + std::string camera = "lidar"; fs::path tmp_dir = fs::temp_directory_path() / fs::path(bfs::unique_path().string()); struct viam_carto_config vcc = viam_carto_config_setup(60, VIAM_CARTO_THREE_D, tmp_dir.string(), - sensors_vec); + camera); struct viam_carto_algo_config ac = viam_carto_algo_config_setup(); BOOST_TEST(viam_carto_init(&vc, lib, vcc, ac) == VIAM_CARTO_SUCCESS); @@ -499,7 +456,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { // AddSensorReading { viam_carto_sensor_reading sr = new_test_sensor_reading( - "sensor_1", ".artifact/data/viam-cartographer/mock_lidar/0.pcd", + "lidar", ".artifact/data/viam-cartographer/mock_lidar/0.pcd", 1687900053773475); viam::carto_facade::CartoFacade *cf = static_cast(vc->carto_obj); @@ -558,7 +515,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { BOOST_TEST(pr.jmag == 0); BOOST_TEST(pr.kmag == 0); BOOST_TEST(pr.real == 1); - BOOST_TEST(to_std_string(pr.component_reference) == "sensor_1"); + BOOST_TEST(to_std_string(pr.component_reference) == "lidar"); BOOST_TEST(viam_carto_get_position_response_destroy(&pr) == VIAM_CARTO_SUCCESS); @@ -615,7 +572,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { { viam_carto_sensor_reading sr; // must be they first sensor in the sensor list - sr.sensor = bfromcstr("sensor_1"); + sr.sensor = bfromcstr("lidar"); std::string pcd = "empty lidar reading"; // passing 0 as the second parameter makes the string empty sr.sensor_reading = blk2bstr(pcd.c_str(), 0); @@ -631,7 +588,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { { viam_carto_sensor_reading sr; // must be they first sensor in the sensor list - sr.sensor = bfromcstr("sensor_1"); + sr.sensor = bfromcstr("lidar"); std::string pcd = "invalid lidar reading"; sr.sensor_reading = blk2bstr(pcd.c_str(), pcd.length()); BOOST_TEST(sr.sensor_reading != nullptr); @@ -645,7 +602,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { // unable to aquire lock { viam_carto_sensor_reading sr = new_test_sensor_reading( - "sensor_1", ".artifact/data/viam-cartographer/mock_lidar/0.pcd", + "lidar", ".artifact/data/viam-cartographer/mock_lidar/0.pcd", 1687900053773475); viam::carto_facade::CartoFacade *cf = static_cast(vc->carto_obj); @@ -703,7 +660,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { BOOST_TEST(pr.jmag == 0); BOOST_TEST(pr.kmag == 0); BOOST_TEST(pr.real == 1); - BOOST_TEST(to_std_string(pr.component_reference) == "sensor_1"); + BOOST_TEST(to_std_string(pr.component_reference) == "lidar"); BOOST_TEST(viam_carto_get_position_response_destroy(&pr) == VIAM_CARTO_SUCCESS); @@ -722,7 +679,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { { VLOG(1) << "viam_carto_add_sensor_reading 1"; viam_carto_sensor_reading sr = new_test_sensor_reading( - "sensor_1", ".artifact/data/viam-cartographer/mock_lidar/0.pcd", + "lidar", ".artifact/data/viam-cartographer/mock_lidar/0.pcd", 1629037851000000); BOOST_TEST(viam_carto_add_sensor_reading(vc, &sr) == VIAM_CARTO_SUCCESS); @@ -740,7 +697,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { BOOST_TEST(pr.jmag == 0); BOOST_TEST(pr.kmag == 0); BOOST_TEST(pr.real == 1); - BOOST_TEST(to_std_string(pr.component_reference) == "sensor_1"); + BOOST_TEST(to_std_string(pr.component_reference) == "lidar"); BOOST_TEST(viam_carto_get_position_response_destroy(&pr) == VIAM_CARTO_SUCCESS); @@ -769,7 +726,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { { VLOG(1) << "viam_carto_add_sensor_reading 2"; viam_carto_sensor_reading sr = new_test_sensor_reading( - "sensor_1", ".artifact/data/viam-cartographer/mock_lidar/1.pcd", + "lidar", ".artifact/data/viam-cartographer/mock_lidar/1.pcd", 1629037853000000); BOOST_TEST(viam_carto_add_sensor_reading(vc, &sr) == VIAM_CARTO_SUCCESS); @@ -789,7 +746,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { BOOST_TEST(pr.jmag == 0); BOOST_TEST(pr.kmag == 0); BOOST_TEST(pr.real == 1); - BOOST_TEST(to_std_string(pr.component_reference) == "sensor_1"); + BOOST_TEST(to_std_string(pr.component_reference) == "lidar"); BOOST_TEST(viam_carto_get_position_response_destroy(&pr) == VIAM_CARTO_SUCCESS); @@ -827,7 +784,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { { VLOG(1) << "viam_carto_add_sensor_reading 3"; viam_carto_sensor_reading sr = new_test_sensor_reading( - "sensor_1", ".artifact/data/viam-cartographer/mock_lidar/2.pcd", + "lidar", ".artifact/data/viam-cartographer/mock_lidar/2.pcd", 1629037855000000); BOOST_TEST(viam_carto_add_sensor_reading(vc, &sr) == VIAM_CARTO_SUCCESS); @@ -846,7 +803,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { BOOST_TEST(pr.jmag == 0); BOOST_TEST(pr.kmag != 0); BOOST_TEST(pr.real != 1); - BOOST_TEST(to_std_string(pr.component_reference) == "sensor_1"); + BOOST_TEST(to_std_string(pr.component_reference) == "lidar"); BOOST_TEST(viam_carto_get_position_response_destroy(&pr) == VIAM_CARTO_SUCCESS); @@ -875,7 +832,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { // AddSensorReading { viam_carto_sensor_reading sr = new_test_sensor_reading( - "sensor_1", ".artifact/data/viam-cartographer/mock_lidar/0.pcd", + "lidar", ".artifact/data/viam-cartographer/mock_lidar/0.pcd", 1687900053773475); viam::carto_facade::CartoFacade *cf = static_cast(vc->carto_obj); @@ -920,31 +877,21 @@ BOOST_AUTO_TEST_CASE(CartoFacade_config) { viam_carto_lib *lib; BOOST_TEST(viam_carto_lib_init(&lib, 0, 1) == VIAM_CARTO_SUCCESS); - std::vector sensors_vec; - sensors_vec.push_back("sensor_1"); - sensors_vec.push_back("sensor_2"); - sensors_vec.push_back("sensor_3"); - sensors_vec.push_back("sensor_4"); - sensors_vec.push_back("sensor_5"); + std::string camera = "lidar"; fs::path tmp_dir = fs::temp_directory_path() / fs::path(bfs::unique_path().string()); struct viam_carto_config vcc = viam_carto_config_setup(1, VIAM_CARTO_THREE_D, tmp_dir.string(), - sensors_vec); + camera); struct config c = viam::carto_facade::from_viam_carto_config(vcc); - BOOST_TEST(to_std_string(c.component_reference) == "sensor_1"); + BOOST_TEST(to_std_string(c.component_reference) == "lidar"); BOOST_TEST(c.data_dir == tmp_dir.string()); BOOST_TEST(c.lidar_config == VIAM_CARTO_THREE_D); BOOST_TEST(c.map_rate_sec.count() == 1); - BOOST_TEST(c.sensors.size() == 5); - BOOST_TEST(c.sensors[0] == "sensor_1"); - BOOST_TEST(c.sensors[1] == "sensor_2"); - BOOST_TEST(c.sensors[2] == "sensor_3"); - BOOST_TEST(c.sensors[3] == "sensor_4"); - BOOST_TEST(c.sensors[4] == "sensor_5"); + BOOST_TEST(c.camera == "lidar"); viam_carto_config_teardown(vcc); BOOST_TEST(bdestroy(c.component_reference) == BSTR_OK); @@ -965,18 +912,13 @@ BOOST_AUTO_TEST_CASE(CartoFacade_start_stop) { // Setup viam_carto *vc; - std::vector sensors_vec; - sensors_vec.push_back("sensor_1"); - sensors_vec.push_back("sensor_2"); - sensors_vec.push_back("sensor_3"); - sensors_vec.push_back("sensor_4"); - sensors_vec.push_back("sensor_5"); + std::string camera = "lidar"; fs::path tmp_dir = fs::temp_directory_path() / fs::path(bfs::unique_path().string()); struct viam_carto_config vcc = viam_carto_config_setup(1, VIAM_CARTO_THREE_D, tmp_dir.string(), - sensors_vec); + camera); struct viam_carto_algo_config ac = viam_carto_algo_config_setup(); BOOST_TEST(viam_carto_init(&vc, lib, vcc, ac) == VIAM_CARTO_SUCCESS); diff --git a/viam_cartographer.go b/viam_cartographer.go index 3c4fb2d9..e16f9ec8 100644 --- a/viam_cartographer.go +++ b/viam_cartographer.go @@ -37,7 +37,7 @@ var ( const ( // DefaultExecutableName is what this program expects to call to start the cartographer grpc server. DefaultExecutableName = "carto_grpc_server" - defaultDataRateMsec = 200 + defaultLidarDataRateMsec = 200 defaultMapRateSec = 60 defaultDialMaxTimeoutSec = 30 defaultSensorValidationMaxTimeoutSec = 30 @@ -117,12 +117,12 @@ func TerminateCartoLib() error { func initSensorProcess(cancelCtx context.Context, cartoSvc *CartographerService) { spConfig := sensorprocess.Config{ - CartoFacade: cartoSvc.cartofacade, - Lidar: cartoSvc.timedLidar, - LidarName: cartoSvc.primarySensorName, - DataRateMs: cartoSvc.dataRateMs, - Timeout: cartoSvc.cartoFacadeTimeout, - Logger: cartoSvc.logger, + CartoFacade: cartoSvc.cartofacade, + Lidar: cartoSvc.timedLidar, + LidarName: cartoSvc.lidarName, + LidarDataRateMsec: cartoSvc.lidarDataRateMsec, + Timeout: cartoSvc.cartoFacadeTimeout, + Logger: cartoSvc.logger, } cartoSvc.sensorProcessWorkers.Add(1) @@ -160,15 +160,26 @@ func New( c.Model.Name, svcConfig.ConfigParams["mode"]) } - dataRateMsec, mapRateSec := vcConfig.GetOptionalParameters( + lidarDataRateMsec, mapRateSec, err := vcConfig.GetOptionalParameters( svcConfig, - defaultDataRateMsec, + defaultLidarDataRateMsec, defaultMapRateSec, logger, ) + if err != nil { + return nil, err + } + + // feature flag for new config + name := "" + if svcConfig.IMUIntegrationEnabled { + name = svcConfig.Camera["name"] + } else { + name = svcConfig.Sensors[0] + } // Get the lidar for the Dim2D cartographer sub algorithm - lidar, err := s.NewLidar(ctx, deps, svcConfig.Sensors, logger) + lidar, err := s.NewLidar(ctx, deps, name, logger) if err != nil { return nil, err } @@ -180,22 +191,21 @@ func New( // use the override in testing if non nil // otherwise use the lidar from deps as the // timed sensor - timedSensor := testTimedSensorOverride - if timedSensor == nil { - timedSensor = lidar + timedLidar := testTimedSensorOverride + if timedLidar == nil { + timedLidar = lidar } // Cartographer SLAM Service Object cartoSvc := &CartographerService{ Named: c.ResourceName().AsNamed(), - primarySensorName: lidar.Name, + lidarName: lidar.Name, lidar: lidar, - timedLidar: timedSensor, + lidarDataRateMsec: lidarDataRateMsec, + timedLidar: timedLidar, subAlgo: subAlgo, configParams: svcConfig.ConfigParams, dataDirectory: svcConfig.DataDirectory, - sensors: svcConfig.Sensors, - dataRateMs: dataRateMsec, mapRateSec: mapRateSec, cancelSensorProcessFunc: cancelSensorProcessFunc, cancelCartoFacadeFunc: cancelCartoFacadeFunc, @@ -214,10 +224,9 @@ func New( } } }() - if err = s.ValidateGetData( cancelSensorProcessCtx, - timedSensor, + timedLidar, time.Duration(sensorValidationMaxTimeoutSec)*time.Second, time.Duration(cartoSvc.sensorValidationIntervalSec)*time.Second, cartoSvc.logger); err != nil { @@ -229,8 +238,8 @@ func New( if err != nil { return nil, err } - initSensorProcess(cancelSensorProcessCtx, cartoSvc) + return cartoSvc, nil } @@ -334,10 +343,10 @@ func initCartoFacade(ctx context.Context, cartoSvc *CartographerService) error { } cartoCfg := cartofacade.CartoConfig{ - Sensors: cartoSvc.sensors, + Camera: cartoSvc.lidarName, MapRateSecond: cartoSvc.mapRateSec, DataDir: cartoSvc.dataDirectory, - ComponentReference: cartoSvc.primarySensorName, + ComponentReference: cartoSvc.lidarName, LidarConfig: cartofacade.TwoD, } @@ -389,19 +398,18 @@ type CartographerService struct { mu sync.Mutex SlamMode cartofacade.SlamMode closed bool - primarySensorName string + lidarName string + lidarDataRateMsec int lidar s.Lidar timedLidar s.TimedSensor subAlgo SubAlgo configParams map[string]string dataDirectory string - sensors []string cartofacade cartofacade.Interface cartoFacadeTimeout time.Duration - dataRateMs int mapRateSec int cancelSensorProcessFunc func() @@ -440,7 +448,7 @@ func (cartoSvc *CartographerService) GetPosition(ctx context.Context) (spatialma "kmag": pos.Kmag, }, } - return CheckQuaternionFromClientAlgo(pose, cartoSvc.primarySensorName, returnedExt) + return CheckQuaternionFromClientAlgo(pose, cartoSvc.lidarName, returnedExt) } // GetPointCloudMap creates a request calls the slam algorithms GetPointCloudMap endpoint and returns a callback diff --git a/viam_cartographer_internal_test.go b/viam_cartographer_internal_test.go index 448a591f..38b74d6d 100644 --- a/viam_cartographer_internal_test.go +++ b/viam_cartographer_internal_test.go @@ -92,7 +92,7 @@ func TestGetPositionEndpoint(t *testing.T) { var inputQuat map[string]interface{} t.Run("empty component reference success", func(t *testing.T) { - svc.primarySensorName = "" + svc.lidarName = "" inputPose = commonv1.Pose{X: 0, Y: 0, Z: 0, OX: 0, OY: 0, OZ: 1, Theta: 0} inputQuat = map[string]interface{}{"real": 1.0, "imag": 0.0, "jmag": 0.0, "kmag": 0.0} @@ -100,7 +100,7 @@ func TestGetPositionEndpoint(t *testing.T) { }) t.Run("origin pose success", func(t *testing.T) { - svc.primarySensorName = "primarySensor1" + svc.lidarName = "primarySensor1" inputPose = commonv1.Pose{X: 0, Y: 0, Z: 0, OX: 0, OY: 0, OZ: 1, Theta: 0} inputQuat = map[string]interface{}{"real": 1.0, "imag": 0.0, "jmag": 0.0, "kmag": 0.0} @@ -108,7 +108,7 @@ func TestGetPositionEndpoint(t *testing.T) { }) t.Run("non origin pose success", func(t *testing.T) { - svc.primarySensorName = "primarySensor2" + svc.lidarName = "primarySensor2" inputPose = commonv1.Pose{X: 5, Y: 5, Z: 5, OX: 0, OY: 0, OZ: 1, Theta: 0} inputQuat = map[string]interface{}{"real": 1.0, "imag": 1.0, "jmag": 0.0, "kmag": 0.0} @@ -116,7 +116,7 @@ func TestGetPositionEndpoint(t *testing.T) { }) t.Run("error case", func(t *testing.T) { - svc.primarySensorName = "primarySensor3" + svc.lidarName = "primarySensor3" mockCartoFacade.GetPositionFunc = func( ctx context.Context, timeout time.Duration, diff --git a/viam_cartographer_test.go b/viam_cartographer_test.go index 9c551f62..a8fb65d7 100644 --- a/viam_cartographer_test.go +++ b/viam_cartographer_test.go @@ -25,6 +25,7 @@ import ( const ( testExecutableName = "true" // the program "true", not the boolean value + testDataFreqHz = "5" testDataRateMsec = 200 ) @@ -125,7 +126,7 @@ func TestNew(t *testing.T) { }() attrCfg := &vcConfig.Config{ - Sensors: []string{"invalid_sensor"}, + Sensors: []string{"invalid_lidar"}, ConfigParams: map[string]string{"mode": "2d"}, DataDirectory: dataDirectory, DataRateMsec: testDataRateMsec, @@ -145,7 +146,7 @@ func TestNew(t *testing.T) { defer fsCleanupFunc() attrCfg := &vcConfig.Config{ - Sensors: []string{"replay_sensor"}, + Sensors: []string{"replay_lidar"}, ConfigParams: map[string]string{"mode": "2d"}, DataDirectory: dataDirectory, MapRateSec: &_zeroInt, @@ -163,7 +164,7 @@ func TestNew(t *testing.T) { _, componentReference, err := svc.GetPosition(context.Background()) test.That(t, err, test.ShouldBeNil) - test.That(t, componentReference, test.ShouldEqual, "replay_sensor") + test.That(t, componentReference, test.ShouldEqual, "replay_lidar") pcmFunc, err := svc.GetPointCloudMap(context.Background()) test.That(t, err, test.ShouldBeNil) @@ -257,11 +258,185 @@ func TestNew(t *testing.T) { }) } +func TestNewFeatureFlag(t *testing.T) { + logger := golog.NewTestLogger(t) + + t.Run("Successful creation of cartographer slam service with good lidar with feature flag enabled", 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{ + Camera: map[string]string{"name": "good_lidar", "data_frequency_hz": testDataFreqHz}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + IMUIntegrationEnabled: true, + } + + svc, err := testhelper.CreateSLAMService(t, attrCfg, logger) + test.That(t, err, test.ShouldBeNil) + + test.That(t, svc.Close(context.Background()), test.ShouldBeNil) + }) + + t.Run("Failed creation of cartographer slam service with invalid sensor "+ + "that errors during call to NextPointCloud with feature flag enabled", 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{ + Camera: map[string]string{"name": "invalid_lidar", "data_frequency_hz": testDataFreqHz}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + IMUIntegrationEnabled: true, + } + + _, err = testhelper.CreateSLAMService(t, attrCfg, logger) + test.That(t, err, test.ShouldBeError, + + errors.New("failed to get data from lidar: ValidateGetData timeout: NextPointCloud error: invalid sensor")) + }) + + t.Run("Successful creation of cartographer slam service in localization mode with feature flag enabled", func(t *testing.T) { + termFunc := testhelper.InitTestCL(t, logger) + defer termFunc() + + dataDirectory, fsCleanupFunc := testhelper.InitInternalState(t) + defer fsCleanupFunc() + + attrCfg := &vcConfig.Config{ + Camera: map[string]string{"name": "good_lidar"}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + MapRateSec: &_zeroInt, + IMUIntegrationEnabled: true, + } + + svc, err := testhelper.CreateSLAMService(t, attrCfg, logger) + test.That(t, err, test.ShouldBeNil) + + cs, ok := svc.(*viamcartographer.CartographerService) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, cs.SlamMode, test.ShouldEqual, cartofacade.LocalizingMode) + + timestamp1, err := svc.GetLatestMapInfo(context.Background()) + test.That(t, err, test.ShouldBeNil) + + _, componentReference, err := svc.GetPosition(context.Background()) + test.That(t, err, test.ShouldBeNil) + test.That(t, componentReference, test.ShouldEqual, "good_lidar") + + pcmFunc, err := svc.GetPointCloudMap(context.Background()) + test.That(t, err, test.ShouldBeNil) + + pcm, err := slam.HelperConcatenateChunksToFull(pcmFunc) + test.That(t, err, test.ShouldBeNil) + test.That(t, pcm, test.ShouldNotBeNil) + + isFunc, err := svc.GetInternalState(context.Background()) + test.That(t, err, test.ShouldBeNil) + + is, err := slam.HelperConcatenateChunksToFull(isFunc) + test.That(t, err, test.ShouldBeNil) + test.That(t, is, test.ShouldNotBeNil) + + timestamp2, err := svc.GetLatestMapInfo(context.Background()) + test.That(t, err, test.ShouldBeNil) + test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue) + test.That(t, timestamp1, test.ShouldResemble, timestamp2) + + test.That(t, svc.Close(context.Background()), test.ShouldBeNil) + }) + + t.Run("Successful creation of cartographer slam service in non localization mode with feature flag enabled", func(t *testing.T) { + termFunc := testhelper.InitTestCL(t, logger) + defer termFunc() + + dataDirectory, fsCleanupFunc := testhelper.InitInternalState(t) + defer fsCleanupFunc() + + attrCfg := &vcConfig.Config{ + Camera: map[string]string{"name": "good_lidar"}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + IMUIntegrationEnabled: true, + } + + svc, err := testhelper.CreateSLAMService(t, attrCfg, logger) + test.That(t, err, test.ShouldBeNil) + + cs, ok := svc.(*viamcartographer.CartographerService) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, cs.SlamMode, test.ShouldEqual, cartofacade.UpdatingMode) + + _, componentReference, err := svc.GetPosition(context.Background()) + test.That(t, err, test.ShouldBeNil) + test.That(t, componentReference, test.ShouldEqual, "good_lidar") + + timestamp1, err := svc.GetLatestMapInfo(context.Background()) + test.That(t, err, test.ShouldBeNil) + + pcmFunc, err := svc.GetPointCloudMap(context.Background()) + test.That(t, err, test.ShouldBeNil) + + pcm, err := slam.HelperConcatenateChunksToFull(pcmFunc) + test.That(t, err, test.ShouldBeNil) + test.That(t, pcm, test.ShouldNotBeNil) + + isFunc, err := svc.GetInternalState(context.Background()) + test.That(t, err, test.ShouldBeNil) + + is, err := slam.HelperConcatenateChunksToFull(isFunc) + test.That(t, err, test.ShouldBeNil) + test.That(t, is, test.ShouldNotBeNil) + + timestamp2, err := svc.GetLatestMapInfo(context.Background()) + test.That(t, err, test.ShouldBeNil) + + test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue) + test.That(t, timestamp2.After(timestamp1), test.ShouldBeTrue) + + test.That(t, svc.Close(context.Background()), test.ShouldBeNil) + }) + + t.Run("Fails to create cartographer slam service with no sensor with feature flag enabled", func(t *testing.T) { + termFunc := testhelper.InitTestCL(t, logger) + defer termFunc() + + dataDirectory, fsCleanupFunc := testhelper.InitInternalState(t) + defer fsCleanupFunc() + + attrCfg := &vcConfig.Config{ + Camera: map[string]string{}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + IMUIntegrationEnabled: true, + } + + svc, err := testhelper.CreateSLAMService(t, attrCfg, logger) + test.That(t, err, test.ShouldBeError, errors.New("error validating \"path\": \"camera[name]\" is required")) + test.That(t, svc, test.ShouldBeNil) + }) +} + func TestClose(t *testing.T) { logger := golog.NewTestLogger(t) ctx := context.Background() - t.Run("is idempotent and makes all endpoints return closed errors", func(t *testing.T) { + t.Run("is idempotent and makes all endpoints return closed errors with feature flag enabled", func(t *testing.T) { termFunc := testhelper.InitTestCL(t, logger) defer termFunc() @@ -273,10 +448,11 @@ func TestClose(t *testing.T) { }() attrCfg := &vcConfig.Config{ - Sensors: []string{"replay_sensor"}, - ConfigParams: map[string]string{"mode": "2d"}, - DataDirectory: dataDirectory, - MapRateSec: &testMapRateSec, + Camera: map[string]string{"name": "replay_lidar"}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + MapRateSec: &testMapRateSec, + IMUIntegrationEnabled: true, } svc, err := testhelper.CreateSLAMService(t, attrCfg, logger) @@ -325,11 +501,11 @@ func TestDoCommand(t *testing.T) { test.That(t, err, test.ShouldBeNil) attrCfg := &vcConfig.Config{ - Sensors: []string{"good_lidar"}, - ConfigParams: map[string]string{"mode": "2d", "test_param": "viam"}, - DataDirectory: dataDirectory, - MapRateSec: &testMapRateSec, - DataRateMsec: testDataRateMsec, + Camera: map[string]string{"name": "good_lidar", "data_frequency_hz": testDataFreqHz}, + ConfigParams: map[string]string{"mode": "2d", "test_param": "viam"}, + DataDirectory: dataDirectory, + MapRateSec: &testMapRateSec, + IMUIntegrationEnabled: true, } svc, err := testhelper.CreateSLAMService(t, attrCfg, logger) test.That(t, err, test.ShouldBeNil) @@ -339,7 +515,7 @@ func TestDoCommand(t *testing.T) { test.That(t, err, test.ShouldEqual, viamgrpc.UnimplementedError) test.That(t, resp, test.ShouldBeNil) }) - t.Run("returns UnimplementedError when given no parmeters", func(t *testing.T) { + t.Run("returns UnimplementedError when given no parameters", func(t *testing.T) { cmd := map[string]interface{}{} resp, err := svc.DoCommand(context.Background(), cmd) test.That(t, err, test.ShouldEqual, viamgrpc.UnimplementedError)