Skip to content

Commit

Permalink
[RSDK-4106] Restructure lidar name and data rate to use map (#232)
Browse files Browse the repository at this point in the history 
Co-authored-by: Nick Sanford <nick.sanford@viam.com>
  • Loading branch information
@pstrutz @nicksanford
pstrutz and nicksanford authored Aug 2, 2023
1 parent 84c08db commit f6b8d8b
Show file tree
Hide file tree
Showing 18 changed files with 715 additions and 461 deletions.
57 changes: 4 additions & 53 deletions cartofacade/capi.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down Expand Up @@ -99,7 +86,7 @@ const (

// CartoConfig contains config values from app
type CartoConfig struct {
Sensors []string
Camera string
MapRateSecond int
DataDir string
ComponentReference string
Expand Down Expand Up @@ -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
}

Expand Down Expand Up @@ -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{}
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand All @@ -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:
Expand Down
8 changes: 2 additions & 6 deletions cartofacade/capi_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
})
}
Expand Down
6 changes: 3 additions & 3 deletions cartofacade/testhelpers.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand All @@ -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,
}
}
Expand Down
84 changes: 63 additions & 21 deletions config/config.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,8 @@
package config

import (
"strconv"

"github.com/edaniels/golog"
"github.com/pkg/errors"
"go.viam.com/utils"
Expand All @@ -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"] == "" {
Expand All @@ -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
Expand All @@ -69,5 +111,5 @@ func GetOptionalParameters(config *Config,
mapRateSec = *config.MapRateSec
}

return dataRateMsec, mapRateSec
return lidarDataRateMsec, mapRateSec, nil
}
Loading

0 comments on commit f6b8d8b

Please sign in to comment.