diff --git a/Makefile b/Makefile index cab20f79..7744714f 100644 --- a/Makefile +++ b/Makefile @@ -95,9 +95,14 @@ else $(error "Unsupported system. Only apt and brew currently supported.") endif -build: ensure-submodule-initialized grpc/buf build-module +build: bin/cartographer-module + +viam-cartographer/build/carto_grpc_server: ensure-submodule-initialized grpc/buf cd viam-cartographer && cmake -Bbuild -G Ninja ${EXTRA_CMAKE_FLAGS} && cmake --build build +bin/cartographer-module: viam-cartographer/build/carto_grpc_server + mkdir -p bin && go build $(GO_BUILD_LDFLAGS) -o bin/cartographer-module module/main.go + # Ideally build-asan would be added to build-debug, but can't yet # as these options they fail on arm64 linux. This is b/c that # platform currently uses gcc as opposed to clang & gcc doesn't @@ -111,9 +116,6 @@ build-asan: build-debug build-debug: EXTRA_CMAKE_FLAGS += -DCMAKE_BUILD_TYPE=Debug -DFORCE_DEBUG_BUILD=True build-debug: build -build-module: - mkdir -p bin && go build $(GO_BUILD_LDFLAGS) -o bin/cartographer-module module/main.go - test-cpp: viam-cartographer/build/unit_tests -p -l all diff --git a/cartofacade/capi.go b/cartofacade/capi.go index 82c081c6..e74742a2 100644 --- a/cartofacade/capi.go +++ b/cartofacade/capi.go @@ -77,8 +77,10 @@ type GetPosition struct { type LidarConfig int64 const ( - twoD LidarConfig = iota - threeD + // TwoD LidarConfig denotes a 2d lidar + TwoD LidarConfig = iota + // ThreeD LidarConfig denotes a 3d lidar + ThreeD ) // CartoConfig contains config values from app @@ -92,19 +94,19 @@ type CartoConfig struct { // CartoAlgoConfig contains config values from app type CartoAlgoConfig struct { - optimizeOnStart bool - optimizeEveryNNodes int - numRangeData int - missingDataRayLength float32 - maxRange float32 - minRange float32 - maxSubmapsToKeep int - freshSubmapsCount int - minCoveredArea float64 - minAddedSubmapsCount int - occupiedSpaceWeight float64 - translationWeight float64 - rotationWeight float64 + OptimizeOnStart bool + OptimizeEveryNNodes int + NumRangeData int + MissingDataRayLength float32 + MaxRange float32 + MinRange float32 + MaxSubmapsToKeep int + FreshSubmapsCount int + MinCoveredArea float64 + MinAddedSubmapsCount int + OccupiedSpaceWeight float64 + TranslationWeight float64 + RotationWeight float64 } // NewLib calls viam_carto_lib_init and returns a pointer to a viam carto lib object. @@ -311,9 +313,9 @@ func goStringToBstring(goStr string) C.bstring { func toLidarConfig(lidarConfig LidarConfig) (C.viam_carto_LIDAR_CONFIG, error) { switch lidarConfig { - case twoD: + case TwoD: return C.VIAM_CARTO_TWO_D, nil - case threeD: + case ThreeD: return C.VIAM_CARTO_THREE_D, nil default: return 0, errors.New("invalid lidar config value") @@ -349,19 +351,19 @@ func getConfig(cfg CartoConfig) (C.viam_carto_config, error) { func toAlgoConfig(acfg CartoAlgoConfig) C.viam_carto_algo_config { vcac := C.viam_carto_algo_config{} - vcac.optimize_on_start = C.bool(acfg.optimizeOnStart) - vcac.optimize_every_n_nodes = C.int(acfg.optimizeEveryNNodes) - vcac.num_range_data = C.int(acfg.numRangeData) - vcac.missing_data_ray_length = C.float(acfg.missingDataRayLength) - vcac.max_range = C.float(acfg.maxRange) - vcac.min_range = C.float(acfg.minRange) - vcac.max_submaps_to_keep = C.int(acfg.maxSubmapsToKeep) - vcac.fresh_submaps_count = C.int(acfg.freshSubmapsCount) - vcac.min_covered_area = C.double(acfg.minCoveredArea) - vcac.min_added_submaps_count = C.int(acfg.minAddedSubmapsCount) - vcac.occupied_space_weight = C.double(acfg.occupiedSpaceWeight) - vcac.translation_weight = C.double(acfg.translationWeight) - vcac.rotation_weight = C.double(acfg.rotationWeight) + vcac.optimize_on_start = C.bool(acfg.OptimizeOnStart) + vcac.optimize_every_n_nodes = C.int(acfg.OptimizeEveryNNodes) + vcac.num_range_data = C.int(acfg.NumRangeData) + vcac.missing_data_ray_length = C.float(acfg.MissingDataRayLength) + vcac.max_range = C.float(acfg.MaxRange) + vcac.min_range = C.float(acfg.MinRange) + vcac.max_submaps_to_keep = C.int(acfg.MaxSubmapsToKeep) + vcac.fresh_submaps_count = C.int(acfg.FreshSubmapsCount) + vcac.min_covered_area = C.double(acfg.MinCoveredArea) + vcac.min_added_submaps_count = C.int(acfg.MinAddedSubmapsCount) + vcac.occupied_space_weight = C.double(acfg.OccupiedSpaceWeight) + vcac.translation_weight = C.double(acfg.TranslationWeight) + vcac.rotation_weight = C.double(acfg.RotationWeight) return vcac } diff --git a/cartofacade/capi_test.go b/cartofacade/capi_test.go index b0a5dad5..4ad9a6b6 100644 --- a/cartofacade/capi_test.go +++ b/cartofacade/capi_test.go @@ -72,7 +72,7 @@ func TestGetConfig(t *testing.T) { freeBstringArray(vcc.sensors, vcc.sensors_len) - test.That(t, vcc.lidar_config, test.ShouldEqual, twoD) + test.That(t, vcc.lidar_config, test.ShouldEqual, TwoD) }) } diff --git a/cartofacade/testhelpers.go b/cartofacade/testhelpers.go index 9ded50cc..366d906c 100644 --- a/cartofacade/testhelpers.go +++ b/cartofacade/testhelpers.go @@ -16,7 +16,7 @@ func GetTestConfig(sensor string) (CartoConfig, string, error) { MapRateSecond: 5, DataDir: dir, ComponentReference: "component", - LidarConfig: twoD, + LidarConfig: TwoD, }, dir, nil } @@ -24,25 +24,25 @@ func GetTestConfig(sensor string) (CartoConfig, string, error) { func GetBadTestConfig() CartoConfig { return CartoConfig{ Sensors: []string{"rplidar", "imu"}, - LidarConfig: twoD, + LidarConfig: TwoD, } } // GetTestAlgoConfig gets a sample algo config for testing purposes. func GetTestAlgoConfig() CartoAlgoConfig { return CartoAlgoConfig{ - optimizeOnStart: false, - optimizeEveryNNodes: 3, - numRangeData: 100, - missingDataRayLength: 25.0, - maxRange: 25.0, - minRange: 0.2, - maxSubmapsToKeep: 3, - freshSubmapsCount: 3, - minCoveredArea: 1.0, - minAddedSubmapsCount: 1, - occupiedSpaceWeight: 20.0, - translationWeight: 10.0, - rotationWeight: 1.0, + OptimizeOnStart: false, + OptimizeEveryNNodes: 3, + NumRangeData: 100, + MissingDataRayLength: 25.0, + MaxRange: 25.0, + MinRange: 0.2, + MaxSubmapsToKeep: 3, + FreshSubmapsCount: 3, + MinCoveredArea: 1.0, + MinAddedSubmapsCount: 1, + OccupiedSpaceWeight: 20.0, + TranslationWeight: 10.0, + RotationWeight: 1.0, } } diff --git a/integration_test.go b/integration_test.go index e4c863b5..9854778a 100644 --- a/integration_test.go +++ b/integration_test.go @@ -26,7 +26,8 @@ import ( viamcartographer "github.com/viamrobotics/viam-cartographer" vcConfig "github.com/viamrobotics/viam-cartographer/config" "github.com/viamrobotics/viam-cartographer/dataprocess" - "github.com/viamrobotics/viam-cartographer/internal/testhelper" + internaltesthelper "github.com/viamrobotics/viam-cartographer/internal/testhelper" + "github.com/viamrobotics/viam-cartographer/testhelper" ) const ( @@ -99,7 +100,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su t.Skip() } - dataDir, err := testhelper.CreateTempFolderArchitecture(logger) + dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger) test.That(t, err, test.ShouldBeNil) prevNumFiles := 0 @@ -126,14 +127,14 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su // Release point cloud for service validation testhelper.IntegrationLidarReleasePointCloudChan <- 1 // Create slam service using a real cartographer binary - svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, true, viamcartographer.DefaultExecutableName) + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, true, viamcartographer.DefaultExecutableName) test.That(t, err, test.ShouldBeNil) // Release point cloud, since cartographer looks for the second most recent point cloud testhelper.IntegrationLidarReleasePointCloudChan <- 1 // Make sure we initialize in mapping mode - logReader := svc.(testhelper.Service).GetSLAMProcessBufferedLogReader() + logReader := svc.(internaltesthelper.Service).GetSLAMProcessBufferedLogReader() for { line, err := logReader.ReadString('\n') test.That(t, err, test.ShouldBeNil) @@ -153,7 +154,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su line, err := logReader.ReadString('\n') test.That(t, err, test.ShouldBeNil) if strings.Contains(line, "Passed sensor data to SLAM") { - prevNumFiles = testhelper.CheckDeleteProcessedData( + prevNumFiles = internaltesthelper.CheckDeleteProcessedData( t, subAlgo, dataDir, @@ -206,11 +207,11 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su } // Create slam service using a real cartographer binary - svc, err = testhelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName) + svc, err = internaltesthelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName) test.That(t, err, test.ShouldBeNil) // Make sure we initialize in mapping mode - logReader = svc.(testhelper.Service).GetSLAMProcessBufferedLogReader() + logReader = svc.(internaltesthelper.Service).GetSLAMProcessBufferedLogReader() for { line, err := logReader.ReadString('\n') test.That(t, err, test.ShouldBeNil) @@ -226,7 +227,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su line, err := logReader.ReadString('\n') test.That(t, err, test.ShouldBeNil) if strings.Contains(line, "Passed sensor data to SLAM") { - prevNumFiles = testhelper.CheckDeleteProcessedData( + prevNumFiles = internaltesthelper.CheckDeleteProcessedData( t, subAlgo, dataDir, @@ -252,7 +253,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su time.Sleep(time.Millisecond * cartoSleepMsec) // Remove existing pointclouds, but leave maps and config (so we keep the lua files). - test.That(t, testhelper.ResetFolder(dataDir+"/data"), test.ShouldBeNil) + test.That(t, internaltesthelper.ResetFolder(dataDir+"/data"), test.ShouldBeNil) prevNumFiles = 0 // Count the initial number of maps in the map directory (should equal 1) @@ -280,11 +281,11 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su // Release point cloud for service validation testhelper.IntegrationLidarReleasePointCloudChan <- 1 // Create slam service using a real cartographer binary - svc, err = testhelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName) + svc, err = internaltesthelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName) test.That(t, err, test.ShouldBeNil) // Make sure we initialize in localization mode - logReader = svc.(testhelper.Service).GetSLAMProcessBufferedLogReader() + logReader = svc.(internaltesthelper.Service).GetSLAMProcessBufferedLogReader() for { line, err := logReader.ReadString('\n') test.That(t, err, test.ShouldBeNil) @@ -304,7 +305,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su line, err := logReader.ReadString('\n') test.That(t, err, test.ShouldBeNil) if strings.Contains(line, "Passed sensor data to SLAM") { - prevNumFiles = testhelper.CheckDeleteProcessedData( + prevNumFiles = internaltesthelper.CheckDeleteProcessedData( t, subAlgo, dataDir, @@ -320,7 +321,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su testCartographerMap(t, svc, true) // Remove maps so that testing is done on the map generated by the internal map - test.That(t, testhelper.ResetFolder(dataDir+"/map"), test.ShouldBeNil) + test.That(t, internaltesthelper.ResetFolder(dataDir+"/map"), test.ShouldBeNil) testCartographerInternalState(t, svc, dataDir) @@ -334,7 +335,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su time.Sleep(time.Millisecond * cartoSleepMsec) // Remove existing pointclouds, but leave maps and config (so we keep the lua files). - test.That(t, testhelper.ResetFolder(dataDir+"/data"), test.ShouldBeNil) + test.That(t, internaltesthelper.ResetFolder(dataDir+"/data"), test.ShouldBeNil) prevNumFiles = 0 // Test online mode using the map generated in the offline test @@ -356,11 +357,11 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su // Release point cloud for service validation testhelper.IntegrationLidarReleasePointCloudChan <- 1 // Create slam service using a real cartographer binary - svc, err = testhelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName) + svc, err = internaltesthelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName) test.That(t, err, test.ShouldBeNil) // Make sure we initialize in updating mode - logReader = svc.(testhelper.Service).GetSLAMProcessBufferedLogReader() + logReader = svc.(internaltesthelper.Service).GetSLAMProcessBufferedLogReader() for { line, err := logReader.ReadString('\n') test.That(t, err, test.ShouldBeNil) @@ -380,7 +381,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su line, err := logReader.ReadString('\n') test.That(t, err, test.ShouldBeNil) if strings.Contains(line, "Passed sensor data to SLAM") { - prevNumFiles = testhelper.CheckDeleteProcessedData( + prevNumFiles = internaltesthelper.CheckDeleteProcessedData( t, subAlgo, dataDir, @@ -404,7 +405,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su testCartographerDir(t, dataDir, 2) // Clear out directory - testhelper.ClearDirectory(t, dataDir) + internaltesthelper.ClearDirectory(t, dataDir) } // Checks the current slam directory to see if the number of files matches the expected amount. diff --git a/internal/dim-2d/dim-2d_test.go b/internal/dim-2d/dim-2d_test.go deleted file mode 100644 index e2ce2aca..00000000 --- a/internal/dim-2d/dim-2d_test.go +++ /dev/null @@ -1,117 +0,0 @@ -// Package dim2d_test implements tests for the 2D sub algorithm -package dim2d_test - -import ( - "context" - "os" - "testing" - - "github.com/edaniels/golog" - "github.com/pkg/errors" - "go.viam.com/test" - - dim2d "github.com/viamrobotics/viam-cartographer/internal/dim-2d" - "github.com/viamrobotics/viam-cartographer/internal/testhelper" - "github.com/viamrobotics/viam-cartographer/sensors/lidar" -) - -func TestNewLidar(t *testing.T) { - logger := golog.NewTestLogger(t) - - t.Run("No sensor provided", func(t *testing.T) { - sensors := []string{} - deps := testhelper.SetupDeps(sensors) - actualLidar, err := dim2d.NewLidar(context.Background(), deps, sensors, logger) - expectedLidar := lidar.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 := testhelper.SetupDeps(sensors) - actualLidar, err := dim2d.NewLidar(context.Background(), deps, sensors, logger) - expectedLidar := lidar.Lidar{} - test.That(t, actualLidar, test.ShouldResemble, expectedLidar) - 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]'")) - }) - - t.Run("Failed lidar creation with non-existing sensor", func(t *testing.T) { - sensors := []string{"gibberish"} - deps := testhelper.SetupDeps(sensors) - actualLidar, err := dim2d.NewLidar(context.Background(), deps, sensors, logger) - expectedLidar := lidar.Lidar{} - test.That(t, actualLidar, test.ShouldResemble, expectedLidar) - test.That(t, err, test.ShouldBeError, - errors.New("configuring lidar camera error: error getting lidar camera "+ - "gibberish for slam service: \"rdk:component:camera/gibberish\" missing from dependencies")) - }) - - t.Run("Successful lidar creation", func(t *testing.T) { - sensors := []string{"good_lidar"} - ctx := context.Background() - deps := testhelper.SetupDeps(sensors) - actualLidar, err := dim2d.NewLidar(ctx, deps, sensors, logger) - test.That(t, actualLidar.Name, test.ShouldEqual, sensors[0]) - test.That(t, err, test.ShouldBeNil) - - pc, err := actualLidar.GetData(ctx) - test.That(t, pc, test.ShouldNotBeNil) - test.That(t, err, test.ShouldBeNil) - }) -} - -func TestGetAndSaveData(t *testing.T) { - ctx := context.Background() - logger := golog.NewTestLogger(t) - dataDir, err := testhelper.CreateTempFolderArchitecture(logger) - test.That(t, err, test.ShouldBeNil) - - t.Run("Successful call to GetAndSaveData", func(t *testing.T) { - sensors := []string{"good_lidar"} - deps := testhelper.SetupDeps(sensors) - actualLidar, err := dim2d.NewLidar(ctx, deps, sensors, logger) - test.That(t, actualLidar.Name, test.ShouldEqual, sensors[0]) - test.That(t, err, test.ShouldBeNil) - - _, err = dim2d.GetAndSaveData(ctx, dataDir, actualLidar, logger) - test.That(t, err, test.ShouldBeNil) - - files, err := os.ReadDir(dataDir + "/data/") - test.That(t, len(files), test.ShouldEqual, 1) - test.That(t, err, test.ShouldBeNil) - }) - - testhelper.ClearDirectory(t, dataDir) -} - -func TestValidateGetAndSaveData(t *testing.T) { - ctx := context.Background() - logger := golog.NewTestLogger(t) - dataDir, err := testhelper.CreateTempFolderArchitecture(logger) - test.That(t, err, test.ShouldBeNil) - - t.Run("Successful call to ValidateGetAndSaveData", func(t *testing.T) { - sensors := []string{"good_lidar"} - deps := testhelper.SetupDeps(sensors) - actualLidar, err := dim2d.NewLidar(ctx, deps, sensors, logger) - test.That(t, actualLidar.Name, test.ShouldEqual, sensors[0]) - test.That(t, err, test.ShouldBeNil) - - err = dim2d.ValidateGetAndSaveData(ctx, - dataDir, - actualLidar, - testhelper.SensorValidationMaxTimeoutSecForTest, - testhelper.SensorValidationMaxTimeoutSecForTest, - logger, - ) - test.That(t, err, test.ShouldBeNil) - - files, err := os.ReadDir(dataDir + "/data/") - test.That(t, len(files), test.ShouldEqual, 0) - test.That(t, err, test.ShouldBeNil) - }) - - testhelper.ClearDirectory(t, dataDir) -} diff --git a/internal/testhelper/testhelper.go b/internal/testhelper/testhelper.go index cc524bd8..4a9721b1 100644 --- a/internal/testhelper/testhelper.go +++ b/internal/testhelper/testhelper.go @@ -7,33 +7,23 @@ import ( "bufio" "context" "os" - "strconv" - "sync/atomic" "testing" + "time" "github.com/edaniels/golog" "github.com/pkg/errors" - "github.com/viamrobotics/gostream" - "go.viam.com/rdk/components/camera" - "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/resource" - "go.viam.com/rdk/rimage/transform" "go.viam.com/rdk/services/slam" - "go.viam.com/rdk/testutils/inject" - "go.viam.com/rdk/utils/contextutils" "go.viam.com/test" - "go.viam.com/utils/artifact" "go.viam.com/utils/pexec" viamcartographer "github.com/viamrobotics/viam-cartographer" vcConfig "github.com/viamrobotics/viam-cartographer/config" "github.com/viamrobotics/viam-cartographer/sensors/lidar" + externaltesthelper "github.com/viamrobotics/viam-cartographer/testhelper" ) const ( - // NumPointClouds is the number of pointclouds saved in artifact - // for the cartographer integration tests. - NumPointClouds = 15 // SensorValidationMaxTimeoutSecForTest is used in the ValidateGetAndSaveData // function to ensure that the sensor in the GetAndSaveData function // returns data within an acceptable time. @@ -43,16 +33,8 @@ const ( // sensor that is used in the GetAndSaveData function. SensorValidationIntervalSecForTest = 1 testDialMaxTimeoutSec = 1 - // TestTime can be used to test specific timestamps provided by a replay sensor. - TestTime = "2006-01-02T15:04:05.9999Z" - // BadTime can be used to represent something that should cause an error while parsing it as a time. - BadTime = "NOT A TIME" ) -// IntegrationLidarReleasePointCloudChan is the lidar pointcloud release -// channel for the integration tests. -var IntegrationLidarReleasePointCloudChan = make(chan int, 1) - // Service in the internal package includes additional exported functions relating to the data and // slam processes in the slam service. These functions are not exported to the user. This resolves // a circular import caused by the inject package. @@ -65,118 +47,6 @@ type Service interface { GetSLAMProcessBufferedLogReader() bufio.Reader } -// SetupDeps returns the dependencies based on the sensors passed as arguments. -func SetupDeps(sensors []string) resource.Dependencies { - deps := make(resource.Dependencies) - - for _, sensor := range sensors { - switch sensor { - case "good_lidar": - deps[camera.Named(sensor)] = getGoodLidar() - 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 "cartographer_int_lidar": - deps[camera.Named(sensor)] = getIntegrationLidar() - default: - continue - } - } - return deps -} - -func getGoodLidar() *inject.Camera { - cam := &inject.Camera{} - cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { - return pointcloud.New(), nil - } - cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { - return nil, errors.New("lidar not camera") - } - cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { - return nil, transform.NewNoIntrinsicsError("") - } - cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { - return camera.Properties{}, nil - } - return cam -} - -func getReplaySensor(testTime string) *inject.Camera { - cam := &inject.Camera{} - cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { - md := ctx.Value(contextutils.MetadataContextKey) - if mdMap, ok := md.(map[string][]string); ok { - mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} - } - return pointcloud.New(), nil - } - cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { - return nil, errors.New("lidar not camera") - } - cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { - return nil, transform.NewNoIntrinsicsError("") - } - cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { - return camera.Properties{}, nil - } - return cam -} - -func getInvalidSensor() *inject.Camera { - cam := &inject.Camera{} - cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { - return nil, errors.New("invalid sensor") - } - cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { - return nil, errors.New("invalid sensor") - } - cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { - return nil, transform.NewNoIntrinsicsError("") - } - return cam -} - -func getIntegrationLidar() *inject.Camera { - cam := &inject.Camera{} - var index uint64 - cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { - select { - case <-IntegrationLidarReleasePointCloudChan: - i := atomic.AddUint64(&index, 1) - 1 - if i >= NumPointClouds { - return nil, errors.New("No more cartographer point clouds") - } - file, err := os.Open(artifact.MustPath("viam-cartographer/mock_lidar/" + strconv.FormatUint(i, 10) + ".pcd")) - if err != nil { - return nil, err - } - pointCloud, err := pointcloud.ReadPCD(file) - if err != nil { - return nil, err - } - return pointCloud, nil - default: - return nil, errors.Errorf("Lidar not ready to return point cloud %v", atomic.LoadUint64(&index)) - } - } - cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { - return nil, errors.New("lidar not camera") - } - cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { - return nil, transform.NewNoIntrinsicsError("") - } - cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { - return camera.Properties{}, nil - } - return cam -} - // ClearDirectory deletes the contents in the path directory // without deleting path itself. func ClearDirectory(t *testing.T, path string) { @@ -200,7 +70,7 @@ func CreateSLAMService( cfgService := resource.Config{Name: "test", API: slam.API, Model: viamcartographer.Model} cfgService.ConvertedAttributes = cfg - deps := SetupDeps(cfg.Sensors) + deps := externaltesthelper.SetupDeps(cfg.Sensors) sensorDeps, err := cfg.Validate("path") if err != nil { @@ -218,6 +88,7 @@ func CreateSLAMService( SensorValidationMaxTimeoutSecForTest, SensorValidationIntervalSecForTest, testDialMaxTimeoutSec, + 5*time.Second, ) if err != nil { test.That(t, svc, test.ShouldBeNil) @@ -263,7 +134,7 @@ const ( func CreateTempFolderArchitecture(logger golog.Logger) (string, error) { tmpDir, err := os.MkdirTemp("", "*") if err != nil { - return "nil", err + return "", err } if err := vcConfig.SetupDirectories(tmpDir, logger); err != nil { return "", err diff --git a/module/main.go b/module/main.go index 0f3e3d20..f35b84bd 100644 --- a/module/main.go +++ b/module/main.go @@ -41,6 +41,16 @@ func mainWithArgs(ctx context.Context, args []string, logger golog.Logger) error return nil } + if err := viamcartographer.InitCartoLib(logger); err != nil { + return err + } + + defer func() { + if err := viamcartographer.TerminateCartoLib(); err != nil { + logger.Errorw("failed to terminate carto lib", "error", err) + } + }() + // Instantiate the module cartoModule, err := module.NewModuleFromArgs(ctx, logger) if err != nil { diff --git a/sensorprocess/sensorprocess_test.go b/sensorprocess/sensorprocess_test.go index 867c6b8e..943749fd 100644 --- a/sensorprocess/sensorprocess_test.go +++ b/sensorprocess/sensorprocess_test.go @@ -10,8 +10,8 @@ import ( "go.viam.com/test" "github.com/viamrobotics/viam-cartographer/cartofacade" - "github.com/viamrobotics/viam-cartographer/internal/testhelper" "github.com/viamrobotics/viam-cartographer/sensors/lidar" + "github.com/viamrobotics/viam-cartographer/testhelper" ) type addSensorReadingArgs struct { diff --git a/internal/dim-2d/dim-2d.go b/sensors/lidar/dim-2d/dim-2d.go similarity index 70% rename from internal/dim-2d/dim-2d.go rename to sensors/lidar/dim-2d/dim-2d.go index 9028c2cf..00ff3a11 100644 --- a/internal/dim-2d/dim-2d.go +++ b/sensors/lidar/dim-2d/dim-2d.go @@ -11,6 +11,7 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" "go.opencensus.io/trace" + "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/resource" "go.viam.com/rdk/utils/contextutils" goutils "go.viam.com/utils" @@ -54,6 +55,62 @@ func NewLidar( return lidar, nil } +// GetTimedData returns a 2d lidar reading, the timestamp from when it wask taken +// (either in live mode or offline mode) +// and an error if an error occurred getting the lidar data or parsing the offline +// timestamp. +func GetTimedData(ctx context.Context, lidar lidar.Lidar) (time.Time, pointcloud.PointCloud, error) { + ctx, md := contextutils.ContextWithMetadata(ctx) + reqTime := time.Now().UTC() + pointcloud, err := lidar.GetData(ctx) + if err != nil { + return reqTime, nil, err + } + + timeRequestedMetadata, ok := md[contextutils.TimeRequestedMetadataKey] + if ok { + reqTime, err = time.Parse(time.RFC3339Nano, timeRequestedMetadata[0]) + if err != nil { + return reqTime, nil, err + } + } + return reqTime, pointcloud, nil +} + +// ValidateGetData checks every sensorValidationIntervalSec if the provided lidar +// returned a valid timed lidar readings every sensorValidationIntervalSec +// until either success or sensorValidationMaxTimeoutSec has elapsed. +// returns an error if no valid lidar readings were returned. +func ValidateGetData( + ctx context.Context, + lidar lidar.Lidar, + sensorValidationMaxTimeout time.Duration, + sensorValidationInterval time.Duration, + logger golog.Logger, +) error { + ctx, span := trace.StartSpan(ctx, "viamcartographer::internal::dim2d::ValidateGetData") + defer span.End() + + startTime := time.Now().UTC() + + for { + _, _, err := GetTimedData(ctx, lidar) + if err == nil { + break + } + + logger.Debugw("ValidateGetData hit error: ", "error", err) + if time.Since(startTime) >= sensorValidationMaxTimeout { + return errors.Wrap(err, "ValidateGetData timeout") + } + if !goutils.SelectContextOrWait(ctx, sensorValidationInterval) { + return ctx.Err() + } + } + + return nil +} + // ValidateGetAndSaveData makes sure that the provided sensor is actually a lidar and can // return pointclouds. It also ensures that saving the data to files works as intended. func ValidateGetAndSaveData( diff --git a/sensors/lidar/dim-2d/dim-2d_test.go b/sensors/lidar/dim-2d/dim-2d_test.go new file mode 100644 index 00000000..991f26dc --- /dev/null +++ b/sensors/lidar/dim-2d/dim-2d_test.go @@ -0,0 +1,216 @@ +// Package dim2d_test implements tests for the 2D sub algorithm +package dim2d_test + +import ( + "context" + "os" + "testing" + "time" + + "github.com/edaniels/golog" + "github.com/pkg/errors" + "go.viam.com/test" + + internaltesthelper "github.com/viamrobotics/viam-cartographer/internal/testhelper" + "github.com/viamrobotics/viam-cartographer/sensors/lidar" + dim2d "github.com/viamrobotics/viam-cartographer/sensors/lidar/dim-2d" + "github.com/viamrobotics/viam-cartographer/testhelper" +) + +func TestNewLidar(t *testing.T) { + logger := golog.NewTestLogger(t) + + t.Run("No sensor provided", func(t *testing.T) { + sensors := []string{} + deps := testhelper.SetupDeps(sensors) + actualLidar, err := dim2d.NewLidar(context.Background(), deps, sensors, logger) + expectedLidar := lidar.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 := testhelper.SetupDeps(sensors) + actualLidar, err := dim2d.NewLidar(context.Background(), deps, sensors, logger) + expectedLidar := lidar.Lidar{} + test.That(t, actualLidar, test.ShouldResemble, expectedLidar) + 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]'")) + }) + + t.Run("Failed lidar creation with non-existing sensor", func(t *testing.T) { + sensors := []string{"gibberish"} + deps := testhelper.SetupDeps(sensors) + actualLidar, err := dim2d.NewLidar(context.Background(), deps, sensors, logger) + expectedLidar := lidar.Lidar{} + test.That(t, actualLidar, test.ShouldResemble, expectedLidar) + test.That(t, err, test.ShouldBeError, + errors.New("configuring lidar camera error: error getting lidar camera "+ + "gibberish for slam service: \"rdk:component:camera/gibberish\" missing from dependencies")) + }) + + t.Run("Successful lidar creation", func(t *testing.T) { + sensors := []string{"good_lidar"} + ctx := context.Background() + deps := testhelper.SetupDeps(sensors) + actualLidar, err := dim2d.NewLidar(ctx, deps, sensors, logger) + test.That(t, actualLidar.Name, test.ShouldEqual, sensors[0]) + test.That(t, err, test.ShouldBeNil) + + pc, err := actualLidar.GetData(ctx) + test.That(t, pc, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeNil) + }) +} + +func TestGetAndSaveData(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger) + test.That(t, err, test.ShouldBeNil) + + t.Run("Successful call to GetAndSaveData", func(t *testing.T) { + sensors := []string{"good_lidar"} + deps := testhelper.SetupDeps(sensors) + actualLidar, err := dim2d.NewLidar(ctx, deps, sensors, logger) + test.That(t, actualLidar.Name, test.ShouldEqual, sensors[0]) + test.That(t, err, test.ShouldBeNil) + + _, err = dim2d.GetAndSaveData(ctx, dataDir, actualLidar, logger) + test.That(t, err, test.ShouldBeNil) + + files, err := os.ReadDir(dataDir + "/data/") + test.That(t, len(files), test.ShouldEqual, 1) + test.That(t, err, test.ShouldBeNil) + }) + + internaltesthelper.ClearDirectory(t, dataDir) +} + +func TestValidateGetAndSaveData(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger) + test.That(t, err, test.ShouldBeNil) + + t.Run("Successful call to ValidateGetAndSaveData", func(t *testing.T) { + sensors := []string{"good_lidar"} + deps := testhelper.SetupDeps(sensors) + actualLidar, err := dim2d.NewLidar(ctx, deps, sensors, logger) + test.That(t, actualLidar.Name, test.ShouldEqual, sensors[0]) + test.That(t, err, test.ShouldBeNil) + + err = dim2d.ValidateGetAndSaveData(ctx, + dataDir, + actualLidar, + internaltesthelper.SensorValidationMaxTimeoutSecForTest, + internaltesthelper.SensorValidationMaxTimeoutSecForTest, + logger, + ) + test.That(t, err, test.ShouldBeNil) + + files, err := os.ReadDir(dataDir + "/data/") + test.That(t, len(files), test.ShouldEqual, 0) + test.That(t, err, test.ShouldBeNil) + }) + + internaltesthelper.ClearDirectory(t, dataDir) +} + +func TestGetTimedData(t *testing.T) { + logger := golog.NewTestLogger(t) + ctx := context.Background() + + sensors := []string{"invalid_sensor"} + invalidLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger) + test.That(t, err, test.ShouldBeNil) + + sensors = []string{"invalid_replay_sensor"} + invalidReplayLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger) + test.That(t, err, test.ShouldBeNil) + + sensors = []string{"good_lidar"} + goodLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger) + test.That(t, err, test.ShouldBeNil) + + sensors = []string{"replay_sensor"} + goodReplayLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger) + test.That(t, err, test.ShouldBeNil) + + t.Run("when the lidar returns an error, returns that error", func(t *testing.T) { + _, pc, err := dim2d.GetTimedData(ctx, invalidLidar) + test.That(t, err, test.ShouldBeError, errors.New("invalid sensor")) + test.That(t, pc, test.ShouldBeNil) + }) + + t.Run("when the replay lidar succeeds but the timestamp is invalid, returns an error", func(t *testing.T) { + _, pc, err := dim2d.GetTimedData(ctx, invalidReplayLidar) + msg := "parsing time \"NOT A TIME\" as \"2006-01-02T15:04:05.999999999Z07:00\": cannot parse \"NOT A TIME\" as \"2006\"" + test.That(t, err, test.ShouldBeError, errors.New(msg)) + test.That(t, pc, test.ShouldBeNil) + }) + + t.Run("when a live lidar succeeds, returns current time in UTC and the reading", func(t *testing.T) { + beforeReading := time.Now().UTC() + pcTime, pc, err := dim2d.GetTimedData(ctx, goodLidar) + test.That(t, err, test.ShouldBeNil) + test.That(t, pc, test.ShouldNotBeNil) + test.That(t, pcTime.After(beforeReading), test.ShouldBeTrue) + test.That(t, pcTime.Location(), test.ShouldEqual, time.UTC) + }) + + t.Run("when a replay lidar succeeds, returns the replay sensor time and the reading", func(t *testing.T) { + pcTime, pc, err := dim2d.GetTimedData(ctx, goodReplayLidar) + test.That(t, err, test.ShouldBeNil) + test.That(t, pc, test.ShouldNotBeNil) + test.That(t, pcTime, test.ShouldEqual, time.Date(2006, 1, 2, 15, 4, 5, 999900000, time.UTC)) + }) +} + +func TestValidateGetData(t *testing.T) { + logger := golog.NewTestLogger(t) + ctx := context.Background() + + sensors := []string{"good_lidar"} + goodLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger) + test.That(t, err, test.ShouldBeNil) + + sensors = []string{"invalid_sensor"} + invalidLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger) + test.That(t, err, test.ShouldBeNil) + + sensorValidationMaxTimeout := time.Duration(50) * time.Millisecond + sensorValidationInterval := time.Duration(10) * time.Millisecond + + t.Run("returns nil if a lidar reading succeeds immediately", func(t *testing.T) { + err := dim2d.ValidateGetData(ctx, goodLidar, sensorValidationMaxTimeout, sensorValidationInterval, logger) + test.That(t, err, test.ShouldBeNil) + }) + + t.Run("returns nil if a lidar reading succeeds within the timeout", func(t *testing.T) { + sensors = []string{"warming_up_lidar"} + warmingUpLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger) + test.That(t, err, test.ShouldBeNil) + + err = dim2d.ValidateGetData(ctx, warmingUpLidar, sensorValidationMaxTimeout, sensorValidationInterval, logger) + test.That(t, err, test.ShouldBeNil) + }) + + t.Run("returns error if no lidar reading succeeds within the timeout", func(t *testing.T) { + err := dim2d.ValidateGetData(ctx, invalidLidar, sensorValidationMaxTimeout, sensorValidationInterval, logger) + test.That(t, err, test.ShouldBeError, errors.New("ValidateGetData timeout: invalid sensor")) + }) + + t.Run("returns error if no lidar reading succeeds by the time the context is cancelled", func(t *testing.T) { + cancelledCtx, cancelFunc := context.WithCancel(context.Background()) + cancelFunc() + + sensors = []string{"warming_up_lidar"} + warmingUpLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger) + test.That(t, err, test.ShouldBeNil) + + err = dim2d.ValidateGetData(cancelledCtx, warmingUpLidar, sensorValidationMaxTimeout, sensorValidationInterval, logger) + test.That(t, err, test.ShouldBeError, context.Canceled) + }) +} diff --git a/internal/dim-2d/sensor-indices.go b/sensors/lidar/dim-2d/sensor-indices.go similarity index 100% rename from internal/dim-2d/sensor-indices.go rename to sensors/lidar/dim-2d/sensor-indices.go diff --git a/internal/inject/slam_service.go b/testhelper/inject/slam_service.go similarity index 100% rename from internal/inject/slam_service.go rename to testhelper/inject/slam_service.go diff --git a/testhelper/testhelper.go b/testhelper/testhelper.go new file mode 100644 index 00000000..679922e6 --- /dev/null +++ b/testhelper/testhelper.go @@ -0,0 +1,169 @@ +// Package testhelper provides test helpers which don't depend on viamcartographer +package testhelper + +import ( + "context" + "os" + "strconv" + "sync/atomic" + + "github.com/pkg/errors" + "github.com/viamrobotics/gostream" + "go.viam.com/rdk/components/camera" + "go.viam.com/rdk/pointcloud" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/rimage/transform" + "go.viam.com/rdk/testutils/inject" + "go.viam.com/rdk/utils/contextutils" + "go.viam.com/utils/artifact" +) + +const ( + // NumPointClouds is the number of pointclouds saved in artifact + // for the cartographer integration tests. + NumPointClouds = 15 + // TestTime can be used to test specific timestamps provided by a replay sensor. + TestTime = "2006-01-02T15:04:05.9999Z" + // BadTime can be used to represent something that should cause an error while parsing it as a time. + BadTime = "NOT A TIME" +) + +// IntegrationLidarReleasePointCloudChan is the lidar pointcloud release +// channel for the integration tests. +var IntegrationLidarReleasePointCloudChan = make(chan int, 1) + +// SetupDeps returns the dependencies based on the sensors passed as arguments. +func SetupDeps(sensors []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 "cartographer_int_lidar": + deps[camera.Named(sensor)] = getIntegrationLidar() + default: + continue + } + } + return deps +} + +func getWarmingUpLidar() *inject.Camera { + cam := &inject.Camera{} + counter := 0 + cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { + counter++ + if counter == 1 { + return nil, errors.Errorf("warming up %d", counter) + } + return pointcloud.New(), nil + } + cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { + return nil, errors.New("lidar not camera") + } + cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { + return nil, transform.NewNoIntrinsicsError("") + } + cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { + return camera.Properties{}, nil + } + return cam +} + +func getGoodLidar() *inject.Camera { + cam := &inject.Camera{} + cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { + return pointcloud.New(), nil + } + cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { + return nil, errors.New("lidar not camera") + } + cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { + return nil, transform.NewNoIntrinsicsError("") + } + cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { + return camera.Properties{}, nil + } + return cam +} + +func getReplaySensor(testTime string) *inject.Camera { + cam := &inject.Camera{} + cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { + md := ctx.Value(contextutils.MetadataContextKey) + if mdMap, ok := md.(map[string][]string); ok { + mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} + } + return pointcloud.New(), nil + } + cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { + return nil, errors.New("lidar not camera") + } + cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { + return nil, transform.NewNoIntrinsicsError("") + } + cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { + return camera.Properties{}, nil + } + return cam +} + +func getInvalidSensor() *inject.Camera { + cam := &inject.Camera{} + cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { + return nil, errors.New("invalid sensor") + } + cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { + return nil, errors.New("invalid sensor") + } + cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { + return nil, transform.NewNoIntrinsicsError("") + } + return cam +} + +func getIntegrationLidar() *inject.Camera { + cam := &inject.Camera{} + var index uint64 + cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) { + select { + case <-IntegrationLidarReleasePointCloudChan: + i := atomic.AddUint64(&index, 1) - 1 + if i >= NumPointClouds { + return nil, errors.New("No more cartographer point clouds") + } + file, err := os.Open(artifact.MustPath("viam-cartographer/mock_lidar/" + strconv.FormatUint(i, 10) + ".pcd")) + if err != nil { + return nil, err + } + pointCloud, err := pointcloud.ReadPCD(file) + if err != nil { + return nil, err + } + return pointCloud, nil + default: + return nil, errors.Errorf("Lidar not ready to return point cloud %v", atomic.LoadUint64(&index)) + } + } + cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) { + return nil, errors.New("lidar not camera") + } + cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) { + return nil, transform.NewNoIntrinsicsError("") + } + cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) { + return camera.Properties{}, nil + } + return cam +} diff --git a/viam-cartographer.go b/viam-cartographer.go index 04a47026..f9ef59ad 100644 --- a/viam-cartographer.go +++ b/viam-cartographer.go @@ -15,6 +15,7 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" "go.opencensus.io/trace" + "go.uber.org/zap/zapcore" pb "go.viam.com/api/service/slam/v1" viamgrpc "go.viam.com/rdk/grpc" "go.viam.com/rdk/resource" @@ -24,14 +25,19 @@ import ( goutils "go.viam.com/utils" "go.viam.com/utils/pexec" + "github.com/viamrobotics/viam-cartographer/cartofacade" vcConfig "github.com/viamrobotics/viam-cartographer/config" - dim2d "github.com/viamrobotics/viam-cartographer/internal/dim-2d" + "github.com/viamrobotics/viam-cartographer/sensorprocess" "github.com/viamrobotics/viam-cartographer/sensors/lidar" + dim2d "github.com/viamrobotics/viam-cartographer/sensors/lidar/dim-2d" vcUtils "github.com/viamrobotics/viam-cartographer/utils" ) // Model is the model name of cartographer. -var Model = resource.NewModel("viam", "slam", "cartographer") +var ( + Model = resource.NewModel("viam", "slam", "cartographer") + cartoLib cartofacade.CartoLib +) const ( // DefaultExecutableName is what this program expects to call to start the cartographer grpc server. @@ -43,8 +49,25 @@ const ( defaultSensorValidationIntervalSec = 1 parsePortMaxTimeoutSec = 60 localhost0 = "localhost:0" + defaultCartoFacadeTimeout = 5 * time.Second ) +var defaultCartoAlgoCfg = cartofacade.CartoAlgoConfig{ + OptimizeOnStart: false, + OptimizeEveryNNodes: 3, + NumRangeData: 30, + MissingDataRayLength: 25.0, + MaxRange: 25.0, + MinRange: 0.2, + MaxSubmapsToKeep: 3, + FreshSubmapsCount: 3, + MinCoveredArea: 1.0, + MinAddedSubmapsCount: 1, + OccupiedSpaceWeight: 20.0, + TranslationWeight: 10.0, + RotationWeight: 1.0, +} + // SubAlgo defines the cartographer specific sub-algorithms that we support. type SubAlgo string @@ -69,11 +92,53 @@ func init() { defaultSensorValidationMaxTimeoutSec, defaultSensorValidationIntervalSec, defaultDialMaxTimeoutSec, + defaultCartoFacadeTimeout, ) }, }) } +// InitCartoLib is run to initialize the cartographer library +// must be called before module.AddModelFromRegistry is +// called. +func InitCartoLib(logger golog.Logger) error { + minloglevel := 1 // warn + vlog := 0 // disabled + if logger.Level() == zapcore.DebugLevel { + minloglevel = 0 // info + vlog = 1 // verbose enabled + } + lib, err := cartofacade.NewLib(minloglevel, vlog) + if err != nil { + return err + } + cartoLib = lib + return nil +} + +// TerminateCartoLib is run to terminate the cartographer library. +func TerminateCartoLib() error { + return cartoLib.Terminate() +} + +func initSensorProcess(cancelCtx context.Context, cartoSvc *cartographerService) { + spConfig := sensorprocess.Config{ + CartoFacade: cartoSvc.cartofacade, + Lidar: cartoSvc.lidar, + LidarName: cartoSvc.primarySensorName, + DataRateMs: cartoSvc.dataRateMs, + Timeout: cartoSvc.cartoFacadeTimeout, + Logger: cartoSvc.logger, + TelemetryEnabled: cartoSvc.logger.Level() == zapcore.DebugLevel, + } + + cartoSvc.sensorProcessWorkers.Add(1) + go func() { + defer cartoSvc.sensorProcessWorkers.Done() + sensorprocess.Start(cancelCtx, spConfig) + }() +} + // New returns a new slam service for the given robot. func New( ctx context.Context, @@ -85,6 +150,7 @@ func New( sensorValidationMaxTimeoutSec int, sensorValidationIntervalSec int, dialMaxTimeoutSec int, + cartoFacadeTimeout time.Duration, ) (slam.Service, error) { ctx, span := trace.StartSpan(ctx, "viamcartographer::slamService::New") defer span.End() @@ -100,12 +166,7 @@ func New( c.Model.Name, svcConfig.ConfigParams["mode"]) } - // Set up the data directories - if err := vcConfig.SetupDirectories(svcConfig.DataDirectory, logger); err != nil { - return nil, err - } - - port, dataRateMsec, mapRateSec, useLiveData, deleteProcessedData, _, err := vcConfig.GetOptionalParameters( + port, dataRateMsec, mapRateSec, useLiveData, deleteProcessedData, modularizationV2Enabled, err := vcConfig.GetOptionalParameters( svcConfig, localhost0, defaultDataRateMsec, @@ -116,6 +177,12 @@ func New( return nil, err } + if !modularizationV2Enabled { + if err := vcConfig.SetupDirectories(svcConfig.DataDirectory, logger); err != nil { + return nil, err + } + } + // Get the lidar for the Dim2D cartographer sub algorithm lidar, err := dim2d.NewLidar(ctx, deps, svcConfig.Sensors, logger) if err != nil { @@ -124,59 +191,244 @@ func New( // Need to pass in a long-lived context because ctx is short-lived cancelCtx, cancelFunc := context.WithCancel(context.Background()) + // Need to be able to shut down the sensor process before the cartoFacade + cancelSensorProcessCtx, cancelSensorProcessFunc := context.WithCancel(context.Background()) + cancelCartoFacadeCtx, cancelCartoFacadeFunc := context.WithCancel(context.Background()) // Cartographer SLAM Service Object cartoSvc := &cartographerService{ - Named: c.ResourceName().AsNamed(), - primarySensorName: lidar.Name, - executableName: executableName, - subAlgo: subAlgo, - slamProcess: pexec.NewProcessManager(logger), - configParams: svcConfig.ConfigParams, - dataDirectory: svcConfig.DataDirectory, - useLiveData: useLiveData, - deleteProcessedData: deleteProcessedData, - port: port, - dataRateMs: dataRateMsec, - mapRateSec: mapRateSec, - cancelFunc: cancelFunc, - logger: logger, - bufferSLAMProcessLogs: bufferSLAMProcessLogs, - localizationMode: mapRateSec == 0, - mapTimestamp: time.Now().UTC(), - } - - success := false + Named: c.ResourceName().AsNamed(), + primarySensorName: lidar.Name, + lidar: lidar, + executableName: executableName, + subAlgo: subAlgo, + slamProcess: pexec.NewProcessManager(logger), + configParams: svcConfig.ConfigParams, + dataDirectory: svcConfig.DataDirectory, + sensors: svcConfig.Sensors, + useLiveData: useLiveData, + deleteProcessedData: deleteProcessedData, + port: port, + dataRateMs: dataRateMsec, + mapRateSec: mapRateSec, + cancelFunc: cancelFunc, + cancelSensorProcessFunc: cancelSensorProcessFunc, + cancelCartoFacadeFunc: cancelCartoFacadeFunc, + logger: logger, + bufferSLAMProcessLogs: bufferSLAMProcessLogs, + modularizationV2Enabled: modularizationV2Enabled, + sensorValidationMaxTimeoutSec: sensorValidationMaxTimeoutSec, + sensorValidationIntervalSec: sensorValidationMaxTimeoutSec, + dialMaxTimeoutSec: dialMaxTimeoutSec, + cartoFacadeTimeout: cartoFacadeTimeout, + localizationMode: mapRateSec == 0, + mapTimestamp: time.Now().UTC(), + } defer func() { - if !success { + if err != nil { + logger.Errorw("New() hit error, closing...", "error", err) if err := cartoSvc.Close(ctx); err != nil { logger.Errorw("error closing out after error", "error", err) } } }() + if modularizationV2Enabled { + if err = dim2d.ValidateGetData( + cancelSensorProcessCtx, + cartoSvc.lidar, + time.Duration(sensorValidationMaxTimeoutSec)*time.Second, + time.Duration(cartoSvc.sensorValidationIntervalSec)*time.Second, + cartoSvc.logger); err != nil { + err = errors.Wrap(err, "failed to get data from lidar") + return nil, err + } + + err = initCartoFacade(cancelCartoFacadeCtx, cartoSvc) + if err != nil { + return nil, err + } + + initSensorProcess(cancelSensorProcessCtx, cartoSvc) + return cartoSvc, nil + } + + err = initCartoGrpcServer(ctx, cancelCtx, cartoSvc) + if err != nil { + return nil, err + } + return cartoSvc, nil +} + +func parseCartoAlgoConfig(configParams map[string]string, logger golog.Logger) (cartofacade.CartoAlgoConfig, error) { + cartoAlgoCfg := defaultCartoAlgoCfg + for k, val := range configParams { + switch k { + case "optimize_on_start": + if val == "true" { + cartoAlgoCfg.OptimizeOnStart = true + } + case "optimize_every_n_nodes": + iVal, err := strconv.Atoi(val) + if err != nil { + return cartoAlgoCfg, err + } + cartoAlgoCfg.OptimizeEveryNNodes = iVal + case "num_range_data": + iVal, err := strconv.Atoi(val) + if err != nil { + return cartoAlgoCfg, err + } + cartoAlgoCfg.NumRangeData = iVal + case "missing_data_ray_length": + fVal, err := strconv.ParseFloat(val, 32) + if err != nil { + return cartoAlgoCfg, err + } + cartoAlgoCfg.MissingDataRayLength = float32(fVal) + case "max_range": + fVal, err := strconv.ParseFloat(val, 32) + if err != nil { + return cartoAlgoCfg, err + } + cartoAlgoCfg.MaxRange = float32(fVal) + case "min_range": + fVal, err := strconv.ParseFloat(val, 32) + if err != nil { + return cartoAlgoCfg, err + } + cartoAlgoCfg.MinRange = float32(fVal) + case "max_submaps_to_keep": + iVal, err := strconv.Atoi(val) + if err != nil { + return cartoAlgoCfg, err + } + cartoAlgoCfg.MaxSubmapsToKeep = iVal + case "fresh_submaps_count": + iVal, err := strconv.Atoi(val) + if err != nil { + return cartoAlgoCfg, err + } + cartoAlgoCfg.FreshSubmapsCount = iVal + case "min_covered_area": + fVal, err := strconv.ParseFloat(val, 64) + if err != nil { + return cartoAlgoCfg, err + } + cartoAlgoCfg.MinCoveredArea = fVal + case "min_added_submaps_count": + iVal, err := strconv.Atoi(val) + if err != nil { + return cartoAlgoCfg, err + } + cartoAlgoCfg.MinAddedSubmapsCount = iVal + case "occupied_space_weight": + fVal, err := strconv.ParseFloat(val, 64) + if err != nil { + return cartoAlgoCfg, err + } + cartoAlgoCfg.OccupiedSpaceWeight = fVal + case "translation_weight": + fVal, err := strconv.ParseFloat(val, 64) + if err != nil { + return cartoAlgoCfg, err + } + cartoAlgoCfg.TranslationWeight = fVal + case "rotation_weight": + fVal, err := strconv.ParseFloat(val, 64) + if err != nil { + return cartoAlgoCfg, err + } + cartoAlgoCfg.RotationWeight = fVal + // ignore mode as it is a special case + case "mode": + default: + logger.Warnf("unused config param: %s: %s", k, val) + } + } + return cartoAlgoCfg, nil +} + +// initCartoFacade +// 1. creates a new initCartoFacade +// 2. initializes it and starts it +// 3. terminates it if start fails. +func initCartoFacade(ctx context.Context, cartoSvc *cartographerService) error { + cartoAlgoConfig, err := parseCartoAlgoConfig(cartoSvc.configParams, cartoSvc.logger) + if err != nil { + return err + } + + cartoCfg := cartofacade.CartoConfig{ + Sensors: cartoSvc.sensors, + MapRateSecond: cartoSvc.mapRateSec, + DataDir: cartoSvc.dataDirectory, + ComponentReference: cartoSvc.primarySensorName, + LidarConfig: cartofacade.TwoD, + } + + cf := cartofacade.New(&cartoLib, cartoCfg, cartoAlgoConfig) + err = cf.Initialize(ctx, cartoSvc.cartoFacadeTimeout, &cartoSvc.cartoFacadeWorkers) + if err != nil { + cartoSvc.logger.Errorw("cartofacade initialize failed", "error", err) + return err + } + err = cf.Start(ctx, cartoSvc.cartoFacadeTimeout) + if err != nil { + cartoSvc.logger.Errorw("cartofacade start failed", "error", err) + termErr := cf.Terminate(ctx, cartoSvc.cartoFacadeTimeout) + if termErr != nil { + cartoSvc.logger.Errorw("cartofacade terminate failed", "error", termErr) + return termErr + } + return err + } + + cartoSvc.cartofacade = &cf + + return nil +} + +func terminateCartoFacade(ctx context.Context, cartoSvc *cartographerService) error { + if cartoSvc.cartofacade == nil { + cartoSvc.logger.Debug("terminateCartoFacade called when cartoSvc.cartofacade is nil") + return nil + } + stopErr := cartoSvc.cartofacade.Stop(ctx, cartoSvc.cartoFacadeTimeout) + if stopErr != nil { + cartoSvc.logger.Errorw("cartofacade stop failed", "error", stopErr) + } + + err := cartoSvc.cartofacade.Terminate(ctx, cartoSvc.cartoFacadeTimeout) + if err != nil { + cartoSvc.logger.Errorw("cartofacade terminate failed", "error", err) + return err + } + return stopErr +} + +func initCartoGrpcServer(ctx, cancelCtx context.Context, cartoSvc *cartographerService) error { if cartoSvc.primarySensorName != "" { - if err := dim2d.ValidateGetAndSaveData(cancelCtx, cartoSvc.dataDirectory, lidar, - sensorValidationMaxTimeoutSec, sensorValidationIntervalSec, cartoSvc.logger); err != nil { - return nil, errors.Wrap(err, "getting and saving data failed") + if err := dim2d.ValidateGetAndSaveData(cancelCtx, cartoSvc.dataDirectory, cartoSvc.lidar, + cartoSvc.sensorValidationMaxTimeoutSec, cartoSvc.sensorValidationIntervalSec, cartoSvc.logger); err != nil { + return errors.Wrap(err, "getting and saving data failed") } - cartoSvc.StartDataProcess(cancelCtx, lidar, nil) - logger.Debugf("Reading data from sensor: %v", cartoSvc.primarySensorName) + cartoSvc.StartDataProcess(cancelCtx, cartoSvc.lidar, nil) + cartoSvc.logger.Debugf("Reading data from sensor: %v", cartoSvc.primarySensorName) } if err := cartoSvc.StartSLAMProcess(ctx); err != nil { - return nil, errors.Wrap(err, "error with slam service slam process") + return errors.Wrap(err, "error with slam service slam process") } - client, clientClose, err := vcConfig.SetupGRPCConnection(ctx, cartoSvc.port, dialMaxTimeoutSec, logger) + client, clientClose, err := vcConfig.SetupGRPCConnection(ctx, cartoSvc.port, cartoSvc.dialMaxTimeoutSec, cartoSvc.logger) if err != nil { - return nil, errors.Wrap(err, "error with initial grpc client to slam algorithm") + return errors.Wrap(err, "error with initial grpc client to slam algorithm") } cartoSvc.clientAlgo = client cartoSvc.clientAlgoClose = clientClose - success = true - return cartoSvc, nil + return nil } // cartographerService is the structure of the slam service. @@ -184,32 +436,58 @@ type cartographerService struct { resource.Named resource.AlwaysRebuild primarySensorName string + lidar lidar.Lidar executableName string subAlgo SubAlgo - slamProcess pexec.ProcessManager - clientAlgo pb.SLAMServiceClient - clientAlgoClose func() error - - configParams map[string]string - dataDirectory string + // deprecated + slamProcess pexec.ProcessManager + // deprecated + clientAlgo pb.SLAMServiceClient + // deprecated + clientAlgoClose func() error + + configParams map[string]string + dataDirectory string + sensors []string + // deprecated deleteProcessedData bool - useLiveData bool + // deprecated + useLiveData bool + + modularizationV2Enabled bool + cartofacade *cartofacade.CartoFacade + cartoFacadeTimeout time.Duration + // deprecated port string dataRateMs int mapRateSec int + // deprecated cancelFunc func() + cancelSensorProcessFunc func() + cancelCartoFacadeFunc func() logger golog.Logger + // deprecated activeBackgroundWorkers sync.WaitGroup - - bufferSLAMProcessLogs bool - slamProcessLogReader io.ReadCloser - slamProcessLogWriter io.WriteCloser + sensorProcessWorkers sync.WaitGroup + cartoFacadeWorkers sync.WaitGroup + + // deprecated + bufferSLAMProcessLogs bool + // deprecated + slamProcessLogReader io.ReadCloser + // deprecated + slamProcessLogWriter io.WriteCloser + // deprecated slamProcessBufferedLogReader bufio.Reader - localizationMode bool - mapTimestamp time.Time + localizationMode bool + mapTimestamp time.Time + sensorValidationMaxTimeoutSec int + sensorValidationIntervalSec int + // deprecated + dialMaxTimeoutSec int } // GetPosition forwards the request for positional data to the slam library's gRPC service. Once a response is received, @@ -349,6 +627,24 @@ func (cartoSvc *cartographerService) DoCommand(ctx context.Context, req map[stri // Close out of all slam related processes. func (cartoSvc *cartographerService) Close(ctx context.Context) error { + // TODO: Make this atomic & idempotent + if cartoSvc.modularizationV2Enabled { + // stop sensor process workers + cartoSvc.cancelSensorProcessFunc() + cartoSvc.sensorProcessWorkers.Wait() + + // terminate carto facade + err := terminateCartoFacade(ctx, cartoSvc) + if err != nil { + cartoSvc.logger.Errorw("close hit error", "error", err) + } + + // stop carto facade workers + cartoSvc.cancelCartoFacadeFunc() + cartoSvc.cartoFacadeWorkers.Wait() + return nil + } + defer func() { if cartoSvc.clientAlgoClose != nil { goutils.UncheckedErrorFunc(cartoSvc.clientAlgoClose) diff --git a/viam-cartographer_internal_test.go b/viam-cartographer_internal_test.go index df8c85ae..f596549c 100644 --- a/viam-cartographer_internal_test.go +++ b/viam-cartographer_internal_test.go @@ -5,6 +5,7 @@ import ( "context" "testing" + "github.com/edaniels/golog" "github.com/golang/geo/r3" "github.com/pkg/errors" commonv1 "go.viam.com/api/common/v1" @@ -16,7 +17,8 @@ import ( "google.golang.org/grpc" "google.golang.org/protobuf/types/known/structpb" - inject "github.com/viamrobotics/viam-cartographer/internal/inject" + "github.com/viamrobotics/viam-cartographer/cartofacade" + inject "github.com/viamrobotics/viam-cartographer/testhelper/inject" ) type pointCloudClientMock struct { @@ -397,3 +399,88 @@ func TestGetInternalStateEndpoint(t *testing.T) { }) }) } + +func TestParseCartoAlgoConfig(t *testing.T) { + logger := golog.NewTestLogger(t) + + t.Run("returns default when config params are empty", func(t *testing.T) { + defaultCartoAlgoCfg := cartofacade.CartoAlgoConfig{ + OptimizeOnStart: false, + OptimizeEveryNNodes: 3, + NumRangeData: 30, + MissingDataRayLength: 25.0, + MaxRange: 25.0, + MinRange: 0.2, + MaxSubmapsToKeep: 3, + FreshSubmapsCount: 3, + MinCoveredArea: 1.0, + MinAddedSubmapsCount: 1, + OccupiedSpaceWeight: 20.0, + TranslationWeight: 10.0, + RotationWeight: 1.0, + } + + configParams := map[string]string{} + cartoAlgoConfig, err := parseCartoAlgoConfig(configParams, logger) + test.That(t, err, test.ShouldBeNil) + test.That(t, cartoAlgoConfig, test.ShouldResemble, defaultCartoAlgoCfg) + }) + + t.Run("returns overrides when config is non empty", func(t *testing.T) { + configParams := map[string]string{ + "optimize_on_start": "true", + "optimize_every_n_nodes": "1", + "num_range_data": "2", + "missing_data_ray_length": "3.0", + "max_range": "4.0", + "min_range": "5.0", + "max_submaps_to_keep": "6", + "fresh_submaps_count": "7", + "min_covered_area": "8.0", + "min_added_submaps_count": "9", + "occupied_space_weight": "10.0", + "translation_weight": "11.0", + "rotation_weight": "12.0", + } + + overRidenCartoAlgoCfg := cartofacade.CartoAlgoConfig{ + OptimizeOnStart: true, + OptimizeEveryNNodes: 1, + NumRangeData: 2, + MissingDataRayLength: 3.0, + MaxRange: 4.0, + MinRange: 5.0, + MaxSubmapsToKeep: 6, + FreshSubmapsCount: 7, + MinCoveredArea: 8.0, + MinAddedSubmapsCount: 9, + OccupiedSpaceWeight: 10.0, + TranslationWeight: 11.0, + RotationWeight: 12.0, + } + + cartoAlgoConfig, err := parseCartoAlgoConfig(configParams, logger) + test.That(t, err, test.ShouldBeNil) + test.That(t, cartoAlgoConfig, test.ShouldResemble, overRidenCartoAlgoCfg) + }) + + t.Run("returns error when unsupported param provided", func(t *testing.T) { + configParams := map[string]string{ + "optimize_on_start": "true", + "invalid_param": "hihi", + } + + _, err := parseCartoAlgoConfig(configParams, logger) + test.That(t, err, test.ShouldBeNil) + }) + + t.Run("returns error when param type is invalid", func(t *testing.T) { + configParams := map[string]string{ + "optimize_every_n_nodes": "hihi", + } + + cartoAlgoConfig, err := parseCartoAlgoConfig(configParams, logger) + test.That(t, err, test.ShouldBeError, errors.New("strconv.Atoi: parsing \"hihi\": invalid syntax")) + test.That(t, cartoAlgoConfig, test.ShouldResemble, defaultCartoAlgoCfg) + }) +} diff --git a/viam-cartographer_test.go b/viam-cartographer_test.go index f1ca0db8..11339e47 100644 --- a/viam-cartographer_test.go +++ b/viam-cartographer_test.go @@ -19,11 +19,15 @@ import ( "github.com/pkg/errors" viamgrpc "go.viam.com/rdk/grpc" "go.viam.com/test" + "go.viam.com/utils/artifact" "google.golang.org/grpc" + viamcartographer "github.com/viamrobotics/viam-cartographer" vcConfig "github.com/viamrobotics/viam-cartographer/config" - "github.com/viamrobotics/viam-cartographer/internal/testhelper" + "github.com/viamrobotics/viam-cartographer/dataprocess" + internaltesthelper "github.com/viamrobotics/viam-cartographer/internal/testhelper" "github.com/viamrobotics/viam-cartographer/sensors/lidar" + "github.com/viamrobotics/viam-cartographer/testhelper" ) const ( @@ -39,9 +43,42 @@ var ( _zeroTime = time.Time{} ) +func initTestCL(t *testing.T, logger golog.Logger) func() { + t.Helper() + err := viamcartographer.InitCartoLib(logger) + test.That(t, err, test.ShouldBeNil) + return func() { + err = viamcartographer.TerminateCartoLib() + test.That(t, err, test.ShouldBeNil) + } +} + +func initInternalState(t *testing.T) (string, func()) { + dataDirectory, err := os.MkdirTemp("", "*") + test.That(t, err, test.ShouldBeNil) + + internalStateDir := filepath.Join(dataDirectory, "internal_state") + err = os.Mkdir(internalStateDir, os.ModePerm) + test.That(t, err, test.ShouldBeNil) + + file := "viam-cartographer/outputs/viam-office-02-22-3/internal_state/internal_state_0.pbstream" + internalState, err := os.ReadFile(artifact.MustPath(file)) + test.That(t, err, test.ShouldBeNil) + + timestamp := time.Date(2006, 1, 2, 15, 4, 5, 999900000, time.UTC) + filename := dataprocess.CreateTimestampFilename(dataDirectory+"/internal_state", "internal_state", ".pbstream", timestamp) + err = os.WriteFile(filename, internalState, os.ModePerm) + test.That(t, err, test.ShouldBeNil) + + return dataDirectory, func() { + err := os.RemoveAll(dataDirectory) + test.That(t, err, test.ShouldBeNil) + } +} + func TestNew(t *testing.T) { logger := golog.NewTestLogger(t) - dataDir, err := testhelper.CreateTempFolderArchitecture(logger) + dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger) test.That(t, err, test.ShouldBeNil) t.Run("Successful creation of cartographer slam service with no sensor", func(t *testing.T) { @@ -55,7 +92,7 @@ func TestNew(t *testing.T) { UseLiveData: &_false, } - svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) test.That(t, err, test.ShouldBeNil) grpcServer.Stop() @@ -73,7 +110,7 @@ func TestNew(t *testing.T) { UseLiveData: &_false, } - _, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + _, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) 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]'")) @@ -90,7 +127,7 @@ func TestNew(t *testing.T) { UseLiveData: &_true, } - _, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + _, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) test.That(t, err, test.ShouldBeError, errors.New("configuring lidar camera error: error getting lidar camera "+ "gibberish for slam service: \"rdk:component:camera/gibberish\" missing from dependencies")) @@ -107,7 +144,7 @@ func TestNew(t *testing.T) { UseLiveData: &_true, } - svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) test.That(t, err, test.ShouldBeNil) grpcServer.Stop() @@ -124,12 +161,12 @@ func TestNew(t *testing.T) { UseLiveData: &_true, } - _, err = testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + _, err = internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) test.That(t, err, test.ShouldBeError, errors.New("getting and saving data failed: error getting data from sensor: invalid sensor")) }) - testhelper.ClearDirectory(t, dataDir) + internaltesthelper.ClearDirectory(t, dataDir) t.Run("Successful creation of cartographer slam service in localization mode", func(t *testing.T) { grpcServer, port := setupTestGRPCServer(t) @@ -143,7 +180,7 @@ func TestNew(t *testing.T) { MapRateSec: &_zeroInt, } - svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) test.That(t, err, test.ShouldBeNil) timestamp1, err := svc.GetLatestMapInfo(context.Background()) @@ -170,7 +207,7 @@ func TestNew(t *testing.T) { MapRateSec: &testMapRateSec, } - svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) test.That(t, err, test.ShouldBeNil) timestamp1, err := svc.GetLatestMapInfo(context.Background()) @@ -186,11 +223,183 @@ func TestNew(t *testing.T) { grpcServer.Stop() test.That(t, svc.Close(context.Background()), test.ShouldBeNil) }) + + t.Run("Fails to create cartographer slam service with no sensor when feature flag enabled", func(t *testing.T) { + termFunc := initTestCL(t, logger) + defer termFunc() + + dataDirectory, fsCleanupFunc := initInternalState(t) + defer fsCleanupFunc() + + attrCfg := &vcConfig.Config{ + ModularizationV2Enabled: &_true, + Sensors: []string{}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + UseLiveData: &_false, + } + + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + test.That(t, err, test.ShouldBeError, errors.New("error validating \"path\": \"at least one sensor must be configured\" is required")) + test.That(t, svc, test.ShouldBeNil) + }) + + t.Run("Failed creation of cartographer slam service with more than one sensor when feature flag enabled", func(t *testing.T) { + termFunc := initTestCL(t, logger) + defer termFunc() + + dataDirectory, fsCleanupFunc := initInternalState(t) + defer fsCleanupFunc() + + attrCfg := &vcConfig.Config{ + ModularizationV2Enabled: &_true, + Sensors: []string{"lidar", "one-too-many"}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + UseLiveData: &_false, + } + + _, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + 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]'")) + }) + + t.Run("Failed creation of cartographer slam service with non-existing sensor when feature flag enabled", func(t *testing.T) { + termFunc := initTestCL(t, logger) + defer termFunc() + + dataDirectory, fsCleanupFunc := initInternalState(t) + defer fsCleanupFunc() + + attrCfg := &vcConfig.Config{ + ModularizationV2Enabled: &_true, + Sensors: []string{"gibberish"}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + DataRateMsec: testDataRateMsec, + UseLiveData: &_true, + } + + _, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + test.That(t, err, test.ShouldBeError, + errors.New("configuring lidar camera error: error getting lidar camera "+ + "gibberish for slam service: \"rdk:component:camera/gibberish\" missing from dependencies")) + }) + + t.Run("Successful creation of cartographer slam service with good lidar when feature flag enabled", func(t *testing.T) { + termFunc := initTestCL(t, logger) + defer termFunc() + + dataDirectory, fsCleanupFunc := initInternalState(t) + defer fsCleanupFunc() + + attrCfg := &vcConfig.Config{ + ModularizationV2Enabled: &_true, + Sensors: []string{"good_lidar"}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + DataRateMsec: testDataRateMsec, + UseLiveData: &_true, + } + + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + 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 when feature flag enabled", func(t *testing.T) { + termFunc := initTestCL(t, logger) + defer termFunc() + + dataDirectory, fsCleanupFunc := initInternalState(t) + defer fsCleanupFunc() + + attrCfg := &vcConfig.Config{ + ModularizationV2Enabled: &_true, + Sensors: []string{"invalid_sensor"}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + DataRateMsec: testDataRateMsec, + UseLiveData: &_true, + } + + _, err = internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + test.That(t, err, test.ShouldBeError, + errors.New("failed to get data from lidar: ValidateGetData timeout: invalid sensor")) + }) + + t.Run("Successful creation of cartographer slam service in localization mode when feature flag enabled", func(t *testing.T) { + termFunc := initTestCL(t, logger) + defer termFunc() + + dataDirectory, fsCleanupFunc := initInternalState(t) + defer fsCleanupFunc() + + attrCfg := &vcConfig.Config{ + ModularizationV2Enabled: &_true, + Sensors: []string{"replay_sensor"}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + UseLiveData: &_true, + MapRateSec: &_zeroInt, + } + + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + test.That(t, err, test.ShouldBeNil) + + // TODO: Implement these + // timestamp1, err := svc.GetLatestMapInfo(context.Background()) + // test.That(t, err, test.ShouldBeNil) + // _, err = svc.GetPointCloudMap(context.Background()) + // test.That(t, err, test.ShouldBeNil) + // 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 when feature flag enabled", func(t *testing.T) { + termFunc := initTestCL(t, logger) + defer termFunc() + + dataDirectory, err := os.MkdirTemp("", "*") + test.That(t, err, test.ShouldBeNil) + + attrCfg := &vcConfig.Config{ + ModularizationV2Enabled: &_true, + Sensors: []string{"replay_sensor"}, + ConfigParams: map[string]string{"mode": "2d"}, + DataDirectory: dataDirectory, + UseLiveData: &_false, + MapRateSec: &testMapRateSec, + } + + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + test.That(t, err, test.ShouldBeNil) + + // TODO: Implement these + // timestamp1, err := svc.GetLatestMapInfo(context.Background()) + // test.That(t, err, test.ShouldBeNil) + // _, err = svc.GetPointCloudMap(context.Background()) + // test.That(t, err, test.ShouldBeNil) + // 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) + }) } func TestDataProcess(t *testing.T) { logger, obs := golog.NewObservedTestLogger(t) - dataDir, err := testhelper.CreateTempFolderArchitecture(logger) + dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger) test.That(t, err, test.ShouldBeNil) grpcServer, port := setupTestGRPCServer(t) @@ -203,16 +412,16 @@ func TestDataProcess(t *testing.T) { UseLiveData: &_true, } - svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) test.That(t, err, test.ShouldBeNil) grpcServer.Stop() test.That(t, svc.Close(context.Background()), test.ShouldBeNil) - slamSvc := svc.(testhelper.Service) + slamSvc := svc.(internaltesthelper.Service) t.Run("Successful startup of data process with good lidar", func(t *testing.T) { - defer testhelper.ClearDirectory(t, filepath.Join(dataDir, "data")) + defer internaltesthelper.ClearDirectory(t, filepath.Join(dataDir, "data")) sensors := []string{"good_lidar"} lidar, err := lidar.New(testhelper.SetupDeps(sensors), sensors, 0) @@ -247,7 +456,7 @@ func TestDataProcess(t *testing.T) { }) t.Run("When replay sensor is configured, we read timestamps from the context", func(t *testing.T) { - defer testhelper.ClearDirectory(t, filepath.Join(dataDir, "data")) + defer internaltesthelper.ClearDirectory(t, filepath.Join(dataDir, "data")) sensors := []string{"replay_sensor"} lidar, err := lidar.New(testhelper.SetupDeps(sensors), sensors, 0) @@ -267,12 +476,12 @@ func TestDataProcess(t *testing.T) { test.That(t, svc.Close(context.Background()), test.ShouldBeNil) - testhelper.ClearDirectory(t, dataDir) + internaltesthelper.ClearDirectory(t, dataDir) } func TestEndpointFailures(t *testing.T) { logger := golog.NewTestLogger(t) - dataDir, err := testhelper.CreateTempFolderArchitecture(logger) + dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger) test.That(t, err, test.ShouldBeNil) grpcServer, port := setupTestGRPCServer(t) @@ -286,7 +495,7 @@ func TestEndpointFailures(t *testing.T) { UseLiveData: &_true, } - svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) test.That(t, err, test.ShouldBeNil) pNew, frame, err := svc.GetPosition(context.Background()) @@ -311,12 +520,12 @@ func TestEndpointFailures(t *testing.T) { grpcServer.Stop() test.That(t, svc.Close(context.Background()), test.ShouldBeNil) - testhelper.ClearDirectory(t, dataDir) + internaltesthelper.ClearDirectory(t, dataDir) } func TestSLAMProcess(t *testing.T) { logger := golog.NewTestLogger(t) - dataDir, err := testhelper.CreateTempFolderArchitecture(logger) + dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger) test.That(t, err, test.ShouldBeNil) t.Run("Successful start of live SLAM process with default parameters", func(t *testing.T) { @@ -329,10 +538,10 @@ func TestSLAMProcess(t *testing.T) { UseLiveData: &_true, } - svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) test.That(t, err, test.ShouldBeNil) - slamSvc := svc.(testhelper.Service) + slamSvc := svc.(internaltesthelper.Service) processCfg := slamSvc.GetSLAMProcessConfig() cmd := append([]string{processCfg.Name}, processCfg.Args...) @@ -369,10 +578,10 @@ func TestSLAMProcess(t *testing.T) { UseLiveData: &_false, } - svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) test.That(t, err, test.ShouldBeNil) - slamSvc := svc.(testhelper.Service) + slamSvc := svc.(internaltesthelper.Service) processCfg := slamSvc.GetSLAMProcessConfig() cmd := append([]string{processCfg.Name}, processCfg.Args...) @@ -410,17 +619,17 @@ func TestSLAMProcess(t *testing.T) { Port: "localhost:" + strconv.Itoa(port), UseLiveData: &_true, } - _, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, "hokus_pokus_does_not_exist_filename") + _, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, "hokus_pokus_does_not_exist_filename") test.That(t, fmt.Sprint(err), test.ShouldContainSubstring, "executable file not found in $PATH") grpcServer.Stop() }) - testhelper.ClearDirectory(t, dataDir) + internaltesthelper.ClearDirectory(t, dataDir) } func TestDoCommand(t *testing.T) { logger := golog.NewTestLogger(t) - dataDir, err := testhelper.CreateTempFolderArchitecture(logger) + dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger) test.That(t, err, test.ShouldBeNil) grpcServer, port := setupTestGRPCServer(t) attrCfg := &vcConfig.Config{ @@ -432,7 +641,7 @@ func TestDoCommand(t *testing.T) { Port: "localhost:" + strconv.Itoa(port), UseLiveData: &_true, } - svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) + svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName) test.That(t, err, test.ShouldBeNil) t.Run("returns UnimplementedError when given other parmeters", func(t *testing.T) { cmd := map[string]interface{}{"fake_flag": true} @@ -448,7 +657,7 @@ func TestDoCommand(t *testing.T) { }) grpcServer.Stop() test.That(t, svc.Close(context.Background()), test.ShouldBeNil) - testhelper.ClearDirectory(t, dataDir) + internaltesthelper.ClearDirectory(t, dataDir) } // SetupTestGRPCServer sets up and starts a grpc server.