Skip to content

Commit

Permalink
[RSDK-4137] Config changes for IMU Support (#235)
Browse files Browse the repository at this point in the history
  • Loading branch information
@pstrutz
pstrutz authored Aug 2, 2023
1 parent f6b8d8b commit b2c3aba
Show file tree
Hide file tree
Showing 10 changed files with 155 additions and 52 deletions.
2 changes: 2 additions & 0 deletions cartofacade/capi.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,7 @@ const (
// CartoConfig contains config values from app
type CartoConfig struct {
Camera string
MovementSensor string
MapRateSecond int
DataDir string
ComponentReference string
Expand Down Expand Up @@ -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 {
Expand Down
26 changes: 23 additions & 3 deletions cartofacade/capi_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand All @@ -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) {
Expand Down Expand Up @@ -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)
Expand Down
24 changes: 12 additions & 12 deletions cartofacade/carto_facade_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand All @@ -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)

Expand All @@ -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)

Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -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)
Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down
3 changes: 2 additions & 1 deletion cartofacade/testhelpers.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,14 +5,15 @@ 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
}

return CartoConfig{
Camera: cameraName,
MovementSensor: movementSensorName,
MapRateSecond: 5,
DataDir: dir,
ComponentReference: "component",
Expand Down
27 changes: 22 additions & 5 deletions config/config.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"`
Expand Down Expand Up @@ -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]

Expand Down Expand Up @@ -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 {
Expand All @@ -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 {
Expand All @@ -111,5 +128,5 @@ func GetOptionalParameters(config *Config, defaultLidarDataRateMsec, defaultMapR
mapRateSec = *config.MapRateSec
}

return lidarDataRateMsec, mapRateSec, nil
return lidarDataRateMsec, imuName, imuDataRateMsec, mapRateSec, nil
}
50 changes: 42 additions & 8 deletions config/config_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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)
Expand Down Expand Up @@ -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) {
Expand All @@ -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) {
Expand Down
1 change: 1 addition & 0 deletions viam-cartographer/src/carto_facade/carto_facade.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 2 additions & 0 deletions viam-cartographer/src/carto_facade/carto_facade.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
Loading

0 comments on commit b2c3aba

Please sign in to comment.