diff --git a/cartofacade/capi.go b/cartofacade/capi.go index 7c59138f..bc802e43 100644 --- a/cartofacade/capi.go +++ b/cartofacade/capi.go @@ -87,6 +87,7 @@ const ( // CartoConfig contains config values from app type CartoConfig struct { Camera string + MovementSensor string MapRateSecond int DataDir string ComponentReference string @@ -323,6 +324,7 @@ 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) + vcc.movement_sensor = goStringToBstring(cfg.MovementSensor) lidarCfg, err := toLidarConfig(cfg.LidarConfig) if err != nil { diff --git a/cartofacade/capi_test.go b/cartofacade/capi_test.go index 70574b89..a9fe9117 100644 --- a/cartofacade/capi_test.go +++ b/cartofacade/capi_test.go @@ -54,8 +54,8 @@ func testAddSensorReading(t *testing.T, vc Carto, pcdPath string, timestamp time } func TestGetConfig(t *testing.T) { - t.Run("config properly converted between C and go", func(t *testing.T) { - cfg, dir, err := GetTestConfig("mysensor") + t.Run("config properly converted between C and go with no IMU specified", func(t *testing.T) { + cfg, dir, err := GetTestConfig("mysensor", "") defer os.RemoveAll(dir) test.That(t, err, test.ShouldBeNil) @@ -70,6 +70,26 @@ func TestGetConfig(t *testing.T) { test.That(t, vcc.lidar_config, test.ShouldEqual, TwoD) }) + + t.Run("config properly converted between C and go with an IMU specified", func(t *testing.T) { + cfg, dir, err := GetTestConfig("mylidar", "myIMU") + defer os.RemoveAll(dir) + test.That(t, err, test.ShouldBeNil) + + vcc, err := getConfig(cfg) + test.That(t, err, test.ShouldBeNil) + + camera := bstringToGoString(vcc.camera) + test.That(t, camera, test.ShouldResemble, "mylidar") + + movementSensor := bstringToGoString(vcc.movement_sensor) + test.That(t, movementSensor, test.ShouldResemble, "myIMU") + + dataDir := bstringToGoString(vcc.data_dir) + test.That(t, dataDir, test.ShouldResemble, dir) + + test.That(t, vcc.lidar_config, test.ShouldEqual, TwoD) + }) } func TestGetPositionResponse(t *testing.T) { @@ -114,7 +134,7 @@ func TestCGoAPI(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, pvcl, test.ShouldNotBeNil) - cfg, dir, err := GetTestConfig("mysensor") + cfg, dir, err := GetTestConfig("mysensor", "") defer os.RemoveAll(dir) test.That(t, err, test.ShouldBeNil) diff --git a/cartofacade/carto_facade_test.go b/cartofacade/carto_facade_test.go index 2ce4a2bf..60f6810d 100644 --- a/cartofacade/carto_facade_test.go +++ b/cartofacade/carto_facade_test.go @@ -28,7 +28,7 @@ func TestRequest(t *testing.T) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) activeBackgroundWorkers := sync.WaitGroup{} - config, dir, err := GetTestConfig("mysensor") + config, dir, err := GetTestConfig("mysensor", "") defer os.RemoveAll(dir) test.That(t, err, test.ShouldBeNil) @@ -54,7 +54,7 @@ func TestRequest(t *testing.T) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) activeBackgroundWorkers := sync.WaitGroup{} - config, dir, err := GetTestConfig("mysensor") + config, dir, err := GetTestConfig("mysensor", "") defer os.RemoveAll(dir) test.That(t, err, test.ShouldBeNil) @@ -80,7 +80,7 @@ func TestRequest(t *testing.T) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) activeBackgroundWorkers := sync.WaitGroup{} - config, dir, err := GetTestConfig("mysensor") + config, dir, err := GetTestConfig("mysensor", "") defer os.RemoveAll(dir) test.That(t, err, test.ShouldBeNil) @@ -108,7 +108,7 @@ func TestRequest(t *testing.T) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) activeBackgroundWorkers := sync.WaitGroup{} - config, dir, err := GetTestConfig("mysensor") + config, dir, err := GetTestConfig("mysensor", "") defer os.RemoveAll(dir) test.That(t, err, test.ShouldBeNil) @@ -144,7 +144,7 @@ func TestInitialize(t *testing.T) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) activeBackgroundWorkers := sync.WaitGroup{} - cfg, dir, err := GetTestConfig("mysensor") + cfg, dir, err := GetTestConfig("mysensor", "") algoCfg := GetTestAlgoConfig() test.That(t, err, test.ShouldBeNil) defer os.RemoveAll(dir) @@ -171,7 +171,7 @@ func TestStart(t *testing.T) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) activeBackgroundWorkers := sync.WaitGroup{} - cfg, dir, err := GetTestConfig("mysensor") + cfg, dir, err := GetTestConfig("mysensor", "") algoCfg := GetTestAlgoConfig() test.That(t, err, test.ShouldBeNil) defer os.RemoveAll(dir) @@ -222,7 +222,7 @@ func TestStop(t *testing.T) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) activeBackgroundWorkers := sync.WaitGroup{} - cfg, dir, err := GetTestConfig("mysensor") + cfg, dir, err := GetTestConfig("mysensor", "") algoCfg := GetTestAlgoConfig() test.That(t, err, test.ShouldBeNil) defer os.RemoveAll(dir) @@ -273,7 +273,7 @@ func TestTerminate(t *testing.T) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) activeBackgroundWorkers := sync.WaitGroup{} - cfg, dir, err := GetTestConfig("mysensor") + cfg, dir, err := GetTestConfig("mysensor", "") algoCfg := GetTestAlgoConfig() test.That(t, err, test.ShouldBeNil) defer os.RemoveAll(dir) @@ -324,7 +324,7 @@ func TestAddSensorReading(t *testing.T) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) activeBackgroundWorkers := sync.WaitGroup{} - cfg, dir, err := GetTestConfig("mysensor") + cfg, dir, err := GetTestConfig("mysensor", "") algoCfg := GetTestAlgoConfig() test.That(t, err, test.ShouldBeNil) defer os.RemoveAll(dir) @@ -386,7 +386,7 @@ func TestGetPosition(t *testing.T) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) activeBackgroundWorkers := sync.WaitGroup{} - cfg, dir, err := GetTestConfig("mysensor") + cfg, dir, err := GetTestConfig("mysensor", "") algoCfg := GetTestAlgoConfig() test.That(t, err, test.ShouldBeNil) defer os.RemoveAll(dir) @@ -441,7 +441,7 @@ func TestGetInternalState(t *testing.T) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) activeBackgroundWorkers := sync.WaitGroup{} - cfg, dir, err := GetTestConfig("mysensor") + cfg, dir, err := GetTestConfig("mysensor", "") algoCfg := GetTestAlgoConfig() test.That(t, err, test.ShouldBeNil) defer os.RemoveAll(dir) @@ -494,7 +494,7 @@ func TestGetPointCloudMap(t *testing.T) { cancelCtx, cancelFunc := context.WithCancel(context.Background()) activeBackgroundWorkers := sync.WaitGroup{} - cfg, dir, err := GetTestConfig("mysensor") + cfg, dir, err := GetTestConfig("mysensor", "") algoCfg := GetTestAlgoConfig() test.That(t, err, test.ShouldBeNil) defer os.RemoveAll(dir) diff --git a/cartofacade/testhelpers.go b/cartofacade/testhelpers.go index ff58106b..419341b8 100644 --- a/cartofacade/testhelpers.go +++ b/cartofacade/testhelpers.go @@ -5,7 +5,7 @@ import ( ) // GetTestConfig gets a sample config for testing purposes. -func GetTestConfig(cameraName string) (CartoConfig, string, error) { +func GetTestConfig(cameraName, movementSensorName string) (CartoConfig, string, error) { dir, err := os.MkdirTemp("", "slam-test") if err != nil { return CartoConfig{}, "", err @@ -13,6 +13,7 @@ func GetTestConfig(cameraName string) (CartoConfig, string, error) { return CartoConfig{ Camera: cameraName, + MovementSensor: movementSensorName, MapRateSecond: 5, DataDir: dir, ComponentReference: "component", diff --git a/config/config.go b/config/config.go index e90dcdd9..721a9093 100644 --- a/config/config.go +++ b/config/config.go @@ -17,6 +17,7 @@ func newError(configError string) error { // Config describes how to configure the SLAM service. type Config struct { Camera map[string]string `json:"camera"` + MovementSensor map[string]string `json:"movement_sensor"` ConfigParams map[string]string `json:"config_params"` DataDirectory string `json:"data_dir"` MapRateSec *int `json:"map_rate_sec"` @@ -51,7 +52,7 @@ func (config *Config) Validate(path string) ([]string, error) { } } else { if config.Sensors == nil || len(config.Sensors) < 1 { - return nil, utils.NewConfigValidationError(path, errors.New("\"sensors\" must not be empty")) + return nil, utils.NewConfigValidationError(path, errSensorsMustNotBeEmpty) } cameraName = config.Sensors[0] @@ -79,9 +80,11 @@ func (config *Config) Validate(path string) ([]string, error) { // GetOptionalParameters sets any unset optional config parameters to the values passed to this function, // and returns them. -func GetOptionalParameters(config *Config, defaultLidarDataRateMsec, defaultMapRateSec int, logger golog.Logger, -) (int, int, error) { +func GetOptionalParameters(config *Config, defaultLidarDataRateMsec, defaultIMUDataRateMsec, defaultMapRateSec int, logger golog.Logger, +) (int, string, int, int, error) { lidarDataRateMsec := defaultLidarDataRateMsec + imuName := "" + imuDataRateMsec := defaultIMUDataRateMsec // feature flag for new config if config.IMUIntegrationEnabled { @@ -91,10 +94,24 @@ func GetOptionalParameters(config *Config, defaultLidarDataRateMsec, defaultMapR } else { lidarDataFreqHz, err := strconv.Atoi(strCameraDataFreqHz) if err != nil { - return 0, 0, newError("camera[data_frequency_hz] must only contain digits") + return 0, "", 0, 0, newError("camera[data_frequency_hz] must only contain digits") } lidarDataRateMsec = 1000 / lidarDataFreqHz } + exists := false + imuName, exists = config.MovementSensor["name"] + if exists { + strMovementSensorDataFreqHz, ok := config.MovementSensor["data_frequency_hz"] + if !ok { + logger.Debugf("config did not provide movement_sensor[data_frequency_hz], setting to default value of %d", 1000/defaultIMUDataRateMsec) + } else { + imuDataFreqHz, err := strconv.Atoi(strMovementSensorDataFreqHz) + if err != nil { + return 0, "", 0, 0, newError("movement_sensor[data_frequency_hz] must only contain digits") + } + imuDataRateMsec = 1000 / imuDataFreqHz + } + } } else { lidarDataRateMsec = config.DataRateMsec if config.DataRateMsec == 0 { @@ -111,5 +128,5 @@ func GetOptionalParameters(config *Config, defaultLidarDataRateMsec, defaultMapR mapRateSec = *config.MapRateSec } - return lidarDataRateMsec, mapRateSec, nil + return lidarDataRateMsec, imuName, imuDataRateMsec, mapRateSec, nil } diff --git a/config/config_test.go b/config/config_test.go index 3e92e75f..d1ef2dc3 100644 --- a/config/config_test.go +++ b/config/config_test.go @@ -124,9 +124,10 @@ func TestGetOptionalParameters(t *testing.T) { cfgService.Attributes["sensors"] = []string{"a"} cfg, err := newConfig(cfgService) test.That(t, err, test.ShouldBeNil) - lidarDataRateMsec, mapRateSec, err := GetOptionalParameters( + lidarDataRateMsec, _, _, mapRateSec, err := GetOptionalParameters( cfg, 1000, + 1000, 1002, logger) test.That(t, err, test.ShouldBeNil) @@ -145,9 +146,10 @@ func TestGetOptionalParameters(t *testing.T) { cfg.MapRateSec = &two cfg.DataRateMsec = 50 test.That(t, err, test.ShouldBeNil) - lidarDataRateMsec, mapRateSec, err := GetOptionalParameters( + lidarDataRateMsec, _, _, mapRateSec, err := GetOptionalParameters( cfg, 1000, + 1000, 1002, logger) test.That(t, err, test.ShouldBeNil) @@ -263,37 +265,47 @@ func TestGetOptionalParametersFeatureFlag(t *testing.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"} + cfgService.Attributes["movement_sensor"] = map[string]string{"name": "b"} cfg, err := newConfig(cfgService) test.That(t, err, test.ShouldBeNil) - lidarDataRateMsec, mapRateSec, err := GetOptionalParameters( + lidarDataRateMsec, imuName, imuDataRateMsec, mapRateSec, err := GetOptionalParameters( cfg, 1000, + 1000, 1002, logger) test.That(t, err, test.ShouldBeNil) test.That(t, mapRateSec, test.ShouldEqual, 1002) test.That(t, lidarDataRateMsec, test.ShouldEqual, 1000) + test.That(t, imuName, test.ShouldEqual, "b") + test.That(t, imuDataRateMsec, 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", + "data_frequency_hz": "5", + } + cfgService.Attributes["movement_sensor"] = map[string]string{ + "name": "b", + "data_frequency_hz": "20", } 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( + lidarDataRateMsec, imuName, imuDataRateMsec, mapRateSec, err := GetOptionalParameters( cfg, 1000, + 1000, 1002, logger) test.That(t, err, test.ShouldBeNil) test.That(t, mapRateSec, test.ShouldEqual, 2) - test.That(t, lidarDataRateMsec, test.ShouldEqual, 50) + test.That(t, lidarDataRateMsec, test.ShouldEqual, 200) + test.That(t, imuName, test.ShouldEqual, "b") + test.That(t, imuDataRateMsec, test.ShouldEqual, 50) }) t.Run("Unit test return error if lidar data frequency is invalid", func(t *testing.T) { @@ -304,13 +316,35 @@ func TestGetOptionalParametersFeatureFlag(t *testing.T) { } cfg, err := newConfigWithoutValidate(cfgService) test.That(t, err, test.ShouldBeNil) - _, _, err = GetOptionalParameters( + _, _, _, _, err = GetOptionalParameters( cfg, 1000, + 1000, 1002, logger) test.That(t, err, test.ShouldBeError, newError("camera[data_frequency_hz] must only contain digits")) }) + + t.Run("Unit test return error if imu data frequency is invalid", func(t *testing.T) { + cfgService := makeCfgService(true) + cfgService.Attributes["camera"] = map[string]string{ + "name": "a", + "data_frequency_hz": "1", + } + cfgService.Attributes["movement_sensor"] = map[string]string{ + "name": "b", + "data_frequency_hz": "c", + } + cfg, err := newConfigWithoutValidate(cfgService) + test.That(t, err, test.ShouldBeNil) + _, _, _, _, err = GetOptionalParameters( + cfg, + 1000, + 1000, + 1002, + logger) + test.That(t, err, test.ShouldBeError, newError("movement_sensor[data_frequency_hz] must only contain digits")) + }) } func newConfigWithoutValidate(conf resource.Config) (*Config, error) { diff --git a/viam-cartographer/src/carto_facade/carto_facade.cc b/viam-cartographer/src/carto_facade/carto_facade.cc index d845a5ae..6ee6aa49 100644 --- a/viam-cartographer/src/carto_facade/carto_facade.cc +++ b/viam-cartographer/src/carto_facade/carto_facade.cc @@ -112,6 +112,7 @@ void validate_lidar_config(viam_carto_LIDAR_CONFIG lidar_config) { config from_viam_carto_config(viam_carto_config vcc) { struct config c; c.camera = to_std_string(vcc.camera); + c.movement_sensor = to_std_string(vcc.movement_sensor); 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; diff --git a/viam-cartographer/src/carto_facade/carto_facade.h b/viam-cartographer/src/carto_facade/carto_facade.h index e40ed16a..fdf0ee4d 100644 --- a/viam-cartographer/src/carto_facade/carto_facade.h +++ b/viam-cartographer/src/carto_facade/carto_facade.h @@ -130,6 +130,7 @@ typedef struct viam_carto_algo_config { typedef struct viam_carto_config { bstring camera; + bstring movement_sensor; int map_rate_sec; bstring data_dir; viam_carto_LIDAR_CONFIG lidar_config; @@ -291,6 +292,7 @@ static const double resolutionMeters = 0.05; typedef struct config { std::string camera; + std::string movement_sensor; 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 cc7a04a7..b93d2a63 100644 --- a/viam-cartographer/src/carto_facade/carto_facade_test.cc +++ b/viam-cartographer/src/carto_facade/carto_facade_test.cc @@ -31,18 +31,21 @@ 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::string camera) { + std::string camera, + std::string movement_sensor) { 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()); vcc.camera = bfromcstr(camera.c_str()); + vcc.movement_sensor = bfromcstr(movement_sensor.c_str()); return vcc; } void viam_carto_config_teardown(viam_carto_config vcc) { BOOST_TEST(bdestroy(vcc.data_dir) == BSTR_OK); BOOST_TEST(bdestroy(vcc.camera) == BSTR_OK); + BOOST_TEST(bdestroy(vcc.movement_sensor) == BSTR_OK); } viam_carto_sensor_reading new_test_sensor_reading( std::string sensor, std::string pcd_path, @@ -114,28 +117,31 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_validate) { struct viam_carto_algo_config ac = viam_carto_algo_config_setup(); std::string camera = "lidar"; - struct viam_carto_config vcc_empty_data_dir = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, "", camera); + std::string movement_sensor = "imu"; + struct viam_carto_config vcc_empty_data_dir = viam_carto_config_setup( + 1, VIAM_CARTO_THREE_D, "", camera, movement_sensor); BOOST_TEST(viam_carto_init(&vc, lib, vcc_empty_data_dir, ac) == VIAM_CARTO_DATA_DIR_NOT_PROVIDED); std::string camera2 = ""; + std::string movement_sensor2 = ""; struct viam_carto_config vcc_empty_component_ref = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera2); + 1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera2, movement_sensor2); 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(), camera); + -1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera, movement_sensor); 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(), camera); + struct viam_carto_config vcc_invalid_lidar_config = + viam_carto_config_setup(1, static_cast(-1), + tmp_dir.string(), camera, movement_sensor); BOOST_TEST(viam_carto_init(&vc, lib, vcc_invalid_lidar_config, ac) == VIAM_CARTO_LIDAR_CONFIG_INVALID); @@ -143,8 +149,9 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_validate) { fs::path deprecated_path = tmp_dir / fs::path(bfs::unique_path().string()); 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(), camera); + struct viam_carto_config vcc_deprecated_path = + viam_carto_config_setup(1, VIAM_CARTO_THREE_D, deprecated_path.string(), + camera, movement_sensor); BOOST_TEST(viam_carto_init(&vc, lib, vcc_deprecated_path, ac) == VIAM_CARTO_DATA_DIR_INVALID_DEPRECATED_STRUCTURE); @@ -152,12 +159,12 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_validate) { 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(), camera); + 1, VIAM_CARTO_THREE_D, invalid_path.string(), camera, movement_sensor); 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(), camera); + 1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera, movement_sensor); BOOST_TEST(viam_carto_init(nullptr, lib, vcc, ac) == VIAM_CARTO_VC_INVALID); BOOST_TEST(viam_carto_init(nullptr, nullptr, vcc, ac) == @@ -197,6 +204,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode) { BOOST_TEST(viam_carto_lib_init(&lib, 0, 1) == VIAM_CARTO_SUCCESS); std::string camera = "lidar"; + std::string movement_sensor = "imu"; fs::path tmp_dir = fs::temp_directory_path() / fs::path(bfs::unique_path().string()); struct viam_carto_algo_config ac = viam_carto_algo_config_setup(); @@ -206,8 +214,9 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode) { // mapping 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(), camera); + struct viam_carto_config vcc_mapping = + viam_carto_config_setup(1, VIAM_CARTO_THREE_D, mapping_dir.string(), + camera, movement_sensor); BOOST_TEST(viam_carto_init(&vc1, lib, vcc_mapping, ac) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc1->slam_mode == VIAM_CARTO_SLAM_MODE_MAPPING); @@ -267,7 +276,8 @@ 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(), camera); + 1, VIAM_CARTO_THREE_D, updating_dir.string(), camera, + movement_sensor); BOOST_TEST(viam_carto_init(&vc2, lib, vcc_updating, ac) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc2->slam_mode == VIAM_CARTO_SLAM_MODE_UPDATING); @@ -307,7 +317,8 @@ 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(), camera); + 1, VIAM_CARTO_THREE_D, updating_dir.string(), camera, + movement_sensor); BOOST_TEST(viam_carto_init(&vc3, lib, vcc_updating, ac_optimize_on_start) == VIAM_CARTO_SUCCESS); @@ -323,7 +334,8 @@ 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(), camera); + 0, VIAM_CARTO_THREE_D, updating_dir.string(), camera, + movement_sensor); BOOST_TEST(viam_carto_init(&vc4, lib, vcc_localizing, ac) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc4->slam_mode == VIAM_CARTO_SLAM_MODE_LOCALIZING); @@ -356,7 +368,8 @@ 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(), camera); + 0, VIAM_CARTO_THREE_D, updating_dir.string(), camera, + movement_sensor); 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); @@ -373,7 +386,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(), camera); + 0, VIAM_CARTO_THREE_D, empty_dir.string(), camera, movement_sensor); BOOST_TEST(viam_carto_init(&vc6, lib, vcc_invalid, ac) == VIAM_CARTO_SLAM_MODE_INVALID); viam_carto_config_teardown(vcc_invalid); @@ -392,10 +405,11 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_terminate) { viam_carto *vc; std::string camera = "lidar"; + std::string movement_sensor = "imu"; 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(), camera); + 1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera, movement_sensor); 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); @@ -420,6 +434,7 @@ 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.camera) == camera); + BOOST_TEST((cf->config.movement_sensor) == movement_sensor); 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) == "lidar"); @@ -441,12 +456,13 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo) { // Setup viam_carto *vc; std::string camera = "lidar"; + std::string movement_sensor = "imu"; 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(), - camera); + camera, movement_sensor); struct viam_carto_algo_config ac = viam_carto_algo_config_setup(); BOOST_TEST(viam_carto_init(&vc, lib, vcc, ac) == VIAM_CARTO_SUCCESS); @@ -878,12 +894,13 @@ BOOST_AUTO_TEST_CASE(CartoFacade_config) { BOOST_TEST(viam_carto_lib_init(&lib, 0, 1) == VIAM_CARTO_SUCCESS); std::string camera = "lidar"; + std::string movement_sensor = "imu"; 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(), - camera); + camera, movement_sensor); struct config c = viam::carto_facade::from_viam_carto_config(vcc); @@ -892,6 +909,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_config) { BOOST_TEST(c.lidar_config == VIAM_CARTO_THREE_D); BOOST_TEST(c.map_rate_sec.count() == 1); BOOST_TEST(c.camera == "lidar"); + BOOST_TEST(c.movement_sensor == "imu"); viam_carto_config_teardown(vcc); BOOST_TEST(bdestroy(c.component_reference) == BSTR_OK); @@ -913,12 +931,13 @@ BOOST_AUTO_TEST_CASE(CartoFacade_start_stop) { // Setup viam_carto *vc; std::string camera = "lidar"; + std::string movement_sensor = "imu"; 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(), - camera); + camera, movement_sensor); 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 e16f9ec8..fde462b4 100644 --- a/viam_cartographer.go +++ b/viam_cartographer.go @@ -38,6 +38,7 @@ const ( // DefaultExecutableName is what this program expects to call to start the cartographer grpc server. DefaultExecutableName = "carto_grpc_server" defaultLidarDataRateMsec = 200 + defaultIMUDataRateMsec = 50 defaultMapRateSec = 60 defaultDialMaxTimeoutSec = 30 defaultSensorValidationMaxTimeoutSec = 30 @@ -160,9 +161,10 @@ func New( c.Model.Name, svcConfig.ConfigParams["mode"]) } - lidarDataRateMsec, mapRateSec, err := vcConfig.GetOptionalParameters( + lidarDataRateMsec, imuName, imuDataRateMsec, mapRateSec, err := vcConfig.GetOptionalParameters( svcConfig, defaultLidarDataRateMsec, + defaultIMUDataRateMsec, defaultMapRateSec, logger, ) @@ -203,6 +205,8 @@ func New( lidar: lidar, lidarDataRateMsec: lidarDataRateMsec, timedLidar: timedLidar, + imuName: imuName, + imuDataRateMsec: imuDataRateMsec, subAlgo: subAlgo, configParams: svcConfig.ConfigParams, dataDirectory: svcConfig.DataDirectory, @@ -344,6 +348,7 @@ func initCartoFacade(ctx context.Context, cartoSvc *CartographerService) error { cartoCfg := cartofacade.CartoConfig{ Camera: cartoSvc.lidarName, + MovementSensor: cartoSvc.imuName, MapRateSecond: cartoSvc.mapRateSec, DataDir: cartoSvc.dataDirectory, ComponentReference: cartoSvc.lidarName, @@ -402,6 +407,8 @@ type CartographerService struct { lidarDataRateMsec int lidar s.Lidar timedLidar s.TimedSensor + imuName string + imuDataRateMsec int subAlgo SubAlgo configParams map[string]string