diff --git a/Makefile b/Makefile index f0ead89bc..efff31d3f 100644 --- a/Makefile +++ b/Makefile @@ -85,7 +85,7 @@ setup: install-dependencies ensure-submodule-initialized artifact-pull install-dependencies: ifneq (, $(shell which brew)) brew update - brew install abseil boost viamrobotics/brews/ceres-solver@2.1 protobuf ninja cairo googletest lua@5.3 pkg-config cmake go@1.20 grpc clang-format + brew install abseil boost viamrobotics/brews/suite-sparse@7.1 viamrobotics/brews/ceres-solver@2.1 protobuf ninja cairo googletest lua@5.3 pkg-config cmake go@1.20 grpc clang-format brew link lua@5.3 brew install openssl@3 eigen gflags glog sphinx-doc pcl viamrobotics/brews/nlopt-static else ifneq (, $(shell which apt-get)) diff --git a/cartofacade/capi.go b/cartofacade/capi.go index bfe1cafd4..9e709f138 100644 --- a/cartofacade/capi.go +++ b/cartofacade/capi.go @@ -95,14 +95,11 @@ const ( type CartoConfig struct { Camera string MovementSensor string - MapRateSecond int - DataDir string ComponentReference string LidarConfig LidarConfig - CloudStoryEnabled bool - EnableMapping bool - ExistingMap string + EnableMapping bool + ExistingMap string } // CartoAlgoConfig contains config values from app @@ -392,17 +389,6 @@ func getConfig(cfg CartoConfig) (C.viam_carto_config, error) { return C.viam_carto_config{}, err } - // Remove cloud_story_enabled, map_rate_sec, and data_dir from C++ code - // JIRA Ticket: RSDK-52334 https://viam.atlassian.net/browse/RSDK-5334 - vcc.cloud_story_enabled = C.bool(true) - vcc.data_dir = goStringToBstring("/tmp/") - if cfg.EnableMapping { - // Set to arbitrarily high value to ensure no maps get saved during operation - vcc.map_rate_sec = C.int(9000) - } else { - vcc.map_rate_sec = C.int(0) - } - vcc.lidar_config = lidarCfg vcc.enable_mapping = C.bool(cfg.EnableMapping) @@ -519,20 +505,16 @@ func toError(status C.int) error { return errors.New("VIAM_CARTO_LIB_NOT_INITIALIZED") case C.VIAM_CARTO_UNKNOWN_ERROR: return errors.New("VIAM_CARTO_UNKNOWN_ERROR") - case C.VIAM_CARTO_DATA_DIR_NOT_PROVIDED: - return errors.New("VIAM_CARTO_DATA_DIR_NOT_PROVIDED") case C.VIAM_CARTO_SLAM_MODE_INVALID: return errors.New("VIAM_CARTO_SLAM_MODE_INVALID") case C.VIAM_CARTO_LIDAR_CONFIG_INVALID: return errors.New("VIAM_CARTO_LIDAR_CONFIG_INVALID") - case C.VIAM_CARTO_MAP_RATE_SEC_INVALID: - return errors.New("VIAM_CARTO_MAP_RATE_SEC_INVALID") case C.VIAM_CARTO_COMPONENT_REFERENCE_INVALID: return errors.New("VIAM_CARTO_COMPONENT_REFERENCE_INVALID") case C.VIAM_CARTO_LUA_CONFIG_NOT_FOUND: return errors.New("VIAM_CARTO_LUA_CONFIG_NOT_FOUND") - case C.VIAM_CARTO_DATA_DIR_INVALID_DEPRECATED_STRUCTURE: - return errors.New("VIAM_CARTO_DATA_DIR_INVALID_DEPRECATED_STRUCTURE") + case C.VIAM_CARTO_INTERNAL_STATE_FILE_SYSTEM_ERROR: + return errors.New("VIAM_CARTO_INTERNAL_STATE_FILE_SYSTEM_ERROR") case C.VIAM_CARTO_MAP_CREATION_ERROR: return errors.New("VIAM_CARTO_MAP_CREATION_ERROR") case C.VIAM_CARTO_UNKNOWN_SENSOR_NAME: @@ -567,6 +549,8 @@ func toError(status C.int) error { return errors.New("VIAM_CARTO_NOT_IN_TERMINATABLE_STATE") case C.VIAM_CARTO_IMU_PROVIDED_AND_IMU_ENABLED_MISMATCH: return errors.New("VIAM_CARTO_IMU_PROVIDED_AND_IMU_ENABLED_MISMATCH") + case C.VIAM_CARTO_IMU_READING_EMPTY: + return errors.New("VIAM_CARTO_IMU_READING_EMPTY") case C.VIAM_CARTO_IMU_READING_INVALID: return errors.New("VIAM_CARTO_IMU_READING_INVALID") case C.VIAM_CARTO_ODOMETER_READING_INVALID: diff --git a/config/config.go b/config/config.go index 9840391d2..7ecbd57d9 100644 --- a/config/config.go +++ b/config/config.go @@ -31,14 +31,14 @@ type OptionalConfigParams struct { LidarDataFrequencyHz int MovementSensorName string MovementSensorDataFrequencyHz int - MapRateSec int EnableMapping bool ExistingMap string } var ( errCameraMustHaveName = errors.New("\"camera[name]\" is required") - errLocalizationInOfflineMode = newError("camera[data_freq_hz] and enable_mapping = false. localization in offline mode not supported.") + errLocalizationInOfflineMode = newError("\"camera[data_freq_hz]\" and enable_mapping = false." + + " Localization in offline mode is not supported.") ) // Validate creates the list of implicit dependencies. @@ -65,7 +65,8 @@ func (config *Config) Validate(path string) ([]string, error) { deps = append(deps, movementSensorName) } - if config.ConfigParams["mode"] == "" { + mode, ok := config.ConfigParams["mode"] + if !ok || mode == "" { return nil, utils.NewConfigValidationFieldRequiredError(path, "config_params[mode]") } diff --git a/config/config_test.go b/config/config_test.go index 850ca1897..0b149f9ca 100644 --- a/config/config_test.go +++ b/config/config_test.go @@ -126,7 +126,6 @@ func TestGetOptionalParameters(t *testing.T) { test.That(t, optionalConfigParams.MovementSensorName, test.ShouldEqual, "") test.That(t, optionalConfigParams.MovementSensorDataFrequencyHz, test.ShouldEqual, 0) test.That(t, optionalConfigParams.EnableMapping, test.ShouldBeFalse) - test.That(t, optionalConfigParams.MapRateSec, test.ShouldEqual, 0) test.That(t, optionalConfigParams.ExistingMap, test.ShouldEqual, "") }) @@ -150,7 +149,6 @@ func TestGetOptionalParameters(t *testing.T) { test.That(t, optionalConfigParams.MovementSensorName, test.ShouldEqual, "") test.That(t, optionalConfigParams.MovementSensorDataFrequencyHz, test.ShouldEqual, 0) test.That(t, optionalConfigParams.EnableMapping, test.ShouldBeFalse) - test.That(t, optionalConfigParams.MapRateSec, test.ShouldEqual, 0) test.That(t, optionalConfigParams.ExistingMap, test.ShouldEqual, "") }) diff --git a/etc/packaging/appimages/cartographer-module-aarch64.yml b/etc/packaging/appimages/cartographer-module-aarch64.yml index aa2981591..46c30cef8 100644 --- a/etc/packaging/appimages/cartographer-module-aarch64.yml +++ b/etc/packaging/appimages/cartographer-module-aarch64.yml @@ -39,6 +39,7 @@ AppDir: - libboost-filesystem1.74.0 - libpcl-io1.13 - libnlopt0 + - libxcb-shm0 files: include: [] diff --git a/etc/packaging/appimages/cartographer-module-x86_64.yml b/etc/packaging/appimages/cartographer-module-x86_64.yml index b7a4d8afc..8ff7d6f2d 100644 --- a/etc/packaging/appimages/cartographer-module-x86_64.yml +++ b/etc/packaging/appimages/cartographer-module-x86_64.yml @@ -39,7 +39,8 @@ AppDir: - libboost-filesystem1.74.0 - libpcl-io1.13 - libnlopt0 - + - libxcb-shm0 + files: include: [] exclude: diff --git a/integration_test.go b/integration_test.go index b6cdf1fec..c6d39ba0e 100644 --- a/integration_test.go +++ b/integration_test.go @@ -117,7 +117,7 @@ func TestIntegrationCartographer(t *testing.T) { test.That(t, err, test.ShouldBeNil) }() - // Set mapRateSec for mapping mode + // Enable mapping mode enableMapping := true // Run mapping test diff --git a/postprocess/postprocess.go b/postprocess/postprocess.go new file mode 100644 index 000000000..ff4bae405 --- /dev/null +++ b/postprocess/postprocess.go @@ -0,0 +1,233 @@ +// Package postprocess contains functionality to postprocess pointcloud maps +package postprocess + +import ( + "bytes" + "errors" + "image/color" + "math" + + "github.com/golang/geo/r3" + "go.viam.com/rdk/pointcloud" +) + +// Instruction describes the action of the postprocess step. +type Instruction int + +const ( + // Add is the instruction for adding points. + Add Instruction = iota + // Remove is the instruction for removing points. + Remove = iota +) + +const ( + fullConfidence = 100 + removalRadius = 100 // mm + xKey = "X" + yKey = "Y" + + // ToggleCommand can be used to turn postprocessing on and off. + ToggleCommand = "postprocess_toggle" + // AddCommand can be used to add points to the pointcloud map. + AddCommand = "postprocess_add" + // RemoveCommand can be used to remove points from the pointcloud map. + RemoveCommand = "postprocess_remove" + // UndoCommand can be used to undo last postprocessing step. + UndoCommand = "postprocess_undo" + // PathCommand can be used to specify a pcd that has already been postprocessed. + PathCommand = "postprocess_path" +) + +var ( + errPointsNotASlice = errors.New("could not parse provided points as a slice") + errPointNotAMap = errors.New("could not parse provided point as a map") + errXNotProvided = errors.New("could X not provided") + errXNotFloat64 = errors.New("could not parse provided X as a float64") + errYNotProvided = errors.New("could X not provided") + errYNotFloat64 = errors.New("could not parse provided X as a float64") + errRemovingPoints = errors.New("unexpected number of points after removal") + errNilUpdatedData = errors.New("cannot provide nil updated data") +) + +// Task can be used to construct a postprocessing step. +type Task struct { + Instruction Instruction + Points []r3.Vector +} + +// ParseDoCommand parses postprocessing DoCommands into Tasks. +func ParseDoCommand( + unstructuredPoints interface{}, + instruction Instruction, +) (Task, error) { + pointSlice, ok := unstructuredPoints.([]interface{}) + if !ok { + return Task{}, errPointsNotASlice + } + + task := Task{Instruction: instruction} + for _, point := range pointSlice { + pointMap, ok := point.(map[string]interface{}) + if !ok { + return Task{}, errPointNotAMap + } + + x, ok := pointMap[xKey] + if !ok { + return Task{}, errXNotProvided + } + + xFloat, ok := x.(float64) + if !ok { + return Task{}, errXNotFloat64 + } + + y, ok := pointMap[yKey] + if !ok { + return Task{}, errYNotProvided + } + + yFloat, ok := y.(float64) + if !ok { + return Task{}, errXNotFloat64 + } + + task.Points = append(task.Points, r3.Vector{X: xFloat, Y: yFloat}) + } + return task, nil +} + +/* +UpdatePointCloud iterated through a list of tasks and adds or removes points from data +and writes the updated pointcloud to updatedData. +*/ +func UpdatePointCloud( + data []byte, + updatedData *[]byte, + tasks []Task, +) error { + if updatedData == nil { + return errNilUpdatedData + } + + *updatedData = append(*updatedData, data...) + + // iterate through tasks and add or remove points + for _, task := range tasks { + switch task.Instruction { + case Add: + err := updatePointCloudWithAddedPoints(updatedData, task.Points) + if err != nil { + return err + } + case Remove: + err := updatePointCloudWithRemovedPoints(updatedData, task.Points) + if err != nil { + return err + } + } + } + return nil +} + +func updatePointCloudWithAddedPoints(updatedData *[]byte, points []r3.Vector) error { + if updatedData == nil { + return errNilUpdatedData + } + + reader := bytes.NewReader(*updatedData) + pc, err := pointcloud.ReadPCD(reader) + if err != nil { + return err + } + + for _, point := range points { + /* + Viam expects pointcloud data with fields "x y z" or "x y z rgb", and for + this to be specified in the pointcloud header in the FIELDS entry. If color + data is included in the pointcloud, Viam's services assume that the color + value encodes a confidence score for that data point. Viam expects the + confidence score to be encoded in the blue parameter of the RGB value, on a + scale from 1-100. + */ + err := pc.Set(point, pointcloud.NewColoredData(color.NRGBA{B: fullConfidence, R: math.MaxUint8})) + if err != nil { + return err + } + } + + var buf bytes.Buffer + err = pointcloud.ToPCD(pc, &buf, pointcloud.PCDBinary) + if err != nil { + return err + } + + // Initialize updatedData with new points + *updatedData = make([]byte, buf.Len()) + updatedReader := bytes.NewReader(buf.Bytes()) + _, err = updatedReader.Read(*updatedData) + if err != nil { + return err + } + + return nil +} + +func updatePointCloudWithRemovedPoints(updatedData *[]byte, points []r3.Vector) error { + if updatedData == nil { + return errNilUpdatedData + } + + reader := bytes.NewReader(*updatedData) + pc, err := pointcloud.ReadPCD(reader) + if err != nil { + return err + } + + updatedPC := pointcloud.NewWithPrealloc(pc.Size() - len(points)) + pointsVisited := 0 + + filterRemovedPoints := func(p r3.Vector, d pointcloud.Data) bool { + pointsVisited++ + // Always return true so iteration continues + + for _, point := range points { + // remove all points within the removalRadius from the removed points + if point.Distance(p) <= removalRadius { + return true + } + } + + err := updatedPC.Set(p, d) + // end early if point cannot be set + return err == nil + } + + pc.Iterate(0, 0, filterRemovedPoints) + + // confirm iterate did not have to end early + if pc.Size() != pointsVisited { + /* + Note: this condition will occur if some error occurred while copying valid points + and will be how we can tell that this error occurred: err := updatedPC.Set(p, d) + */ + return errRemovingPoints + } + + buf := bytes.Buffer{} + err = pointcloud.ToPCD(updatedPC, &buf, pointcloud.PCDBinary) + if err != nil { + return err + } + + // Overwrite updatedData with new points + *updatedData = make([]byte, buf.Len()) + updatedReader := bytes.NewReader(buf.Bytes()) + _, err = updatedReader.Read(*updatedData) + if err != nil { + return err + } + + return nil +} diff --git a/postprocess/postprocess_test.go b/postprocess/postprocess_test.go new file mode 100644 index 000000000..318d0eba4 --- /dev/null +++ b/postprocess/postprocess_test.go @@ -0,0 +1,175 @@ +package postprocess + +import ( + "bytes" + "errors" + "fmt" + "image/color" + "math" + "testing" + + "github.com/golang/geo/r3" + "go.viam.com/rdk/pointcloud" + "go.viam.com/test" +) + +type TestCase struct { + msg string + cmd interface{} + err error +} + +func TestParseDoCommand(t *testing.T) { + for _, tc := range []TestCase{ + { + msg: "errors if unstructuredPoints is not a slice", + cmd: "hello", + err: errPointsNotASlice, + }, + { + msg: "errors if unstructuredPoints is not a slice of maps", + cmd: []interface{}{1}, + err: errPointNotAMap, + }, + { + msg: "errors if unstructuredPoints contains a point where X is not float64", + cmd: []interface{}{map[string]interface{}{"Y": float64(2)}}, + err: errXNotProvided, + }, + { + msg: "errors if unstructuredPoints contains a point where X is not float64", + cmd: []interface{}{map[string]interface{}{"X": 1, "Y": float64(2)}}, + err: errXNotFloat64, + }, + { + msg: "errors if unstructuredPoints contains a point where Y is not provided", + cmd: []interface{}{map[string]interface{}{"X": float64(1)}}, + err: errYNotProvided, + }, + { + msg: "errors if unstructuredPoints contains a point where Y is not float64", + cmd: []interface{}{map[string]interface{}{"X": float64(1), "Y": 2}}, + err: errYNotFloat64, + }, + } { + t.Run(fmt.Sprintf("%s for Add task", tc.msg), func(t *testing.T) { + task, err := ParseDoCommand(tc.cmd, Add) + test.That(t, err, test.ShouldBeError, tc.err) + test.That(t, task, test.ShouldResemble, Task{}) + }) + + t.Run(fmt.Sprintf("%s for Remove task", tc.msg), func(t *testing.T) { + task, err := ParseDoCommand(tc.cmd, Remove) + test.That(t, err, test.ShouldBeError, tc.err) + test.That(t, task, test.ShouldResemble, Task{}) + }) + } + + t.Run("succeeds if unstructuredPoints is a slice of maps with float64 values", func(t *testing.T) { + expectedPoint := r3.Vector{X: 1, Y: 2} + task, err := ParseDoCommand([]interface{}{map[string]interface{}{"X": float64(1), "Y": float64(2)}}, Add) + test.That(t, err, test.ShouldBeNil) + test.That(t, task, test.ShouldResemble, Task{Instruction: Add, Points: []r3.Vector{expectedPoint}}) + }) +} + +func TestUpdatePointCloudWithAddedPoints(t *testing.T) { + t.Run("errors if byte slice cannot be converted to PCD", func(t *testing.T) { + originalPointsBytes := []byte("hello") + err := updatePointCloudWithAddedPoints(&originalPointsBytes, []r3.Vector{{X: 2, Y: 2}, {X: 3, Y: 3}}) + test.That(t, err, test.ShouldBeError, errors.New("error reading header line 0: EOF")) + }) + + t.Run("successfully returns point cloud with postprocessed points", func(t *testing.T) { + originalPoints := []r3.Vector{{X: 0, Y: 0}, {X: 1, Y: 1}} + var originalPointsBytes []byte + err := vecSliceToBytes(originalPoints, &originalPointsBytes) + test.That(t, err, test.ShouldBeNil) + + postprocessedPoints := []r3.Vector{{X: 0, Y: 0}, {X: 1, Y: 1}, {X: 2, Y: 2}, {X: 3, Y: 3}} + var postprocessedPointsBytes []byte + err = vecSliceToBytes(postprocessedPoints, &postprocessedPointsBytes) + test.That(t, err, test.ShouldBeNil) + + // update original byte slice with new points + err = updatePointCloudWithAddedPoints(&originalPointsBytes, []r3.Vector{{X: 2, Y: 2}, {X: 3, Y: 3}}) + test.That(t, err, test.ShouldBeNil) + test.That(t, postprocessedPointsBytes, test.ShouldResemble, originalPointsBytes) + }) +} + +func TestUpdatePointCloudWithRemovedPoints(t *testing.T) { + t.Run("errors if byte slice cannot be converted to PCD", func(t *testing.T) { + originalPointsBytes := []byte("hello") + err := updatePointCloudWithRemovedPoints(&originalPointsBytes, []r3.Vector{{X: 2, Y: 2}, {X: 3, Y: 3}}) + test.That(t, err, test.ShouldBeError, errors.New("error reading header line 0: EOF")) + }) + + t.Run("successfully returns point cloud with postprocessed points", func(t *testing.T) { + originalPoints := []r3.Vector{{X: 0, Y: 0}, {X: 1000, Y: 1000}, {X: 2000, Y: 2000}, {X: 2020, Y: 2020}, {X: 3000, Y: 3000}} + var originalPointsBytes []byte + err := vecSliceToBytes(originalPoints, &originalPointsBytes) + test.That(t, err, test.ShouldBeNil) + + postprocessedPoints := []r3.Vector{{X: 0, Y: 0}, {X: 1000, Y: 1000}} + var postprocessedPointsBytes []byte + err = vecSliceToBytes(postprocessedPoints, &postprocessedPointsBytes) + test.That(t, err, test.ShouldBeNil) + + // update original byte slice with new points + err = updatePointCloudWithRemovedPoints(&originalPointsBytes, []r3.Vector{{X: 2000, Y: 2000}, {X: 3000, Y: 3000}}) + test.That(t, err, test.ShouldBeNil) + test.That(t, postprocessedPointsBytes, test.ShouldResemble, originalPointsBytes) + }) +} + +func TestUpdatePointCloud(t *testing.T) { + originalPoints := []r3.Vector{{X: 0, Y: 0}, {X: 1000, Y: 1000}, {X: 2000, Y: 2000}, {X: 3000, Y: 3000}} + var originalPointsBytes []byte + err := vecSliceToBytes(originalPoints, &originalPointsBytes) + test.That(t, err, test.ShouldBeNil) + + postprocessedPoints := []r3.Vector{ + {X: 0, Y: 0}, + {X: 1000, Y: 1000}, + {X: 3000, Y: 3000}, + {X: 5000, Y: 5000}, + } + var postprocessedPointsBytes []byte + err = vecSliceToBytes(postprocessedPoints, &postprocessedPointsBytes) + test.That(t, err, test.ShouldBeNil) + + tasks := []Task{ + { + Instruction: Add, + Points: []r3.Vector{{X: 4000, Y: 4000}, {X: 5000, Y: 5000}}, + }, + { + Instruction: Remove, + Points: []r3.Vector{{X: 2000, Y: 2000}, {X: 4000, Y: 4000}}, + }, + } + var updatedData []byte + err = UpdatePointCloud(originalPointsBytes, &updatedData, tasks) + test.That(t, err, test.ShouldBeNil) + test.That(t, updatedData, test.ShouldResemble, postprocessedPointsBytes) +} + +func vecSliceToBytes(points []r3.Vector, outputData *[]byte) error { + pc := pointcloud.NewWithPrealloc(len(points)) + for _, p := range points { + pc.Set(p, pointcloud.NewColoredData(color.NRGBA{B: fullConfidence, R: math.MaxUint8})) + } + + buf := bytes.Buffer{} + err := pointcloud.ToPCD(pc, &buf, pointcloud.PCDBinary) + if err != nil { + return err + } + + // Initialize updatedData with new points + *outputData = make([]byte, buf.Len()) + updatedReader := bytes.NewReader(buf.Bytes()) + updatedReader.Read(*outputData) + return nil +} diff --git a/sensorprocess/lidarsensorprocess_test.go b/sensorprocess/lidarsensorprocess_test.go new file mode 100644 index 000000000..570a00088 --- /dev/null +++ b/sensorprocess/lidarsensorprocess_test.go @@ -0,0 +1,303 @@ +package sensorprocess + +import ( + "context" + "errors" + "testing" + "time" + + "go.viam.com/rdk/logging" + "go.viam.com/test" + + "github.com/viamrobotics/viam-cartographer/cartofacade" + s "github.com/viamrobotics/viam-cartographer/sensors" + "github.com/viamrobotics/viam-cartographer/sensors/inject" +) + +func TestStartLidar(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: true, + Timeout: 10 * time.Second, + } + + t.Run("exits loop when the context was cancelled", func(t *testing.T) { + cancelCtx, cancelFunc := context.WithCancel(context.Background()) + + lidar, imu := s.FinishedReplayLidar, s.NoMovementSensor + replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), 5, logger) + test.That(t, err, test.ShouldBeNil) + + config.Lidar = replaySensor + + cancelFunc() + + config.StartLidar(cancelCtx) + }) +} + +func TestAddLidarReadingInOnline(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + + dataFrequencyHz := 5 + injectLidar := inject.TimedLidar{} + injectLidar.NameFunc = func() string { return "good_lidar" } + injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + Timeout: 10 * time.Second, + } + + t.Run("returns error when lidar GetData returns error, doesn't try to add lidar data", func(t *testing.T) { + invalidAddLidarReadingInOnlineTestHelper(context.Background(), t, cf, config, s.LidarWithErroringFunctions, 10) + }) + + t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { + invalidAddLidarReadingInOnlineTestHelper(context.Background(), t, cf, config, s.InvalidReplayLidar, 10) + }) + + t.Run("online lidar adds sensor reading once and ignores errors", func(t *testing.T) { + validAddLidarReadingInOnlineTestHelper(context.Background(), t, config, cf, s.GoodLidar) + }) +} + +func TestTryAddLidarReadingUntilSuccess(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + cf.RunFinalOptimizationFunc = func(context.Context, time.Duration) error { + return nil + } + + dataFrequencyHz := 0 + + lidarReading := s.TimedLidarReadingResponse{ + Reading: []byte("12345"), + ReadingTime: time.Now().UTC(), + } + + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: false, + Lidar: &injectLidar, + Timeout: 10 * time.Second, + } + + t.Run("replay lidar adds sensor data until success", func(t *testing.T) { + lidar, imu := s.ReplayLidar, s.NoMovementSensor + replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) + test.That(t, err, test.ShouldBeNil) + + var calls []addLidarReadingArgs + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + args := addLidarReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + calls = append(calls, args) + if len(calls) == 1 { + return errUnknown + } + if len(calls) == 2 { + return cartofacade.ErrUnableToAcquireLock + } + return nil + } + config.Lidar = replaySensor + + config.tryAddLidarReadingUntilSuccess(context.Background(), lidarReading) + test.That(t, len(calls), test.ShouldEqual, 3) + + firstTimestamp := calls[0].currentReading.ReadingTime + for i, call := range calls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, string(lidar)) + test.That(t, call.currentReading.Reading, test.ShouldResemble, lidarReading.Reading) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + } + }) +} + +func TestTryAddLidarReadingOnce(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + reading := s.TimedLidarReadingResponse{ + Reading: []byte("12345"), + ReadingTime: time.Now().UTC(), + } + + dataFrequencyHz := 5 + injectLidar := inject.TimedLidar{} + injectLidar.NameFunc = func() string { return "good_lidar" } + injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + Timeout: 10 * time.Second, + } + t.Run("when AddLidarReading blocks for more than the data rate and succeeds, time to sleep is 0", func(t *testing.T) { + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return nil + } + + timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddLidarReading is slower than data rate and returns a lock error, time to sleep is 0", func(t *testing.T) { + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddLidarReading blocks for more than the date rate "+ + "and returns an unexpected error, time to sleep is 0", func(t *testing.T) { + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return errUnknown + } + + timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddLidarReading is faster than the date rate and succeeds, time to sleep is <= date rate", func(t *testing.T) { + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + return nil + } + + timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.Lidar.DataFrequencyHz()) + }) + + t.Run("when AddLidarReading is faster than the date rate "+ + "and returns lock error, time to sleep is <= date rate", func(t *testing.T) { + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.Lidar.DataFrequencyHz()) + }) + + t.Run("when AddLidarReading is faster than date rate "+ + "and returns an unexpected error, time to sleep is <= date rate", func(t *testing.T) { + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + return errUnknown + } + + timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.Lidar.DataFrequencyHz()) + }) +} + +func TestTryAddLidarReading(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + reading := s.TimedLidarReadingResponse{ + Reading: []byte("12345"), + ReadingTime: time.Now().UTC(), + } + + dataFrequencyHz := 5 + injectLidar := inject.TimedLidar{} + injectLidar.NameFunc = func() string { return "good_lidar" } + injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + Timeout: 10 * time.Second, + } + t.Run("return error when AddLidarReading errors out", func(t *testing.T) { + expectedErr := errors.New("failed to get lidar reading") + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + return expectedErr + } + + err := config.tryAddLidarReading(context.Background(), reading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, expectedErr) + }) + + t.Run("succeeds when AddLidarReading succeeds", func(t *testing.T) { + cf.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + return nil + } + + err := config.tryAddLidarReading(context.Background(), reading) + test.That(t, err, test.ShouldBeNil) + }) +} diff --git a/sensorprocess/movementsensorprocess.go b/sensorprocess/movementsensorprocess.go index ee42e6195..c731b12c5 100644 --- a/sensorprocess/movementsensorprocess.go +++ b/sensorprocess/movementsensorprocess.go @@ -13,81 +13,118 @@ import ( s "github.com/viamrobotics/viam-cartographer/sensors" ) -// StartIMU polls the IMU to get the next sensor reading and adds it to the cartofacade. -// Stops when the context is Done. -func (config *Config) StartIMU(ctx context.Context) { +// StartMovementSensor polls the movement sensor to get the next sensor reading +// and adds it to the cartofacade. Stops when the context is Done. +func (config *Config) StartMovementSensor(ctx context.Context) { for { select { case <-ctx.Done(): return default: - if err := config.addIMUReadingInOnline(ctx); err != nil { + if err := config.addMovementSensorReadingInOnline(ctx); err != nil { config.Logger.Warn(err) } } } } -// addIMUReadingInOnline ensures the most recent IMU scan, after corresponding lidar scans, gets processed by cartographer. -func (config *Config) addIMUReadingInOnline(ctx context.Context) error { - // get next IMU data response; ignoring status since it is always false - imuReading, err := config.IMU.TimedMovementSensorReading(ctx) +// addMovementSensorReadingInOnline attempts to get and add a movement sensor reading to the +// cartofacade. +func (config *Config) addMovementSensorReadingInOnline(ctx context.Context) error { + // get next movement sensor data response; ignoring status since it is always false + movementSensorReading, err := config.MovementSensor.TimedMovementSensorReading(ctx) if err != nil { - config.Logger.Warn(err) if errors.Is(err, replaymovementsensor.ErrEndOfDataset) { time.Sleep(1 * time.Second) } return err } - // add IMU data to cartographer and sleep remainder of time interval - timeToSleep := config.tryAddIMUReadingOnce(ctx, *imuReading.TimedIMUResponse) + // add movement sensor data to cartographer and sleep remainder of time interval + timeToSleep := config.tryAddMovementSensorReadingOnce(ctx, movementSensorReading) - if !imuReading.TestIsReplaySensor { + if !movementSensorReading.TestIsReplaySensor { time.Sleep(time.Duration(timeToSleep) * time.Millisecond) - config.Logger.Debugf("imu sleep for %vms", timeToSleep) + config.Logger.Debugf("movement sensor sleep for %vms", timeToSleep) } return nil } -// tryAddIMUReadingUntilSuccess adds a reading to the cartofacade and retries on error (offline mode). +// tryAddMovementSensorReadingUntilSuccess adds a reading to the cartofacade and retries on error (offline mode). // While add sensor reading fails, keep trying to add the same reading - in offline mode we want to // process each reading so if we cannot acquire the lock we should try again. -func (config *Config) tryAddIMUReadingUntilSuccess(ctx context.Context, reading s.TimedIMUReadingResponse) error { +func (config *Config) tryAddMovementSensorReadingUntilSuccess(ctx context.Context, reading s.TimedMovementSensorReadingResponse) error { + var imuDone, odometerDone bool + // set IMU as done since it is not supported: we won't attempt to add IMU data to cartographer + if !config.MovementSensor.Properties().IMUSupported { + imuDone = true + } + // set odometer as done since it is not supported: we won't attempt to add odometer data to cartographer + if !config.MovementSensor.Properties().OdometerSupported { + odometerDone = true + } for { select { case <-ctx.Done(): return ctx.Err() default: - if err := config.tryAddIMUReading(ctx, reading); err != nil { - if !errors.Is(err, cartofacade.ErrUnableToAcquireLock) { - config.Logger.Warnw("Retrying sensor reading due to error from cartofacade", "error", err) + if !odometerDone { + if err := config.tryAddOdometerReading(ctx, *reading.TimedOdometerResponse); err != nil { + if !errors.Is(err, cartofacade.ErrUnableToAcquireLock) { + config.Logger.Warnw("Retrying odometer sensor reading due to error from cartofacade", "error", err) + } + } else { + odometerDone = true } - } else { + } + if !imuDone { + if err := config.tryAddIMUReading(ctx, *reading.TimedIMUResponse); err != nil { + if !errors.Is(err, cartofacade.ErrUnableToAcquireLock) { + config.Logger.Warnw("Retrying IMU sensor reading due to error from cartofacade", "error", err) + } + } else { + imuDone = true + } + } + if imuDone && odometerDone { return nil } } } } -// tryAddIMUReadingOnce adds a reading to the carto facade and does not retry. Returns remainder of time interval. -func (config *Config) tryAddIMUReadingOnce(ctx context.Context, reading s.TimedIMUReadingResponse) int { +// tryAddMovementSensorReadingOnce adds a reading to the carto facade and does not retry. Returns remainder of time interval. +func (config *Config) tryAddMovementSensorReadingOnce(ctx context.Context, reading s.TimedMovementSensorReadingResponse) int { startTime := time.Now().UTC() - if err := config.tryAddIMUReading(ctx, reading); err != nil { - if errors.Is(err, cartofacade.ErrUnableToAcquireLock) { - config.Logger.Debugw("Skipping sensor reading due to lock contention in cartofacade", "error", err) - } else { - config.Logger.Warnw("Skipping sensor reading due to error from cartofacade", "error", err) + + if config.MovementSensor.Properties().OdometerSupported { + if err := config.tryAddOdometerReading(ctx, *reading.TimedOdometerResponse); err != nil { + if errors.Is(err, cartofacade.ErrUnableToAcquireLock) { + config.Logger.Debugw("Skipping odometer sensor reading due to lock contention in cartofacade", "error", err) + } else { + config.Logger.Warnw("Skipping odometer sensor reading due to error from cartofacade", "error", err) + } } } + + if config.MovementSensor.Properties().IMUSupported { + if err := config.tryAddIMUReading(ctx, *reading.TimedIMUResponse); err != nil { + if errors.Is(err, cartofacade.ErrUnableToAcquireLock) { + config.Logger.Debugw("Skipping IMU sensor reading due to lock contention in cartofacade", "error", err) + } else { + config.Logger.Warnw("Skipping IMU sensor reading due to error from cartofacade", "error", err) + } + } + } + timeElapsedMs := int(time.Since(startTime).Milliseconds()) - return int(math.Max(0, float64(1000/config.IMU.DataFrequencyHz()-timeElapsedMs))) + return int(math.Max(0, float64(1000/config.MovementSensor.DataFrequencyHz()-timeElapsedMs))) } -// tryAddIMUReading tries to add a reading to the carto facade. +// tryAddIMUReading tries to add an IMU reading to the carto facade. func (config *Config) tryAddIMUReading(ctx context.Context, reading s.TimedIMUReadingResponse) error { - err := config.CartoFacade.AddIMUReading(ctx, config.Timeout, config.IMU.Name(), reading) + err := config.CartoFacade.AddIMUReading(ctx, config.Timeout, config.MovementSensor.Name(), reading) if err != nil { config.Logger.Debugf("%v \t | IMU | Failure \t \t | %v \n", reading.ReadingTime, reading.ReadingTime.Unix()) } else { @@ -95,3 +132,14 @@ func (config *Config) tryAddIMUReading(ctx context.Context, reading s.TimedIMURe } return err } + +// tryAddOdometerReading tries to add an odometer reading to the carto facade. +func (config *Config) tryAddOdometerReading(ctx context.Context, reading s.TimedOdometerReadingResponse) error { + err := config.CartoFacade.AddOdometerReading(ctx, config.Timeout, config.MovementSensor.Name(), reading) + if err != nil { + config.Logger.Debugf("%v \t | Odometer | Failure \t \t | %v \n", reading.ReadingTime, reading.ReadingTime.Unix()) + } else { + config.Logger.Debugf("%v \t | Odometer | Success \t \t | %v \n", reading.ReadingTime, reading.ReadingTime.Unix()) + } + return err +} diff --git a/sensorprocess/movementsensorprocess_test.go b/sensorprocess/movementsensorprocess_test.go new file mode 100644 index 000000000..3bfca446a --- /dev/null +++ b/sensorprocess/movementsensorprocess_test.go @@ -0,0 +1,344 @@ +package sensorprocess + +import ( + "context" + "errors" + "testing" + "time" + + "github.com/golang/geo/r3" + geo "github.com/kellydunn/golang-geo" + "go.viam.com/rdk/logging" + "go.viam.com/rdk/spatialmath" + "go.viam.com/test" + + "github.com/viamrobotics/viam-cartographer/cartofacade" + s "github.com/viamrobotics/viam-cartographer/sensors" + "github.com/viamrobotics/viam-cartographer/sensors/inject" +) + +func TestStartMovementSensor(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return 5 } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + Timeout: 10 * time.Second, + } + + t.Run("exits loop when the context was cancelled", func(t *testing.T) { + cancelCtx, cancelFunc := context.WithCancel(context.Background()) + + lidar, imu := s.NoLidar, s.FinishedReplayIMU + replaySensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(lidar, imu), string(imu), 20, logger) + test.That(t, err, test.ShouldBeNil) + + config.MovementSensor = replaySensor + + cancelFunc() + + config.StartMovementSensor(cancelCtx) + }) +} + +func TestAddMovementSensorReadingInOnline(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + + injectMovementSensor := inject.TimedMovementSensor{} + injectMovementSensor.NameFunc = func() string { return "good_movement_sensor" } + injectMovementSensor.DataFrequencyHzFunc = func() int { return 20 } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: true, + MovementSensor: &injectMovementSensor, + Timeout: 10 * time.Second, + } + + t.Run("returns error when LinearAcceleration or AngularVelocity return an error, doesn't try to add IMU data", func(t *testing.T) { + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, s.IMUWithErroringFunctions) + }) + + t.Run("returns error when Position or Orientation return an error, doesn't try to add odometer data", func(t *testing.T) { + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, s.OdometerWithErroringFunctions) + }) + + t.Run("returns error when LinearAcceleration, AngularVelocity, Position, and Orientation return an error, "+ + "doesn't try to add movement sensor data", func(t *testing.T) { + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, + s.MovementSensorBothIMUAndOdometerWithErroringFunctions) + }) + + t.Run("returns error when IMU replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, s.InvalidReplayIMU) + }) + + t.Run("returns error when odometer replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, s.InvalidReplayOdometer) + }) + + t.Run("returns error when replay movement sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { + invalidAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, cf, config, s.InvalidReplayMovementSensorBothIMUAndOdometer) + }) + + t.Run("online replay IMU adds sensor reading once and ignores errors", func(t *testing.T) { + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.ReplayIMU) + test.That(t, len(odometerCalls), test.ShouldBeZeroValue) + readingTime, err := time.Parse(time.RFC3339Nano, s.TestTimestamp) + test.That(t, err, test.ShouldBeNil) + test.That(t, imuCalls[0].currentReading.ReadingTime.Equal(readingTime), test.ShouldBeTrue) + }) + + t.Run("online replay odometer adds sensor reading once and ignores errors", func(t *testing.T) { + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, s.ReplayOdometer) + test.That(t, len(imuCalls), test.ShouldBeZeroValue) + readingTime, err := time.Parse(time.RFC3339Nano, s.TestTimestamp) + test.That(t, err, test.ShouldBeNil) + test.That(t, odometerCalls[0].currentReading.ReadingTime.Equal(readingTime), test.ShouldBeTrue) + }) + + t.Run("online replay movement sensor adds sensor reading once and ignores errors", func(t *testing.T) { + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, + s.ReplayMovementSensorBothIMUAndOdometer) + readingTime, err := time.Parse(time.RFC3339Nano, s.TestTimestamp) + test.That(t, err, test.ShouldBeNil) + test.That(t, imuCalls[0].currentReading.ReadingTime.Equal(readingTime), test.ShouldBeTrue) + test.That(t, odometerCalls[0].currentReading.ReadingTime.Equal(readingTime), test.ShouldBeTrue) + }) + + t.Run("online IMU adds sensor reading once and ignores errors", func(t *testing.T) { + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, + s.GoodIMU) + test.That(t, len(odometerCalls), test.ShouldBeZeroValue) + test.That(t, imuCalls[0].currentReading.ReadingTime.Before(imuCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) + test.That(t, imuCalls[1].currentReading.ReadingTime.Before(imuCalls[2].currentReading.ReadingTime), test.ShouldBeTrue) + }) + + t.Run("online odometer adds sensor reading once and ignores errors", func(t *testing.T) { + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, + s.GoodOdometer) + test.That(t, len(imuCalls), test.ShouldBeZeroValue) + test.That(t, odometerCalls[0].currentReading.ReadingTime.Before(odometerCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) + test.That(t, odometerCalls[1].currentReading.ReadingTime.Before(odometerCalls[2].currentReading.ReadingTime), test.ShouldBeTrue) + }) + + t.Run("online movement sensor adds sensor reading once and ignores errors", func(t *testing.T) { + imuCalls, odometerCalls := validAddMovementSensorReadingInOnlineTestHelper(context.Background(), t, config, cf, + s.GoodMovementSensorBothIMUAndOdometer) + test.That(t, imuCalls[0].currentReading.ReadingTime.Before(imuCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) + test.That(t, imuCalls[1].currentReading.ReadingTime.Before(imuCalls[2].currentReading.ReadingTime), test.ShouldBeTrue) + test.That(t, odometerCalls[0].currentReading.ReadingTime.Before(odometerCalls[1].currentReading.ReadingTime), test.ShouldBeTrue) + test.That(t, odometerCalls[1].currentReading.ReadingTime.Before(odometerCalls[2].currentReading.ReadingTime), test.ShouldBeTrue) + }) +} + +func TestTryAddMovementSensorReadingUntilSuccess(t *testing.T) { + logger := logging.NewTestLogger(t) + ctx := context.Background() + + cf := cartofacade.Mock{} + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: false, + Timeout: 10 * time.Second, + } + + t.Run("replay IMU attempts to add sensor data until success", func(t *testing.T) { + validAddMovementSensorReadingUntilSuccessTestHelper(ctx, t, config, cf, s.ReplayIMU) + }) + + t.Run("replay odometer attempts to add sensor data until success", func(t *testing.T) { + validAddMovementSensorReadingUntilSuccessTestHelper(ctx, t, config, cf, s.ReplayOdometer) + }) + + t.Run("replay movement sensor attempts to add sensor data until success", func(t *testing.T) { + validAddMovementSensorReadingUntilSuccessTestHelper(ctx, t, config, cf, s.ReplayMovementSensorBothIMUAndOdometer) + }) +} + +func TestTryAddMovementSensorReadingOnce(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return 5 } + + movementSensorName := "good_movement_sensor" + injectMovementSensor := inject.TimedMovementSensor{} + injectMovementSensor.NameFunc = func() string { return movementSensorName } + injectMovementSensor.DataFrequencyHzFunc = func() int { return 20 } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + MovementSensor: &injectMovementSensor, + Timeout: 10 * time.Second, + } + + t.Run("imu only supported", func(t *testing.T) { + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + OdometerSupported: false, + } + } + config.MovementSensor = &injectMovementSensor + tryAddMovementSensorReadingOnceTestHelper(t, config, cf) + }) + + t.Run("odometer only supported", func(t *testing.T) { + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + OdometerSupported: false, + } + } + config.MovementSensor = &injectMovementSensor + tryAddMovementSensorReadingOnceTestHelper(t, config, cf) + }) + + t.Run("both imu and odometer supported", func(t *testing.T) { + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + OdometerSupported: false, + } + } + config.MovementSensor = &injectMovementSensor + tryAddMovementSensorReadingOnceTestHelper(t, config, cf) + }) +} + +func TestTryAddIMUReading(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + imuReading := s.TimedIMUReadingResponse{ + LinearAcceleration: r3.Vector{X: 1, Y: 1, Z: 1}, + AngularVelocity: spatialmath.AngularVelocity{X: 1, Y: 1, Z: 1}, + ReadingTime: time.Now().UTC(), + } + + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return 5 } + + injectImu := inject.TimedMovementSensor{} + injectImu.NameFunc = func() string { return "good_imu" } + injectImu.DataFrequencyHzFunc = func() int { return 20 } + injectImu.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + } + } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + MovementSensor: &injectImu, + Timeout: 10 * time.Second, + } + + t.Run("return error when AddIMUReading errors out", func(t *testing.T) { + expectedErr := errors.New("failed to get imu reading") + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return expectedErr + } + + err := config.tryAddIMUReading(context.Background(), imuReading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, expectedErr) + }) + + t.Run("succeeds when AddIMUReading succeeds", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return nil + } + + err := config.tryAddIMUReading(context.Background(), imuReading) + test.That(t, err, test.ShouldBeNil) + }) +} + +func TestTryAddOdometerReading(t *testing.T) { + logger := logging.NewTestLogger(t) + cf := cartofacade.Mock{} + odometerReading := s.TimedOdometerReadingResponse{ + Position: geo.NewPoint(5, 4), + Orientation: &spatialmath.Quaternion{Real: 0.1, Imag: -0.2, Jmag: 2.5, Kmag: -9.1}, + ReadingTime: time.Now().UTC(), + } + + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return 5 } + + injectOdometer := inject.TimedMovementSensor{} + injectOdometer.NameFunc = func() string { return "good_odometer" } + injectOdometer.DataFrequencyHzFunc = func() int { return 20 } + injectOdometer.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + OdometerSupported: true, + } + } + + config := Config{ + Logger: logger, + CartoFacade: &cf, + IsOnline: injectLidar.DataFrequencyHzFunc() != 0, + Lidar: &injectLidar, + MovementSensor: &injectOdometer, + Timeout: 10 * time.Second, + } + + t.Run("return error when AddOdometerReading errors out", func(t *testing.T) { + expectedErr := errors.New("failed to get odometer reading") + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + return expectedErr + } + + err := config.tryAddOdometerReading(context.Background(), odometerReading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, expectedErr) + }) + + t.Run("succeeds when AddOdometerReading succeeds", func(t *testing.T) { + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + return nil + } + + err := config.tryAddOdometerReading(context.Background(), odometerReading) + test.That(t, err, test.ShouldBeNil) + }) +} diff --git a/sensorprocess/sensorprocess.go b/sensorprocess/sensorprocess.go index 636c5f01f..90a464790 100644 --- a/sensorprocess/sensorprocess.go +++ b/sensorprocess/sensorprocess.go @@ -4,6 +4,8 @@ package sensorprocess import ( "context" "errors" + "sort" + "strings" "time" "go.viam.com/rdk/components/camera/replaypcd" @@ -14,43 +16,81 @@ import ( s "github.com/viamrobotics/viam-cartographer/sensors" ) +type sensorType int64 + +const ( + lidar sensorType = iota + movementSensor +) + +type offlineSensorReadingTime struct { + sensorType sensorType + readingTime time.Time +} + // Config holds config needed throughout the process of adding a sensor reading to the cartofacade. type Config struct { CartoFacade cartofacade.Interface IsOnline bool - Lidar s.TimedLidar - IMU s.TimedMovementSensor + Lidar s.TimedLidar + MovementSensor s.TimedMovementSensor Timeout time.Duration InternalTimeout time.Duration Logger logging.Logger } +// getInitialMovementSensorReading gets the initial movement sensor reading. +// It discards all movement sensor readings that were recorded before the first lidar reading. +func (config *Config) getInitialMovementSensorReading(ctx context.Context, + lidarReading s.TimedLidarReadingResponse, +) (s.TimedMovementSensorReadingResponse, error) { + if config.MovementSensor == nil || (!config.MovementSensor.Properties().IMUSupported && + !config.MovementSensor.Properties().OdometerSupported) { + return s.TimedMovementSensorReadingResponse{}, errors.New("movement sensor is not supported") + } + for { + movementSensorReading, err := config.MovementSensor.TimedMovementSensorReading(ctx) + if err != nil { + return s.TimedMovementSensorReadingResponse{}, err + } + + var readingTime time.Time + if config.MovementSensor.Properties().OdometerSupported { + // we can assume that the odometer reading time is earlier than the imu reading + // time, since the odometer reading is taken before the imu reading + readingTime = movementSensorReading.TimedOdometerResponse.ReadingTime + } else { + // we reach this case if the odometer is not supported + readingTime = movementSensorReading.TimedIMUResponse.ReadingTime + } + + if !readingTime.Before(lidarReading.ReadingTime) { + return movementSensorReading, nil + } + } +} + // StartOfflineSensorProcess starts the process of adding lidar and movement sensor data -// in a deterministically defined order to cartographer. +// in a deterministically defined order to cartographer. Returns a bool that indicates +// whether or not the end of either the lidar or movement sensor datasets have been reached. func (config *Config) StartOfflineSensorProcess(ctx context.Context) bool { // get the initial lidar reading lidarReading, err := config.Lidar.TimedLidarReading(ctx) if err != nil { config.Logger.Warn(err) - return errors.Is(err, replaypcd.ErrEndOfDataset) + return strings.Contains(err.Error(), replaypcd.ErrEndOfDataset.Error()) } - var imuReading s.TimedIMUReadingResponse - if config.IMU != nil { + var movementSensorReading s.TimedMovementSensorReadingResponse + if config.MovementSensor != nil && (config.MovementSensor.Properties().IMUSupported || + config.MovementSensor.Properties().OdometerSupported) { // get the initial IMU reading; discard all IMU readings that were recorded before the first lidar reading - for { - movementSensorReading, err := config.IMU.TimedMovementSensorReading(ctx) - if err != nil { - config.Logger.Warn(err) - return errors.Is(err, replaymovementsensor.ErrEndOfDataset) - } - - imuReading = *movementSensorReading.TimedIMUResponse - if !imuReading.ReadingTime.Before(lidarReading.ReadingTime) { - break - } + movementSensorReading, err = config.getInitialMovementSensorReading(ctx, lidarReading) + if err != nil { + config.Logger.Warn(err) + return strings.Contains(err.Error(), replaymovementsensor.ErrEndOfDataset.Error()) } } @@ -60,9 +100,42 @@ func (config *Config) StartOfflineSensorProcess(ctx context.Context) bool { case <-ctx.Done(): return false default: + // create a map of supported sensors and their reading time stamps + readingTimes := []offlineSensorReadingTime{ + {sensorType: lidar, readingTime: lidarReading.ReadingTime}, + } + // default to the slightly later imu timestamp: in case that the odometer time stamp was + // taken before the lidar time stamp, but the imu time stamp was taken after the lidar time + // stamp, we'll want to prioritize adding the lidar measurement before adding the movement + // sensor measurement + if config.MovementSensor != nil && config.MovementSensor.Properties().IMUSupported { + readingTimes = append(readingTimes, + offlineSensorReadingTime{ + sensorType: movementSensor, + readingTime: movementSensorReading.TimedIMUResponse.ReadingTime, + }) + } else if config.MovementSensor != nil && config.MovementSensor.Properties().OdometerSupported { + readingTimes = append(readingTimes, + offlineSensorReadingTime{ + sensorType: movementSensor, + readingTime: movementSensorReading.TimedOdometerResponse.ReadingTime, + }) + } + + // sort the readings based on their time stamp + sort.Slice(readingTimes, + func(i, j int) bool { + // if the timestamps are the same, we want to prioritize the lidar measurement before + // the movement sensor measurement + if readingTimes[i].readingTime.Equal(readingTimes[j].readingTime) { + return readingTimes[i].sensorType == lidar + } + return readingTimes[i].readingTime.Before(readingTimes[j].readingTime) + }) + // insert the reading with the earliest time stamp - if config.IMU == nil || - !lidarReading.ReadingTime.After(imuReading.ReadingTime) { + switch readingTimes[0].sensorType { + case lidar: if err := config.tryAddLidarReadingUntilSuccess(ctx, lidarReading); err != nil { return false } @@ -70,26 +143,25 @@ func (config *Config) StartOfflineSensorProcess(ctx context.Context) bool { lidarReading, err = config.Lidar.TimedLidarReading(ctx) if err != nil { config.Logger.Warn(err) - lidarEndOfDataSetReached := errors.Is(err, replaypcd.ErrEndOfDataset) + lidarEndOfDataSetReached := strings.Contains(err.Error(), replaypcd.ErrEndOfDataset.Error()) if lidarEndOfDataSetReached { config.runFinalOptimization(ctx) } return lidarEndOfDataSetReached } - } else { - if err := config.tryAddIMUReadingUntilSuccess(ctx, imuReading); err != nil { + case movementSensor: + if err := config.tryAddMovementSensorReadingUntilSuccess(ctx, movementSensorReading); err != nil { return false } - movementSensorReading, err := config.IMU.TimedMovementSensorReading(ctx) + movementSensorReading, err = config.MovementSensor.TimedMovementSensorReading(ctx) if err != nil { config.Logger.Warn(err) - msEndOfDataSetReached := errors.Is(err, replaymovementsensor.ErrEndOfDataset) + msEndOfDataSetReached := strings.Contains(err.Error(), replaymovementsensor.ErrEndOfDataset.Error()) if msEndOfDataSetReached { config.runFinalOptimization(ctx) } return msEndOfDataSetReached } - imuReading = *movementSensorReading.TimedIMUResponse } } } diff --git a/sensorprocess/sensorprocess_test.go b/sensorprocess/sensorprocess_test.go index eb7530a69..966cd2b75 100644 --- a/sensorprocess/sensorprocess_test.go +++ b/sensorprocess/sensorprocess_test.go @@ -9,6 +9,7 @@ import ( "time" "github.com/golang/geo/r3" + geo "github.com/kellydunn/golang-geo" "go.viam.com/rdk/components/camera/replaypcd" "go.viam.com/rdk/components/movementsensor/replay" "go.viam.com/rdk/logging" @@ -28,12 +29,21 @@ func TestStartOfflineSensorProcess(t *testing.T) { ReadingTime: time.Now().UTC(), } + odometerReading := s.TimedOdometerReadingResponse{ + Position: geo.NewPoint(5, 4), + Orientation: &spatialmath.Quaternion{Real: 0.1, Imag: -0.2, Jmag: 2.5, Kmag: -9.1}, + ReadingTime: time.Now().UTC(), + } + + imuReading := s.TimedIMUReadingResponse{ + LinearAcceleration: r3.Vector{X: 1, Y: 2, Z: 3}, + AngularVelocity: spatialmath.AngularVelocity{X: 4, Y: 5, Z: 6}, + ReadingTime: time.Now().UTC(), + } + movementSensorReading := s.TimedMovementSensorReadingResponse{ - TimedIMUResponse: &s.TimedIMUReadingResponse{ - LinearAcceleration: r3.Vector{X: 1, Y: 2, Z: 3}, - AngularVelocity: spatialmath.AngularVelocity{X: 4, Y: 5, Z: 6}, - ReadingTime: time.Now().UTC(), - }, + TimedIMUResponse: &imuReading, + TimedOdometerResponse: &odometerReading, } cf := cartofacade.Mock{} @@ -59,6 +69,17 @@ func TestStartOfflineSensorProcess(t *testing.T) { return s.TimedLidarReadingResponse{}, replaypcd.ErrEndOfDataset } + injectMovementSensor.TimedMovementSensorReadingFunc = func(ctx context.Context) (s.TimedMovementSensorReadingResponse, error) { + return movementSensorReading, nil + } + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, + OdometerSupported: true, + } + } + config.MovementSensor = &injectMovementSensor + countAddedLidarData := 0 cf.AddLidarReadingFunc = func(ctx context.Context, timeout time.Duration, lidarName string, currentReading s.TimedLidarReadingResponse, @@ -75,10 +96,19 @@ func TestStartOfflineSensorProcess(t *testing.T) { return nil } + countAddedOdometerData := 0 + cf.AddOdometerReadingFunc = func(ctx context.Context, timeout time.Duration, + odometerName string, currentReading s.TimedOdometerReadingResponse, + ) error { + countAddedIMUData++ + return nil + } + endOfDataSetReached := config.StartOfflineSensorProcess(context.Background()) test.That(t, endOfDataSetReached, test.ShouldBeTrue) test.That(t, countAddedLidarData, test.ShouldEqual, 0) test.That(t, countAddedIMUData, test.ShouldEqual, 0) + test.That(t, countAddedOdometerData, test.ShouldEqual, 0) }) t.Run("returns true when lidar reaches the end of the dataset and the optimization function fails", func(t *testing.T) { @@ -86,12 +116,13 @@ func TestStartOfflineSensorProcess(t *testing.T) { return errors.New("test error") } - lidar, imu := s.FinishedReplayLidar, s.NoMovementSensor + lidar, ms := s.FinishedReplayLidar, s.NoMovementSensor dataFrequencyHz := 0 - replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) + replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, ms), string(lidar), dataFrequencyHz, logger) test.That(t, err, test.ShouldBeNil) config.Lidar = replaySensor + config.MovementSensor = nil endOfDataSetReached := config.StartOfflineSensorProcess(context.Background()) test.That(t, endOfDataSetReached, test.ShouldBeTrue) @@ -106,48 +137,89 @@ func TestStartOfflineSensorProcess(t *testing.T) { cases := []struct { description string imuEnabled bool + odometerEnabled bool lidarReadingTimeAddedMs []int msReadingTimeAddedMs []int expectedDataInsertions []string }{ { - description: "no imu data is added if imu is not enabled", + description: "no movement sensor data is added if movement sensor is not enabled", imuEnabled: false, + odometerEnabled: false, lidarReadingTimeAddedMs: []int{0, 2, 4, 6, 8}, msReadingTimeAddedMs: []int{1, 3, 5}, expectedDataInsertions: []string{"lidar: 0", "lidar: 2", "lidar: 4", "lidar: 6", "lidar: 8"}, }, { - description: "skip imu data until first lidar data is inserted", + description: "skip movement sensor data until first lidar data is inserted", imuEnabled: true, + odometerEnabled: true, lidarReadingTimeAddedMs: []int{5, 7}, msReadingTimeAddedMs: []int{1, 2, 3, 4, 5, 6, 7, 8}, - expectedDataInsertions: []string{"lidar: 5", "imu: 5", "imu: 6", "lidar: 7"}, + expectedDataInsertions: []string{"lidar: 5", "odometer: 5", "imu: 5", "odometer: 6", "imu: 6", "lidar: 7"}, }, { description: "if imu data ends before lidar data ends, stop adding data once end of imu dataset is reached", imuEnabled: true, + odometerEnabled: false, lidarReadingTimeAddedMs: []int{2, 4, 6, 8, 10, 12}, msReadingTimeAddedMs: []int{1, 3, 5}, expectedDataInsertions: []string{"lidar: 2", "imu: 3", "lidar: 4", "imu: 5"}, }, + { + description: "if odometer data ends before lidar data ends, stop adding data once end of odometer dataset is reached", + imuEnabled: false, + odometerEnabled: true, + lidarReadingTimeAddedMs: []int{2, 4, 6, 8, 10, 12}, + msReadingTimeAddedMs: []int{1, 3, 5}, + expectedDataInsertions: []string{"lidar: 2", "odometer: 3", "lidar: 4", "odometer: 5"}, + }, + { + description: "if movement sensor data ends before lidar data ends, stop adding data once end " + + "of movement sensor dataset is reached", + imuEnabled: true, + odometerEnabled: true, + lidarReadingTimeAddedMs: []int{2, 4, 6, 8, 10, 12}, + msReadingTimeAddedMs: []int{1, 3, 5}, + expectedDataInsertions: []string{"lidar: 2", "odometer: 3", "imu: 3", "lidar: 4", "odometer: 5", "imu: 5"}, + }, { description: "if lidar data ends before imu data ends, stop adding data once end of lidar dataset is reached", imuEnabled: true, + odometerEnabled: false, lidarReadingTimeAddedMs: []int{1, 3, 5}, msReadingTimeAddedMs: []int{2, 3, 4, 6, 8, 10, 12}, expectedDataInsertions: []string{"lidar: 1", "imu: 2", "lidar: 3", "imu: 3", "imu: 4", "lidar: 5"}, }, + { + description: "if lidar data ends before odometer data ends, stop adding data once end of lidar dataset is reached", + imuEnabled: false, + odometerEnabled: true, + lidarReadingTimeAddedMs: []int{1, 3, 5}, + msReadingTimeAddedMs: []int{2, 3, 4, 6, 8, 10, 12}, + expectedDataInsertions: []string{"lidar: 1", "odometer: 2", "lidar: 3", "odometer: 3", "odometer: 4", "lidar: 5"}, + }, + { + description: "if lidar data ends before movement sensor data ends, stop adding data once end of lidar dataset is reached", + imuEnabled: true, + odometerEnabled: true, + lidarReadingTimeAddedMs: []int{1, 3, 5}, + msReadingTimeAddedMs: []int{2, 3, 4, 6, 8, 10, 12}, + expectedDataInsertions: []string{ + "lidar: 1", "odometer: 2", "imu: 2", "lidar: 3", "odometer: 3", + "imu: 3", "odometer: 4", "imu: 4", "lidar: 5", + }, + }, } for _, tt := range cases { t.Run(tt.description, func(t *testing.T) { now := time.Now().UTC() - if tt.imuEnabled { - config.IMU = &injectMovementSensor + if tt.imuEnabled || tt.odometerEnabled { + config.MovementSensor = &injectMovementSensor } else { - config.IMU = nil + config.MovementSensor = nil } numLidarData := 0 @@ -160,15 +232,31 @@ func TestStartOfflineSensorProcess(t *testing.T) { return s.TimedLidarReadingResponse{}, replaypcd.ErrEndOfDataset } - numIMUData := 0 + numMovementSensorData := 0 injectMovementSensor.TimedMovementSensorReadingFunc = func(ctx context.Context) (s.TimedMovementSensorReadingResponse, error) { - if numIMUData < len(tt.msReadingTimeAddedMs) { - movementSensorReading.TimedIMUResponse.ReadingTime = now.Add(time.Duration(tt.msReadingTimeAddedMs[numIMUData]) * time.Millisecond) - numIMUData++ + var movementSensorReading s.TimedMovementSensorReadingResponse + if numMovementSensorData < len(tt.msReadingTimeAddedMs) { + if tt.odometerEnabled { + movementSensorReading.TimedOdometerResponse = &odometerReading + odometerReadingTime := now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) + movementSensorReading.TimedOdometerResponse.ReadingTime = odometerReadingTime + } + if tt.imuEnabled { + movementSensorReading.TimedIMUResponse = &imuReading + imuReadingTime := now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) + movementSensorReading.TimedIMUResponse.ReadingTime = imuReadingTime + } + numMovementSensorData++ return movementSensorReading, nil } return s.TimedMovementSensorReadingResponse{}, replay.ErrEndOfDataset } + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: tt.imuEnabled, + OdometerSupported: tt.odometerEnabled, + } + } actualDataInsertions := []string{} @@ -185,11 +273,20 @@ func TestStartOfflineSensorProcess(t *testing.T) { cf.AddIMUReadingFunc = func(ctx context.Context, timeout time.Duration, imuName string, currentReading s.TimedIMUReadingResponse, ) error { - actualDataInsertions = append(actualDataInsertions, "imu: "+fmt.Sprint(tt.msReadingTimeAddedMs[numIMUData-1])) + actualDataInsertions = append(actualDataInsertions, "imu: "+fmt.Sprint(tt.msReadingTimeAddedMs[numMovementSensorData-1])) countAddedIMUData++ return nil } + countAddedOdometerData := 0 + cf.AddOdometerReadingFunc = func(ctx context.Context, timeout time.Duration, + odometerName string, currentReading s.TimedOdometerReadingResponse, + ) error { + actualDataInsertions = append(actualDataInsertions, "odometer: "+fmt.Sprint(tt.msReadingTimeAddedMs[numMovementSensorData-1])) + countAddedOdometerData++ + return nil + } + countItemsInList := func(list []string, keyword string) int { counter := 0 for _, item := range list { @@ -202,506 +299,171 @@ func TestStartOfflineSensorProcess(t *testing.T) { expectedCountAddedLidarData := countItemsInList(tt.expectedDataInsertions, "lidar") expectedCountAddedIMUData := countItemsInList(tt.expectedDataInsertions, "imu") + expectedCountAddedOdometerData := countItemsInList(tt.expectedDataInsertions, "odometer") endOfDataSetReached := config.StartOfflineSensorProcess(context.Background()) test.That(t, endOfDataSetReached, test.ShouldBeTrue) test.That(t, countAddedLidarData, test.ShouldEqual, expectedCountAddedLidarData) test.That(t, countAddedIMUData, test.ShouldEqual, expectedCountAddedIMUData) + test.That(t, countAddedOdometerData, test.ShouldEqual, expectedCountAddedOdometerData) test.That(t, actualDataInsertions, test.ShouldResemble, tt.expectedDataInsertions) }) } }) } -func TestTryAddLidarReading(t *testing.T) { +func TestGetInitialMovementSensorReading(t *testing.T) { logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - reading := s.TimedLidarReadingResponse{ + + lidarReading := s.TimedLidarReadingResponse{ Reading: []byte("12345"), ReadingTime: time.Now().UTC(), } - dataFrequencyHz := 5 - injectLidar := inject.TimedLidar{} - injectLidar.NameFunc = func() string { return "good_lidar" } - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, - Timeout: 10 * time.Second, + odometerReading := s.TimedOdometerReadingResponse{ + Position: geo.NewPoint(5, 4), + Orientation: &spatialmath.Quaternion{Real: 0.1, Imag: -0.2, Jmag: 2.5, Kmag: -9.1}, + ReadingTime: time.Now().UTC(), } - t.Run("when AddLidarReading blocks for more than the data rate and succeeds, time to sleep is 0", func(t *testing.T) { - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return nil - } - - timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - - t.Run("when AddLidarReading is slower than data rate and returns a lock error, time to sleep is 0", func(t *testing.T) { - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return cartofacade.ErrUnableToAcquireLock - } - - timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - - t.Run("when AddLidarReading blocks for more than the date rate "+ - "and returns an unexpected error, time to sleep is 0", func(t *testing.T) { - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return errUnknown - } - timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - - t.Run("when AddLidarReading is faster than the date rate and succeeds, time to sleep is <= date rate", func(t *testing.T) { - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - return nil - } - - timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.Lidar.DataFrequencyHz()) - }) - - t.Run("when AddLidarReading is faster than the date rate "+ - "and returns lock error, time to sleep is <= date rate", func(t *testing.T) { - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - return cartofacade.ErrUnableToAcquireLock - } - - timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.Lidar.DataFrequencyHz()) - }) - - t.Run("when AddLidarReading is faster than date rate "+ - "and returns an unexpected error, time to sleep is <= date rate", func(t *testing.T) { - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - return errUnknown - } - - timeToSleep := config.tryAddLidarReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.Lidar.DataFrequencyHz()) - }) -} - -func TestAddLidarReadingInOnline(t *testing.T) { - logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - - dataFrequencyHz := 5 - injectLidar := inject.TimedLidar{} - injectLidar.NameFunc = func() string { return "good_lidar" } - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, - Timeout: 10 * time.Second, + imuReading := s.TimedIMUReadingResponse{ + LinearAcceleration: r3.Vector{X: 1, Y: 2, Z: 3}, + AngularVelocity: spatialmath.AngularVelocity{X: 4, Y: 5, Z: 6}, + ReadingTime: time.Now().UTC(), } - t.Run("returns error when lidar GetData returns error, doesn't try to add lidar data", func(t *testing.T) { - invalidOnlineModeLidarTestHelper(context.Background(), t, cf, config, s.LidarWithErroringFunctions, 10) - }) - - t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { - invalidOnlineModeLidarTestHelper(context.Background(), t, cf, config, s.InvalidReplayLidar, 10) - }) - - t.Run("online lidar adds sensor reading once and ignores errors", func(t *testing.T) { - onlineModeLidarTestHelper(context.Background(), t, config, cf, s.GoodLidar) - }) -} - -func TestStartLidar(t *testing.T) { - logger := logging.NewTestLogger(t) cf := cartofacade.Mock{} - dataFrequencyHz := 5 injectLidar := inject.TimedLidar{} injectLidar.NameFunc = func() string { return "good_lidar" } - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, - Timeout: 10 * time.Second, - } - - t.Run("exits loop when the context was cancelled", func(t *testing.T) { - cancelCtx, cancelFunc := context.WithCancel(context.Background()) - - lidar, imu := s.FinishedReplayLidar, s.NoMovementSensor - replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) - test.That(t, err, test.ShouldBeNil) - - config.Lidar = replaySensor - - cancelFunc() - - config.StartLidar(cancelCtx) - }) -} - -func TestTryAddIMUReading(t *testing.T) { - logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - reading := s.TimedIMUReadingResponse{ - LinearAcceleration: r3.Vector{X: 1, Y: 1, Z: 1}, - AngularVelocity: spatialmath.AngularVelocity{X: 1, Y: 1, Z: 1}, - ReadingTime: time.Now().UTC(), - } - - injectLidar := inject.TimedLidar{} - injectLidar.DataFrequencyHzFunc = func() int { return 5 } + injectLidar.DataFrequencyHzFunc = func() int { return 0 } - injectImu := inject.TimedMovementSensor{} - injectImu.NameFunc = func() string { return "good_imu" } - injectImu.DataFrequencyHzFunc = func() int { return 20 } + injectMovementSensor := inject.TimedMovementSensor{} + injectMovementSensor.NameFunc = func() string { return "good_movement_sensor" } + injectMovementSensor.DataFrequencyHzFunc = func() int { return 0 } config := Config{ Logger: logger, CartoFacade: &cf, IsOnline: injectLidar.DataFrequencyHzFunc() != 0, Lidar: &injectLidar, - IMU: &injectImu, Timeout: 10 * time.Second, } - t.Run("when AddIMUReading blocks for more than the date rate and succeeds, time to sleep is 0", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return nil - } - - timeToSleep := config.tryAddIMUReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - - t.Run("when AddIMUReading is slower than date rate and returns a lock error, time to sleep is 0", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return cartofacade.ErrUnableToAcquireLock - } - - timeToSleep := config.tryAddIMUReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) - }) - - t.Run("when AddIMUReading blocks for more than the date rate and returns an unexpected error, time to sleep is 0", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - time.Sleep(1 * time.Second) - return errUnknown - } - - timeToSleep := config.tryAddIMUReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldEqual, 0) + t.Run("return error if movement sensor is not supported", func(t *testing.T) { + config.MovementSensor = nil + movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, errors.New("movement sensor is not supported")) + test.That(t, movementSensorReading, test.ShouldResemble, s.TimedMovementSensorReadingResponse{}) }) - t.Run("when AddIMUReading is faster than the date rate and succeeds, time to sleep is <= date rate", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - return nil + t.Run("return error if neither IMU nor odometer are supported", func(t *testing.T) { + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{} } - - timeToSleep := config.tryAddIMUReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.IMU.DataFrequencyHz()) + config.MovementSensor = &injectMovementSensor + movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, errors.New("movement sensor is not supported")) + test.That(t, movementSensorReading, test.ShouldResemble, s.TimedMovementSensorReadingResponse{}) }) - t.Run("when AddIMUReading is faster than the date rate and returns a lock error, time to sleep is <= date rate", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - return cartofacade.ErrUnableToAcquireLock - } - - timeToSleep := config.tryAddIMUReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.IMU.DataFrequencyHz()) - }) - - t.Run("when AddIMUReading is faster than date rate "+ - "and returns an unexpected error, time to sleep is <= date rate", func(t *testing.T) { - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - return errUnknown - } - - timeToSleep := config.tryAddIMUReadingOnce(context.Background(), reading) - test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) - test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.IMU.DataFrequencyHz()) - }) -} - -func TestAddIMUReadingInOnline(t *testing.T) { - logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - - injectImu := inject.TimedMovementSensor{} - injectImu.NameFunc = func() string { return "good_imu" } - injectImu.DataFrequencyHzFunc = func() int { return 20 } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: true, - IMU: &injectImu, - Timeout: 10 * time.Second, - } - - t.Run("returns error when IMU GetData returns error, doesn't try to add IMU data", func(t *testing.T) { - invalidOnlineModeIMUTestHelper(context.Background(), t, cf, config, 10, s.IMUWithErroringFunctions, 10) - }) - - t.Run("returns error when replay sensor timestamp is invalid, doesn't try to add sensor data", func(t *testing.T) { - invalidOnlineModeIMUTestHelper(context.Background(), t, cf, config, 10, s.InvalidReplayIMU, 10) - }) - - t.Run("online replay IMU adds sensor reading once and ignores errors", func(t *testing.T) { - onlineModeIMUTestHelper(context.Background(), t, config, cf, s.ReplayIMU) - }) - - t.Run("online IMU adds sensor reading once and ignores errors", func(t *testing.T) { - onlineModeIMUTestHelper(context.Background(), t, config, cf, s.GoodIMU) - }) -} - -func TestStartIMU(t *testing.T) { - logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - - injectLidar := inject.TimedLidar{} - injectLidar.DataFrequencyHzFunc = func() int { return 5 } - - injectImu := inject.TimedMovementSensor{} - injectImu.NameFunc = func() string { return "good_imu" } - injectImu.DataFrequencyHzFunc = func() int { return 20 } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: injectLidar.DataFrequencyHzFunc() != 0, - Lidar: &injectLidar, - IMU: &injectImu, - Timeout: 10 * time.Second, - } - - t.Run("exits loop when the context was cancelled", func(t *testing.T) { - cancelCtx, cancelFunc := context.WithCancel(context.Background()) - - lidar, imu := s.NoLidar, s.FinishedReplayIMU - replaySensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(lidar, imu), string(imu), 20, logger) - test.That(t, err, test.ShouldBeNil) - - config.IMU = replaySensor - - cancelFunc() - - config.StartIMU(cancelCtx) - }) -} - -func TestTryAddLidarReadingUntilSuccess(t *testing.T) { - logger := logging.NewTestLogger(t) - cf := cartofacade.Mock{} - cf.RunFinalOptimizationFunc = func(context.Context, time.Duration) error { - return nil - } - - dataFrequencyHz := 0 - - lidarReading := s.TimedLidarReadingResponse{ - Reading: []byte("12345"), - ReadingTime: time.Now().UTC(), - } - - injectLidar := inject.TimedLidar{} - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } - - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: false, - Lidar: &injectLidar, - Timeout: 10 * time.Second, - } - - t.Run("replay lidar adds sensor data until success", func(t *testing.T) { - lidar, imu := s.ReplayLidar, s.NoMovementSensor - replaySensor, err := s.NewLidar(context.Background(), s.SetupDeps(lidar, imu), string(lidar), dataFrequencyHz, logger) - test.That(t, err, test.ShouldBeNil) - - var calls []addLidarReadingArgs - cf.AddLidarReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedLidarReadingResponse, - ) error { - args := addLidarReadingArgs{ - timeout: timeout, - sensorName: sensorName, - currentReading: currentReading, - } - calls = append(calls, args) - if len(calls) == 1 { - return errUnknown - } - if len(calls) == 2 { - return cartofacade.ErrUnableToAcquireLock + t.Run("return error if TimedMovementSensorReading errors out", func(t *testing.T) { + expectedErr := errors.New("error in TimedMovementSensorReading") + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: true, } - return nil } - config.Lidar = replaySensor - - config.tryAddLidarReadingUntilSuccess(context.Background(), lidarReading) - test.That(t, len(calls), test.ShouldEqual, 3) - - firstTimestamp := calls[0].currentReading.ReadingTime - for i, call := range calls { - t.Logf("call %d", i) - test.That(t, call.sensorName, test.ShouldResemble, string(lidar)) - test.That(t, call.currentReading.Reading, test.ShouldResemble, lidarReading.Reading) - test.That(t, call.timeout, test.ShouldEqual, config.Timeout) - test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + injectMovementSensor.TimedMovementSensorReadingFunc = func(ctx context.Context) (s.TimedMovementSensorReadingResponse, error) { + return s.TimedMovementSensorReadingResponse{}, expectedErr } + config.MovementSensor = &injectMovementSensor + movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, expectedErr) + test.That(t, movementSensorReading, test.ShouldResemble, s.TimedMovementSensorReadingResponse{}) }) -} - -func TestTryAddIMUReadingUntilSuccess(t *testing.T) { - logger := logging.NewTestLogger(t) - ctx := context.Background() - - cf := cartofacade.Mock{} - dataFrequencyHz := 0 - injectImu := inject.TimedMovementSensor{} - injectImu.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + t.Run("successful data insertion", func(t *testing.T) { + cases := []struct { + description string + imuEnabled bool + odometerEnabled bool + lidarReadingTimeAddedMs int + msReadingTimeAddedMs []int + expectedMsReadingTimeAddedMs int + }{ + { + description: "skip movement sensor data until first lidar data is inserted", + imuEnabled: true, + odometerEnabled: true, + lidarReadingTimeAddedMs: 5, + msReadingTimeAddedMs: []int{1, 2, 3, 4, 6, 7, 8}, + expectedMsReadingTimeAddedMs: 6, + }, + { + description: "skip imu data until first lidar data is inserted", + imuEnabled: true, + odometerEnabled: false, + lidarReadingTimeAddedMs: 7, + msReadingTimeAddedMs: []int{1, 2, 3, 4, 5, 6, 7, 8}, + expectedMsReadingTimeAddedMs: 7, + }, + { + description: "skip odometer data until first lidar data is inserted", + imuEnabled: false, + odometerEnabled: true, + lidarReadingTimeAddedMs: 3, + msReadingTimeAddedMs: []int{1, 2, 5, 6, 7, 8}, + expectedMsReadingTimeAddedMs: 5, + }, + } - config := Config{ - Logger: logger, - CartoFacade: &cf, - IsOnline: false, - IMU: &injectImu, - Timeout: 10 * time.Second, - } + for _, tt := range cases { + t.Run(tt.description, func(t *testing.T) { + now := time.Now().UTC() - t.Run("replay IMU adds sensor data until success", func(t *testing.T) { - lidar, imu := s.NoLidar, s.ReplayIMU - replayIMU, err := s.NewMovementSensor(context.Background(), s.SetupDeps(lidar, imu), string(imu), dataFrequencyHz, logger) - test.That(t, err, test.ShouldBeNil) + if tt.imuEnabled || tt.odometerEnabled { + config.MovementSensor = &injectMovementSensor + } else { + config.MovementSensor = nil + } - injectLidar := inject.TimedLidar{} - injectLidar.DataFrequencyHzFunc = func() int { return dataFrequencyHz } + lidarReading.ReadingTime = now.Add(time.Duration(tt.lidarReadingTimeAddedMs) * time.Millisecond) - var calls []addIMUReadingArgs - cf.AddIMUReadingFunc = func( - ctx context.Context, - timeout time.Duration, - sensorName string, - currentReading s.TimedIMUReadingResponse, - ) error { - args := addIMUReadingArgs{ - timeout: timeout, - sensorName: sensorName, - currentReading: currentReading, - } - calls = append(calls, args) - if len(calls) == 1 { - return errUnknown - } - if len(calls) == 2 { - return cartofacade.ErrUnableToAcquireLock - } - return nil - } - config.IMU = replayIMU - config.Lidar = &injectLidar + numMovementSensorData := 0 + injectMovementSensor.TimedMovementSensorReadingFunc = func(ctx context.Context) (s.TimedMovementSensorReadingResponse, error) { + var movementSensorReading s.TimedMovementSensorReadingResponse + if numMovementSensorData < len(tt.msReadingTimeAddedMs) { + if tt.odometerEnabled { + movementSensorReading.TimedOdometerResponse = &odometerReading + odometerReadingTime := now.Add(time.Duration(tt.msReadingTimeAddedMs[numMovementSensorData]) * time.Millisecond) + movementSensorReading.TimedOdometerResponse.ReadingTime = odometerReadingTime + } + if tt.imuEnabled { + movementSensorReading.TimedIMUResponse = &imuReading + imuReadingTime := now.Add(time.Duration(float64(tt.msReadingTimeAddedMs[numMovementSensorData])+0.1) * time.Millisecond) + movementSensorReading.TimedIMUResponse.ReadingTime = imuReadingTime + } + numMovementSensorData++ + return movementSensorReading, nil + } + return movementSensorReading, replay.ErrEndOfDataset + } + injectMovementSensor.PropertiesFunc = func() s.MovementSensorProperties { + return s.MovementSensorProperties{ + IMUSupported: tt.imuEnabled, + OdometerSupported: tt.odometerEnabled, + } + } - config.tryAddIMUReadingUntilSuccess(ctx, expectedIMUReading) - test.That(t, len(calls), test.ShouldEqual, 3) - - firstTimestamp := calls[0].currentReading.ReadingTime - for i, call := range calls { - t.Logf("call %d", i) - test.That(t, call.sensorName, test.ShouldResemble, string(imu)) - test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, expectedIMUReading.LinearAcceleration) - test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, expectedIMUReading.AngularVelocity) - test.That(t, call.timeout, test.ShouldEqual, config.Timeout) - test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + movementSensorReading, err := config.getInitialMovementSensorReading(context.Background(), lidarReading) + test.That(t, err, test.ShouldBeNil) + test.That(t, movementSensorReading, test.ShouldNotResemble, s.TimedMovementSensorReadingResponse{}) + test.That(t, tt.msReadingTimeAddedMs[numMovementSensorData-1], test.ShouldResemble, tt.expectedMsReadingTimeAddedMs) + }) } }) } diff --git a/sensorprocess/testhelper.go b/sensorprocess/testhelper.go index 9fbdd3d0a..43ea2ab97 100644 --- a/sensorprocess/testhelper.go +++ b/sensorprocess/testhelper.go @@ -6,9 +6,9 @@ import ( "testing" "time" - "github.com/golang/geo/r3" "go.viam.com/rdk/logging" "go.viam.com/rdk/spatialmath" + rdkutils "go.viam.com/rdk/utils" "go.viam.com/test" "github.com/viamrobotics/viam-cartographer/cartofacade" @@ -28,6 +28,12 @@ type addIMUReadingArgs struct { currentReading s.TimedIMUReadingResponse } +type addOdometerReadingArgs struct { + timeout time.Duration + sensorName string + currentReading s.TimedOdometerReadingResponse +} + var ( //nolint:dupword expectedPCD = []byte(`VERSION .7 @@ -41,14 +47,11 @@ VIEWPOINT 0 0 0 1 0 0 0 POINTS 0 DATA binary `) - expectedIMUReading = s.TimedIMUReadingResponse{ - LinearAcceleration: r3.Vector{X: 1, Y: 1, Z: 1}, - AngularVelocity: spatialmath.AngularVelocity{X: 0.017453292519943295, Y: 0.008726646259971648, Z: 0}, - } + errUnknown = errors.New("unknown error") ) -func onlineModeLidarTestHelper( +func validAddLidarReadingInOnlineTestHelper( ctx context.Context, t *testing.T, config Config, @@ -120,19 +123,55 @@ func onlineModeLidarTestHelper( } } -func onlineModeIMUTestHelper( +func invalidAddLidarReadingInOnlineTestHelper( ctx context.Context, t *testing.T, + cartoFacadeMock cartofacade.Mock, config Config, - cf cartofacade.Mock, - testImu s.TestSensor, + testLidar s.TestSensor, + lidarDataFrequencyHz int, ) { + logger := logging.NewTestLogger(t) + lidar, err := s.NewLidar(context.Background(), s.SetupDeps(testLidar, s.NoMovementSensor), string(testLidar), lidarDataFrequencyHz, logger) + test.That(t, err, test.ShouldBeNil) + + var calls []addLidarReadingArgs + cartoFacadeMock.AddLidarReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedLidarReadingResponse, + ) error { + args := addLidarReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + calls = append(calls, args) + return nil + } + config.Lidar = lidar + config.CartoFacade = &cartoFacadeMock + + err = config.addLidarReadingInOnline(ctx) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, len(calls), test.ShouldEqual, 0) +} + +func validAddMovementSensorReadingInOnlineTestHelper( + ctx context.Context, + t *testing.T, + config Config, + cf cartofacade.Mock, + testMovementSensor s.TestSensor, +) ([]addIMUReadingArgs, []addOdometerReadingArgs) { logger := logging.NewTestLogger(t) dataFrequencyHz := 100 - imu, err := s.NewMovementSensor(context.Background(), s.SetupDeps(s.NoLidar, testImu), string(testImu), dataFrequencyHz, logger) + movementSensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(s.NoLidar, testMovementSensor), + string(testMovementSensor), dataFrequencyHz, logger) test.That(t, err, test.ShouldBeNil) - var calls []addIMUReadingArgs + var imuCalls []addIMUReadingArgs cf.AddIMUReadingFunc = func( ctx context.Context, timeout time.Duration, @@ -144,104 +183,172 @@ func onlineModeIMUTestHelper( sensorName: sensorName, currentReading: currentReading, } - calls = append(calls, args) - if len(calls) == 1 { + imuCalls = append(imuCalls, args) + if len(imuCalls) == 1 { return errUnknown } - if len(calls) == 2 { + if len(imuCalls) == 2 { + return cartofacade.ErrUnableToAcquireLock + } + return nil + } + + var odometerCalls []addOdometerReadingArgs + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + args := addOdometerReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + odometerCalls = append(odometerCalls, args) + if len(odometerCalls) == 1 { + return errUnknown + } + if len(odometerCalls) == 2 { return cartofacade.ErrUnableToAcquireLock } return nil } config.CartoFacade = &cf - config.IMU = imu + config.MovementSensor = movementSensor config.IsOnline = true - err = config.addIMUReadingInOnline(ctx) + testNumberCalls := func(movementSensor s.TimedMovementSensor, expectedNumberCalls int) { + if movementSensor.Properties().IMUSupported { + test.That(t, len(imuCalls), test.ShouldEqual, expectedNumberCalls) + } else { + test.That(t, len(imuCalls), test.ShouldEqual, 0) + } + if movementSensor.Properties().OdometerSupported { + test.That(t, len(odometerCalls), test.ShouldEqual, expectedNumberCalls) + } else { + test.That(t, len(odometerCalls), test.ShouldEqual, 0) + } + } + + err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldBeNil) - test.That(t, len(calls), test.ShouldEqual, 1) + testNumberCalls(movementSensor, 1) - err = config.addIMUReadingInOnline(ctx) + err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldBeNil) - test.That(t, len(calls), test.ShouldEqual, 2) + testNumberCalls(movementSensor, 2) - err = config.addIMUReadingInOnline(ctx) + err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldBeNil) - test.That(t, len(calls), test.ShouldEqual, 3) + testNumberCalls(movementSensor, 3) - for i, call := range calls { - t.Logf("call %d", i) - test.That(t, call.sensorName, test.ShouldResemble, string(testImu)) - // the IMU test fixture happens to always return the same readings currently - // in reality they are likely different every time - test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, expectedIMUReading.LinearAcceleration) - test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, expectedIMUReading.AngularVelocity) - test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + if movementSensor.Properties().IMUSupported { + for i, call := range imuCalls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, string(testMovementSensor)) + // the IMU test fixture happens to always return the same readings currently; + // in reality they are likely different every time + test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, s.TestLinAcc) + test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, spatialmath.AngularVelocity{ + X: rdkutils.DegToRad(s.TestAngVel.X), + Y: rdkutils.DegToRad(s.TestAngVel.Y), + Z: rdkutils.DegToRad(s.TestAngVel.Z), + }) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + } } - if testImu == s.GoodIMU { - test.That(t, calls[0].currentReading.ReadingTime.Before(calls[1].currentReading.ReadingTime), test.ShouldBeTrue) - test.That(t, calls[1].currentReading.ReadingTime.Before(calls[2].currentReading.ReadingTime), test.ShouldBeTrue) - } else if testImu == s.ReplayIMU { - readingTime, err := time.Parse(time.RFC3339Nano, s.TestTimestamp) - test.That(t, err, test.ShouldBeNil) - test.That(t, calls[0].currentReading.ReadingTime.Equal(readingTime), test.ShouldBeTrue) - } else { - t.Errorf("no timestamp tests provided for %v", string(testImu)) + if movementSensor.Properties().OdometerSupported { + for i, call := range odometerCalls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, string(testMovementSensor)) + // the odometer test fixture happens to always return the same readings currently; + // in reality they are likely different every time + test.That(t, call.currentReading.Position, test.ShouldResemble, s.TestPosition) + test.That(t, call.currentReading.Orientation, test.ShouldResemble, s.TestOrientation) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + } } + + return imuCalls, odometerCalls } -func invalidOnlineModeLidarTestHelper( +func invalidAddMovementSensorReadingInOnlineTestHelper( ctx context.Context, t *testing.T, cartoFacadeMock cartofacade.Mock, config Config, - testLidar s.TestSensor, - lidarDataFrequencyHz int, + testMovementSensor s.TestSensor, ) { logger := logging.NewTestLogger(t) - lidar, err := s.NewLidar(context.Background(), s.SetupDeps(testLidar, s.NoMovementSensor), string(testLidar), lidarDataFrequencyHz, logger) + lidarFrequencyHz := 10 + movementSensorFrequencyHz := 10 + movementSensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(s.NoLidar, testMovementSensor), + string(testMovementSensor), movementSensorFrequencyHz, logger) test.That(t, err, test.ShouldBeNil) - var calls []addLidarReadingArgs - cartoFacadeMock.AddLidarReadingFunc = func( + var imuCalls []addIMUReadingArgs + cartoFacadeMock.AddIMUReadingFunc = func( ctx context.Context, timeout time.Duration, sensorName string, - currentReading s.TimedLidarReadingResponse, + currentReading s.TimedIMUReadingResponse, ) error { - args := addLidarReadingArgs{ + args := addIMUReadingArgs{ timeout: timeout, sensorName: sensorName, currentReading: currentReading, } - calls = append(calls, args) + imuCalls = append(imuCalls, args) + return nil + } + + var odometerCalls []addOdometerReadingArgs + cartoFacadeMock.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + args := addOdometerReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + odometerCalls = append(odometerCalls, args) return nil } - config.Lidar = lidar config.CartoFacade = &cartoFacadeMock - err = config.addLidarReadingInOnline(ctx) + injectLidar := inject.TimedLidar{} + injectLidar.DataFrequencyHzFunc = func() int { return lidarFrequencyHz } + config.Lidar = &injectLidar + + config.MovementSensor = movementSensor + + err = config.addMovementSensorReadingInOnline(ctx) test.That(t, err, test.ShouldNotBeNil) - test.That(t, len(calls), test.ShouldEqual, 0) + test.That(t, len(imuCalls), test.ShouldEqual, 0) + test.That(t, len(odometerCalls), test.ShouldEqual, 0) } -func invalidOnlineModeIMUTestHelper( +func validAddMovementSensorReadingUntilSuccessTestHelper( ctx context.Context, t *testing.T, - cartoFacadeMock cartofacade.Mock, config Config, - lidarDataFrequencyHz int, - testIMU s.TestSensor, - imuDataFrequencyHz int, + cf cartofacade.Mock, + testMovementSensor s.TestSensor, ) { logger := logging.NewTestLogger(t) - imu, err := s.NewMovementSensor(context.Background(), s.SetupDeps(s.NoLidar, testIMU), string(testIMU), imuDataFrequencyHz, logger) + dataFrequencyHz := 0 + movementSensor, err := s.NewMovementSensor(context.Background(), s.SetupDeps(s.NoLidar, testMovementSensor), + string(testMovementSensor), dataFrequencyHz, logger) test.That(t, err, test.ShouldBeNil) - var calls []addIMUReadingArgs - cartoFacadeMock.AddIMUReadingFunc = func( + var imuCalls []addIMUReadingArgs + cf.AddIMUReadingFunc = func( ctx context.Context, timeout time.Duration, sensorName string, @@ -252,18 +359,357 @@ func invalidOnlineModeIMUTestHelper( sensorName: sensorName, currentReading: currentReading, } - calls = append(calls, args) + imuCalls = append(imuCalls, args) + if len(imuCalls) == 1 { + return errUnknown + } + if len(imuCalls) == 2 { + return cartofacade.ErrUnableToAcquireLock + } return nil } - config.CartoFacade = &cartoFacadeMock - injectLidar := inject.TimedLidar{} - injectLidar.DataFrequencyHzFunc = func() int { return lidarDataFrequencyHz } - config.Lidar = &injectLidar + var odometerCalls []addOdometerReadingArgs + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + args := addOdometerReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + odometerCalls = append(odometerCalls, args) + if len(odometerCalls) == 1 { + return errUnknown + } + if len(odometerCalls) == 2 { + return cartofacade.ErrUnableToAcquireLock + } + return nil + } - config.IMU = imu + config.CartoFacade = &cf + config.MovementSensor = movementSensor + config.IsOnline = true - err = config.addIMUReadingInOnline(ctx) - test.That(t, err, test.ShouldNotBeNil) - test.That(t, len(calls), test.ShouldEqual, 0) + now := time.Now() + var movementSensorReading s.TimedMovementSensorReadingResponse + if movementSensor.Properties().IMUSupported { + movementSensorReading.TimedIMUResponse = &s.TimedIMUReadingResponse{ + AngularVelocity: s.TestAngVel, + LinearAcceleration: s.TestLinAcc, + ReadingTime: now, + } + } + if movementSensor.Properties().OdometerSupported { + movementSensorReading.TimedOdometerResponse = &s.TimedOdometerReadingResponse{ + Position: s.TestPosition, + Orientation: s.TestOrientation, + ReadingTime: now, + } + } + + err = config.tryAddMovementSensorReadingUntilSuccess(ctx, movementSensorReading) + test.That(t, err, test.ShouldBeNil) + if movementSensor.Properties().IMUSupported { + test.That(t, len(imuCalls), test.ShouldEqual, 3) + firstTimestamp := imuCalls[0].currentReading.ReadingTime + for i, call := range imuCalls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, string(testMovementSensor)) + test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, movementSensorReading.TimedIMUResponse.LinearAcceleration) + test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, movementSensorReading.TimedIMUResponse.AngularVelocity) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + } + } + if movementSensor.Properties().OdometerSupported { + test.That(t, len(odometerCalls), test.ShouldEqual, 3) + firstTimestamp := odometerCalls[0].currentReading.ReadingTime + for i, call := range odometerCalls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, string(testMovementSensor)) + test.That(t, call.currentReading.Position, test.ShouldResemble, movementSensorReading.TimedOdometerResponse.Position) + test.That(t, call.currentReading.Orientation, test.ShouldResemble, movementSensorReading.TimedOdometerResponse.Orientation) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + } + } +} + +func tryAddMovementSensorReadingOnceTestHelper( + t *testing.T, + config Config, + cf cartofacade.Mock, +) { + config.CartoFacade = &cf + + // Set up movement sensor reading + now := time.Now().UTC() + var movementSensorReading s.TimedMovementSensorReadingResponse + if config.MovementSensor.Properties().IMUSupported { + movementSensorReading.TimedIMUResponse = &s.TimedIMUReadingResponse{ + AngularVelocity: s.TestAngVel, + LinearAcceleration: s.TestLinAcc, + ReadingTime: now, + } + } + if config.MovementSensor.Properties().OdometerSupported { + movementSensorReading.TimedOdometerResponse = &s.TimedOdometerReadingResponse{ + Position: s.TestPosition, + Orientation: s.TestOrientation, + ReadingTime: now, + } + } + + if config.MovementSensor.Properties().IMUSupported { + // In case that the odometer is also supported, let's assume it works fast and efficiently for all the + // following test cases + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + return nil + } + + t.Run("when AddIMUReading blocks for more than the date rate and succeeds, time to sleep is 0", func(t *testing.T) { + var imuCalls []addIMUReadingArgs + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + time.Sleep(1 * time.Second) + args := addIMUReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + imuCalls = append(imuCalls, args) + return nil + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + + if config.MovementSensor.Properties().IMUSupported { + test.That(t, len(imuCalls), test.ShouldEqual, 1) + for i, call := range imuCalls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, config.MovementSensor.Name()) + test.That(t, call.currentReading.LinearAcceleration, test.ShouldResemble, movementSensorReading.TimedIMUResponse.LinearAcceleration) + test.That(t, call.currentReading.AngularVelocity, test.ShouldResemble, movementSensorReading.TimedIMUResponse.AngularVelocity) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + } + } + }) + + t.Run("when AddIMUReading is slower than date rate and returns a lock error, time to sleep is 0", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddIMUReading blocks for more than the date rate and returns an unexpected error, time to sleep is 0", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return errUnknown + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddIMUReading is faster than the date rate and succeeds, time to sleep is <= date rate", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return nil + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + + t.Run("when AddIMUReading is faster than the date rate and returns a lock error, time to sleep is <= date rate", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + + t.Run("when AddIMUReading or AddOdometerReading are faster than date rate "+ + "and returns an unexpected error, time to sleep is <= date rate", func(t *testing.T) { + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return errUnknown + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + } + + if config.MovementSensor.Properties().OdometerSupported { + // In case that the IMU is also supported, let's assume it works fast and efficiently for all the + // following test cases + cf.AddIMUReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedIMUReadingResponse, + ) error { + return errUnknown + } + + t.Run("when AddOdometerReading blocks for more than the date rate and succeeds, time to sleep is 0", func(t *testing.T) { + var odometerCalls []addOdometerReadingArgs + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + time.Sleep(1 * time.Second) + args := addOdometerReadingArgs{ + timeout: timeout, + sensorName: sensorName, + currentReading: currentReading, + } + odometerCalls = append(odometerCalls, args) + return nil + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + + if config.MovementSensor.Properties().OdometerSupported { + test.That(t, len(odometerCalls), test.ShouldEqual, 1) + firstTimestamp := odometerCalls[0].currentReading.ReadingTime + for i, call := range odometerCalls { + t.Logf("call %d", i) + test.That(t, call.sensorName, test.ShouldResemble, config.MovementSensor.Name()) + test.That(t, call.currentReading.Position, test.ShouldResemble, movementSensorReading.TimedOdometerResponse.Position) + test.That(t, call.currentReading.Orientation, test.ShouldResemble, movementSensorReading.TimedOdometerResponse.Orientation) + test.That(t, call.timeout, test.ShouldEqual, config.Timeout) + test.That(t, call.currentReading.ReadingTime, test.ShouldEqual, firstTimestamp) + } + } + }) + + t.Run("when AddOdometerReading is slower than date rate and returns a lock error, time to sleep is 0", func(t *testing.T) { + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddOdometerReading blocks for more than the date rate and returns an unexpected error, "+ + "time to sleep is 0", func(t *testing.T) { + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + time.Sleep(1 * time.Second) + return errUnknown + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldEqual, 0) + }) + + t.Run("when AddOdometerReading are faster than the date rate and succeeds, time to sleep is <= date rate", func(t *testing.T) { + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + return nil + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + + t.Run("when AddOdometerReading are faster than the date rate and returns a lock error, "+ + "time to sleep is <= date rate", func(t *testing.T) { + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + return cartofacade.ErrUnableToAcquireLock + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + + t.Run("when AddOdometerReading are faster than date rate "+ + "and returns an unexpected error, time to sleep is <= date rate", func(t *testing.T) { + cf.AddOdometerReadingFunc = func( + ctx context.Context, + timeout time.Duration, + sensorName string, + currentReading s.TimedOdometerReadingResponse, + ) error { + return errUnknown + } + + timeToSleep := config.tryAddMovementSensorReadingOnce(context.Background(), movementSensorReading) + test.That(t, timeToSleep, test.ShouldBeGreaterThan, 0) + test.That(t, timeToSleep, test.ShouldBeLessThanOrEqualTo, 1000/config.MovementSensor.DataFrequencyHz()) + }) + } } diff --git a/sensors/movementsensor.go b/sensors/movementsensor.go index 6230b4e17..39aee5873 100644 --- a/sensors/movementsensor.go +++ b/sensors/movementsensor.go @@ -24,7 +24,7 @@ const ( ) var ( - defaultTime = time.Time{} + undefinedTime = time.Time{} // ErrMovementSensorNeitherIMUNorOdometer denotes that the provided movement sensor does neither support // an IMU nor a movement sensor. ErrMovementSensorNeitherIMUNorOdometer = errors.New("'movement_sensor' must either support both LinearAcceleration and " + @@ -58,7 +58,7 @@ type TimedMovementSensorReadingResponse struct { // TimedIMUReadingResponse represents an IMU sensor reading with a time. type TimedIMUReadingResponse struct { - AngularVelocity spatialmath.AngularVelocity + AngularVelocity spatialmath.AngularVelocity // We set the values in radians/s instead of deg/s LinearAcceleration r3.Vector ReadingTime time.Time } @@ -107,36 +107,36 @@ func (ms *MovementSensor) TimedMovementSensorReading(ctx context.Context) (Timed timeoutCtx, cancel := context.WithTimeout(ctx, timedMovementSensorReadingTimeout) defer cancel() - if ms.imuSupported { - imuLoop: + if ms.odometerSupported { + odometerLoop: for { select { case <-timeoutCtx.Done(): - return TimedMovementSensorReadingResponse{}, errors.Wrap(timeoutCtx.Err(), "timed out getting IMU data") + return TimedMovementSensorReadingResponse{}, errors.Wrap(timeoutCtx.Err(), "timed out getting odometer data") default: - if timedIMUReadingResponse, err = ms.timedIMUReading(timeoutCtx, &angVel, &linAcc, - &readingTimeAngularVel, &readingTimeLinearAcc); err != nil && !errors.Is(err, ErrNoValidReadingObtained) { + if timedOdometerReadingResponse, err = ms.timedOdometerReading(timeoutCtx, position, &orientation, + &readingTimePosition, &readingTimeOrientation); err != nil && !errors.Is(err, ErrNoValidReadingObtained) { return TimedMovementSensorReadingResponse{}, err } - if timedIMUReadingResponse != nil { - break imuLoop + if timedOdometerReadingResponse != nil { + break odometerLoop } } } } - if ms.odometerSupported { - odometerLoop: + if ms.imuSupported { + imuLoop: for { select { case <-timeoutCtx.Done(): - return TimedMovementSensorReadingResponse{}, errors.Wrap(timeoutCtx.Err(), "timed out getting odometer data") + return TimedMovementSensorReadingResponse{}, errors.Wrap(timeoutCtx.Err(), "timed out getting IMU data") default: - if timedOdometerReadingResponse, err = ms.timedOdometerReading(timeoutCtx, position, &orientation, - &readingTimePosition, &readingTimeOrientation); err != nil && !errors.Is(err, ErrNoValidReadingObtained) { + if timedIMUReadingResponse, err = ms.timedIMUReading(timeoutCtx, &angVel, &linAcc, + &readingTimeAngularVel, &readingTimeLinearAcc); err != nil && !errors.Is(err, ErrNoValidReadingObtained) { return TimedMovementSensorReadingResponse{}, err } - if timedOdometerReadingResponse != nil { - break odometerLoop + if timedIMUReadingResponse != nil { + break imuLoop } } } @@ -170,7 +170,7 @@ func (ms *MovementSensor) timedIMUReading(ctx context.Context, angVel *spatialma return TimedIMUReadingResponse{}, false } - if *readingTimeLinearAcc == defaultTime || readingTimeLinearAcc.Sub(*readingTimeAngularVel).Milliseconds() < 0 { + if *readingTimeLinearAcc == undefinedTime || readingTimeLinearAcc.Sub(*readingTimeAngularVel).Milliseconds() < 0 { ctxWithMetadata, md := contextutils.ContextWithMetadata(ctx) if *linAcc, err = ms.sensor.LinearAcceleration(ctxWithMetadata, make(map[string]interface{})); err != nil { return &TimedIMUReadingResponse{}, errors.Wrap(err, "could not obtain LinearAcceleration") @@ -190,7 +190,7 @@ func (ms *MovementSensor) timedIMUReading(ctx context.Context, angVel *spatialma return &response, nil } - if *readingTimeAngularVel == defaultTime || readingTimeAngularVel.Sub(*readingTimeLinearAcc).Milliseconds() < 0 { + if *readingTimeAngularVel == undefinedTime || readingTimeAngularVel.Sub(*readingTimeLinearAcc).Milliseconds() < 0 { ctxWithMetadata, md := contextutils.ContextWithMetadata(ctx) if *angVel, err = ms.sensor.AngularVelocity(ctxWithMetadata, make(map[string]interface{})); err != nil { return &TimedIMUReadingResponse{}, errors.Wrap(err, "could not obtain AngularVelocity") @@ -231,7 +231,7 @@ func (ms *MovementSensor) timedOdometerReading(ctx context.Context, position *ge return TimedOdometerReadingResponse{}, false } - if *readingTimePosition == defaultTime || readingTimePosition.Sub(*readingTimeOrientation).Milliseconds() < 0 { + if *readingTimePosition == undefinedTime || readingTimePosition.Sub(*readingTimeOrientation).Milliseconds() < 0 { ctxWithMetadata, md := contextutils.ContextWithMetadata(ctx) if position, _, err = ms.sensor.Position(ctxWithMetadata, make(map[string]interface{})); err != nil { return &TimedOdometerReadingResponse{}, errors.Wrap(err, "could not obtain Position") @@ -251,7 +251,7 @@ func (ms *MovementSensor) timedOdometerReading(ctx context.Context, position *ge return &response, nil } - if *readingTimeOrientation == defaultTime || readingTimeOrientation.Sub(*readingTimePosition).Milliseconds() < 0 { + if *readingTimeOrientation == undefinedTime || readingTimeOrientation.Sub(*readingTimePosition).Milliseconds() < 0 { ctxWithMetadata, md := contextutils.ContextWithMetadata(ctx) if *orientation, err = ms.sensor.Orientation(ctxWithMetadata, make(map[string]interface{})); err != nil { return &TimedOdometerReadingResponse{}, errors.Wrap(err, "could not obtain Orientation") diff --git a/sensors/movementsensor_test.go b/sensors/movementsensor_test.go index eb579bd36..54989385e 100644 --- a/sensors/movementsensor_test.go +++ b/sensors/movementsensor_test.go @@ -57,12 +57,12 @@ func TestNewMovementSensor(t *testing.T) { actualReading, err := actualMs.TimedMovementSensorReading(context.Background()) test.That(t, err, test.ShouldBeNil) - test.That(t, actualReading.TimedIMUResponse.LinearAcceleration, test.ShouldResemble, s.LinAcc) + test.That(t, actualReading.TimedIMUResponse.LinearAcceleration, test.ShouldResemble, s.TestLinAcc) test.That(t, actualReading.TimedIMUResponse.AngularVelocity, test.ShouldResemble, spatialmath.AngularVelocity{ - X: rdkutils.DegToRad(s.AngVel.X), - Y: rdkutils.DegToRad(s.AngVel.Y), - Z: rdkutils.DegToRad(s.AngVel.Z), + X: rdkutils.DegToRad(s.TestAngVel.X), + Y: rdkutils.DegToRad(s.TestAngVel.Y), + Z: rdkutils.DegToRad(s.TestAngVel.Z), }) test.That(t, actualReading.TimedOdometerResponse, test.ShouldBeNil) }) @@ -76,8 +76,8 @@ func TestNewMovementSensor(t *testing.T) { actualReading, err := actualMs.TimedMovementSensorReading(context.Background()) test.That(t, err, test.ShouldBeNil) - test.That(t, actualReading.TimedOdometerResponse.Position, test.ShouldResemble, s.Position) - test.That(t, actualReading.TimedOdometerResponse.Orientation, test.ShouldResemble, s.Orientation) + test.That(t, actualReading.TimedOdometerResponse.Position, test.ShouldResemble, s.TestPosition) + test.That(t, actualReading.TimedOdometerResponse.Orientation, test.ShouldResemble, s.TestOrientation) test.That(t, actualReading.TimedIMUResponse, test.ShouldBeNil) }) } @@ -107,7 +107,7 @@ func TestProperties(t *testing.T) { }) t.Run("both IMU and odometer supported", func(t *testing.T) { - lidar, movementSensor := s.GoodLidar, s.MovementSensorBothIMUAndOdometer + lidar, movementSensor := s.GoodLidar, s.GoodMovementSensorBothIMUAndOdometer deps := s.SetupDeps(lidar, movementSensor) actualMovementSensor, err := s.NewMovementSensor(ctx, deps, string(movementSensor), testDataFrequencyHz, logger) test.That(t, err, test.ShouldBeNil) @@ -160,12 +160,12 @@ func TestTimedMovementSensorReading(t *testing.T) { afterReading := time.Now().UTC() test.That(t, err, test.ShouldBeNil) - test.That(t, actualReading.TimedIMUResponse.LinearAcceleration, test.ShouldResemble, s.LinAcc) + test.That(t, actualReading.TimedIMUResponse.LinearAcceleration, test.ShouldResemble, s.TestLinAcc) test.That(t, actualReading.TimedIMUResponse.AngularVelocity, test.ShouldResemble, spatialmath.AngularVelocity{ - X: rdkutils.DegToRad(s.AngVel.X), - Y: rdkutils.DegToRad(s.AngVel.Y), - Z: rdkutils.DegToRad(s.AngVel.Z), + X: rdkutils.DegToRad(s.TestAngVel.X), + Y: rdkutils.DegToRad(s.TestAngVel.Y), + Z: rdkutils.DegToRad(s.TestAngVel.Z), }) test.That(t, actualReading.TimedIMUResponse.ReadingTime.After(beforeReading), test.ShouldBeTrue) test.That(t, actualReading.TimedIMUResponse.ReadingTime.Before(afterReading), test.ShouldBeTrue) @@ -189,8 +189,8 @@ func TestTimedMovementSensorReading(t *testing.T) { afterReading := time.Now().UTC() test.That(t, err, test.ShouldBeNil) - test.That(t, actualReading.TimedOdometerResponse.Position, test.ShouldResemble, s.Position) - test.That(t, actualReading.TimedOdometerResponse.Orientation, test.ShouldResemble, s.Orientation) + test.That(t, actualReading.TimedOdometerResponse.Position, test.ShouldResemble, s.TestPosition) + test.That(t, actualReading.TimedOdometerResponse.Orientation, test.ShouldResemble, s.TestOrientation) test.That(t, actualReading.TimedOdometerResponse.ReadingTime.After(beforeReading), test.ShouldBeTrue) test.That(t, actualReading.TimedOdometerResponse.ReadingTime.Before(afterReading), test.ShouldBeTrue) test.That(t, actualReading.TimedOdometerResponse.ReadingTime.Location(), test.ShouldEqual, time.UTC) @@ -200,7 +200,7 @@ func TestTimedMovementSensorReading(t *testing.T) { t.Run("when a movemement sensor that supports both an odometer and an IMU succeeds,"+ " returns current time in UTC and the reading", func(t *testing.T) { - lidar, odometer := s.GoodLidar, s.MovementSensorBothIMUAndOdometer + lidar, odometer := s.GoodLidar, s.GoodMovementSensorBothIMUAndOdometer deps := s.SetupDeps(lidar, odometer) actualOdometer, err := s.NewMovementSensor(ctx, deps, string(odometer), testDataFrequencyHz, logger) test.That(t, err, test.ShouldBeNil) @@ -214,8 +214,8 @@ func TestTimedMovementSensorReading(t *testing.T) { afterReading := time.Now().UTC() test.That(t, err, test.ShouldBeNil) - test.That(t, actualReading.TimedOdometerResponse.Position, test.ShouldResemble, s.Position) - test.That(t, actualReading.TimedOdometerResponse.Orientation, test.ShouldResemble, s.Orientation) + test.That(t, actualReading.TimedOdometerResponse.Position, test.ShouldResemble, s.TestPosition) + test.That(t, actualReading.TimedOdometerResponse.Orientation, test.ShouldResemble, s.TestOrientation) test.That(t, actualReading.TimedOdometerResponse.ReadingTime.After(beforeReading), test.ShouldBeTrue) test.That(t, actualReading.TimedOdometerResponse.ReadingTime.Before(afterReading), test.ShouldBeTrue) test.That(t, actualReading.TimedOdometerResponse.ReadingTime.Location(), test.ShouldEqual, time.UTC) diff --git a/sensors/test_deps.go b/sensors/test_deps.go index 5e680a711..6948da8d6 100644 --- a/sensors/test_deps.go +++ b/sensors/test_deps.go @@ -2,6 +2,7 @@ package sensors import ( "context" + "math" "time" "github.com/golang/geo/r3" @@ -26,14 +27,14 @@ const BadTime = "NOT A TIME" var ( // TestTimestamp can be used to test specific timestamps provided by a replay sensor. TestTimestamp = time.Now().UTC().Format("2006-01-02T15:04:05.999999Z") - // LinAcc is the successful mock linear acceleration result used for testing. - LinAcc = r3.Vector{X: 1, Y: 1, Z: 1} - // AngVel is the successful mock angular velocity result used for testing. - AngVel = spatialmath.AngularVelocity{X: 1, Y: .5, Z: 0} - // Position is the successful mock position result used for testing. - Position = geo.NewPoint(1, 2) - // Orientation is the successful mock orientation result used for testing. - Orientation = spatialmath.NewZeroOrientation() + // TestLinAcc is the successful mock linear acceleration result used for testing. + TestLinAcc = r3.Vector{X: 1, Y: 2, Z: 3} + // TestAngVel is the successful mock angular velocity result used for testing. + TestAngVel = spatialmath.AngularVelocity{X: 1.1, Y: .5, Z: 0} + // TestPosition is the successful mock position result used for testing. + TestPosition = geo.NewPoint(5, 4) + // TestOrientation is the successful mock orientation result used for testing. + TestOrientation = &spatialmath.Quaternion{Real: 0.1, Imag: -0.2, Jmag: 2.5, Kmag: -9.1} ) // TestSensor represents sensors used for testing. @@ -43,6 +44,8 @@ const ( // InvalidSensorTestErrMsg represents an error message that indicates that the sensor is invalid. InvalidSensorTestErrMsg = "invalid test sensor" + // ---------- LIDAR Test Sensors --------------. + // GoodLidar is a lidar that works as expected and returns a pointcloud. GoodLidar TestSensor = "good_lidar" // WarmingUpLidar is a lidar whose NextPointCloud function returns a "warming up" error. @@ -63,6 +66,8 @@ const ( // FinishedReplayLidar is a lidar whose NextPointCloud function returns an end of dataset error. FinishedReplayLidar TestSensor = "finished_replay_lidar" + // ------------- IMU Test Sensors ---------------. + // GoodIMU is an IMU that works as expected and returns linear acceleration and angular velocity values. GoodIMU TestSensor = "good_imu" // IMUWithErroringFunctions is an IMU whose functions return errors. @@ -76,15 +81,29 @@ const ( // dataset error. FinishedReplayIMU TestSensor = "finished_replay_imu" + // -------------- ODOMETER Test Sensors ------------. + // GoodOdometer is an odometer that works as expected and returns position and orientation values. GoodOdometer TestSensor = "good_odometer" // OdometerWithErroringFunctions is an Odometer whose functions return errors. OdometerWithErroringFunctions TestSensor = "odometer_with_erroring_functions" + // ReplayOdometer is an odometer that works as expected and returns position and orientation values. + ReplayOdometer TestSensor = "replay_odometer" + // InvalidReplayOdometer is an odometer whose meta timestamp is invalid. + InvalidReplayOdometer TestSensor = "invalid_replay_odometer" + // FinishedReplayOdometer is an odometer whose Position and Orientation functions return an end of + // dataset error. + FinishedReplayOdometer TestSensor = "finished_replay_odometer" + + // ------------- IMU + ODOMETER Test Sensors ----------. + // MovementSensorNotIMUNotOdometer is a movement sensor that does neither support an IMU nor an odometer. MovementSensorNotIMUNotOdometer TestSensor = "movement_sensor_not_imu_not_odometer" - // MovementSensorBothIMUAndOdometer is a movement sensor that dsupports both an IMU nor an odometer. - MovementSensorBothIMUAndOdometer TestSensor = "movement_sensor_imu_and_odometer" + // GoodMovementSensorBothIMUAndOdometer is a movement sensor that supports both an IMU nor an odometer. + GoodMovementSensorBothIMUAndOdometer TestSensor = "good_movement_sensor_imu_and_odometer" + // MovementSensorBothIMUAndOdometerWithErroringFunctions is a movement sensor whose functions return errors. + MovementSensorBothIMUAndOdometerWithErroringFunctions TestSensor = "movement_sensor_imu_and_odometer_with_erroring_functions" // MovementSensorWithErroringPropertiesFunc is a movement sensor whose Properties function returns an error. MovementSensorWithErroringPropertiesFunc TestSensor = "movement_sensor_with_erroring_properties_function" // MovementSensorWithInvalidProperties is a movement sensor whose properties are invalid. @@ -93,6 +112,14 @@ const ( GibberishMovementSensor TestSensor = "gibberish_movement_sensor" // NoMovementSensor is a movement sensor that represents that no movement sensor is set up or added. NoMovementSensor TestSensor = "" + + // ReplayMovementSensorBothIMUAndOdometer is a replay movement sensor that supports both an IMU nor an odometer. + ReplayMovementSensorBothIMUAndOdometer TestSensor = "replay_movement_sensor_imu_and_odometer" + // InvalidReplayMovementSensorBothIMUAndOdometer is a replay movement sensor that supports both an IMU nor an odometer. + InvalidReplayMovementSensorBothIMUAndOdometer TestSensor = "invalid_replay_movement_sensor_imu_and_odometer" + // FinishedReplayMovementSensor is a movement sensor whose LinearAcceleration, AngularVelocity, Position, and Orientation + // functions return an end of dataset error. + FinishedReplayMovementSensor TestSensor = "finished_replay_movement_sensor" ) var ( @@ -107,17 +134,24 @@ var ( } testMovementSensors = map[TestSensor]func() *inject.MovementSensor{ - GoodIMU: getGoodIMU, - IMUWithErroringFunctions: getIMUWithErroringFunctions, - ReplayIMU: func() *inject.MovementSensor { return getReplayIMU(TestTimestamp) }, - InvalidReplayIMU: func() *inject.MovementSensor { return getReplayIMU(BadTime) }, - FinishedReplayIMU: func() *inject.MovementSensor { return getFinishedReplayIMU() }, - GoodOdometer: getGoodOdometer, - OdometerWithErroringFunctions: getOdometerWithErroringFunctions, - MovementSensorNotIMUNotOdometer: getMovementSensorNotIMUAndNotOdometer, - MovementSensorBothIMUAndOdometer: getMovementSensorBothIMUAndOdometer, - MovementSensorWithErroringPropertiesFunc: getMovementSensorWithErroringPropertiesFunc, - MovementSensorWithInvalidProperties: getMovementSensorWithInvalidProperties, + GoodIMU: getGoodIMU, + IMUWithErroringFunctions: getIMUWithErroringFunctions, + ReplayIMU: func() *inject.MovementSensor { return getReplayIMU(TestTimestamp) }, + InvalidReplayIMU: func() *inject.MovementSensor { return getReplayIMU(BadTime) }, + FinishedReplayIMU: func() *inject.MovementSensor { return getFinishedReplayIMU() }, + GoodOdometer: getGoodOdometer, + OdometerWithErroringFunctions: getOdometerWithErroringFunctions, + ReplayOdometer: func() *inject.MovementSensor { return getReplayOdometer(TestTimestamp) }, + InvalidReplayOdometer: func() *inject.MovementSensor { return getReplayOdometer(BadTime) }, + FinishedReplayOdometer: func() *inject.MovementSensor { return getFinishedReplayOdometer() }, + MovementSensorNotIMUNotOdometer: getMovementSensorNotIMUAndNotOdometer, + GoodMovementSensorBothIMUAndOdometer: getGoodMovementSensorBothIMUAndOdometer, + MovementSensorBothIMUAndOdometerWithErroringFunctions: getMovementSensorBothIMUAndOdometerWithErroringFunctions, + MovementSensorWithErroringPropertiesFunc: getMovementSensorWithErroringPropertiesFunc, + MovementSensorWithInvalidProperties: getMovementSensorWithInvalidProperties, + ReplayMovementSensorBothIMUAndOdometer: func() *inject.MovementSensor { return getReplayMovementSensor(TestTimestamp) }, + InvalidReplayMovementSensorBothIMUAndOdometer: func() *inject.MovementSensor { return getReplayMovementSensor(BadTime) }, + FinishedReplayMovementSensor: func() *inject.MovementSensor { return getFinishedReplayMovementSensor() }, } ) @@ -249,10 +283,10 @@ func getFinishedReplayLidar() *inject.Camera { func getGoodIMU() *inject.MovementSensor { imu := &inject.MovementSensor{} imu.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - return LinAcc, nil + return TestLinAcc, nil } imu.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { - return AngVel, nil + return TestAngVel, nil } imu.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -287,14 +321,14 @@ func getReplayIMU(testTime string) *inject.MovementSensor { if mdMap, ok := md.(map[string][]string); ok { mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} } - return LinAcc, nil + return TestLinAcc, nil } imu.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { md := ctx.Value(contextutils.MetadataContextKey) if mdMap, ok := md.(map[string][]string); ok { mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} } - return AngVel, nil + return TestAngVel, nil } imu.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -325,10 +359,10 @@ func getFinishedReplayIMU() *inject.MovementSensor { func getGoodOdometer() *inject.MovementSensor { odometer := &inject.MovementSensor{} odometer.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return Position, 10, nil + return TestPosition, 0.0, nil } odometer.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - return Orientation, nil + return TestOrientation, nil } odometer.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -342,10 +376,52 @@ func getGoodOdometer() *inject.MovementSensor { func getOdometerWithErroringFunctions() *inject.MovementSensor { odometer := &inject.MovementSensor{} odometer.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return &geo.Point{}, 0.0, errors.New(InvalidSensorTestErrMsg) + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), errors.New(InvalidSensorTestErrMsg) + } + odometer.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + return nil, errors.New(InvalidSensorTestErrMsg) + } + odometer.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + PositionSupported: true, + OrientationSupported: true, + }, nil + } + return odometer +} + +func getReplayOdometer(testTime string) *inject.MovementSensor { + odometer := &inject.MovementSensor{} + odometer.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + md := ctx.Value(contextutils.MetadataContextKey) + if mdMap, ok := md.(map[string][]string); ok { + mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} + } + return TestPosition, 0.0, nil } odometer.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - return &spatialmath.Quaternion{}, errors.New(InvalidSensorTestErrMsg) + md := ctx.Value(contextutils.MetadataContextKey) + if mdMap, ok := md.(map[string][]string); ok { + mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} + } + return TestOrientation, nil + } + odometer.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + PositionSupported: true, + OrientationSupported: true, + }, nil + } + return odometer +} + +func getFinishedReplayOdometer() *inject.MovementSensor { + odometer := &inject.MovementSensor{} + odometer.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), replay.ErrEndOfDataset + } + odometer.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + return nil, replay.ErrEndOfDataset } odometer.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -364,19 +440,19 @@ func getMovementSensorNotIMUAndNotOdometer() *inject.MovementSensor { return movementSensor } -func getMovementSensorBothIMUAndOdometer() *inject.MovementSensor { +func getGoodMovementSensorBothIMUAndOdometer() *inject.MovementSensor { movementSensor := &inject.MovementSensor{} movementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return Position, 10, nil + return TestPosition, 0.0, nil } movementSensor.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - return Orientation, nil + return TestOrientation, nil } movementSensor.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - return LinAcc, nil + return TestLinAcc, nil } movementSensor.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { - return AngVel, nil + return TestAngVel, nil } movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -389,6 +465,31 @@ func getMovementSensorBothIMUAndOdometer() *inject.MovementSensor { return movementSensor } +func getMovementSensorBothIMUAndOdometerWithErroringFunctions() *inject.MovementSensor { + movementSensor := &inject.MovementSensor{} + movementSensor.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + return r3.Vector{}, errors.New(InvalidSensorTestErrMsg) + } + movementSensor.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { + return spatialmath.AngularVelocity{}, errors.New(InvalidSensorTestErrMsg) + } + movementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), errors.New(InvalidSensorTestErrMsg) + } + movementSensor.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + return nil, errors.New(InvalidSensorTestErrMsg) + } + movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + AngularVelocitySupported: true, + LinearAccelerationSupported: true, + PositionSupported: true, + OrientationSupported: true, + }, nil + } + return movementSensor +} + func getMovementSensorWithErroringPropertiesFunc() *inject.MovementSensor { movementSensor := &inject.MovementSensor{} movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { @@ -400,10 +501,10 @@ func getMovementSensorWithErroringPropertiesFunc() *inject.MovementSensor { func getMovementSensorWithInvalidProperties() *inject.MovementSensor { movementSensor := &inject.MovementSensor{} movementSensor.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - return LinAcc, nil + return TestLinAcc, nil } movementSensor.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { - return AngVel, nil + return TestAngVel, nil } movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ @@ -415,3 +516,69 @@ func getMovementSensorWithInvalidProperties() *inject.MovementSensor { } return movementSensor } + +func getReplayMovementSensor(testTime string) *inject.MovementSensor { + movementSensor := &inject.MovementSensor{} + movementSensor.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + md := ctx.Value(contextutils.MetadataContextKey) + if mdMap, ok := md.(map[string][]string); ok { + mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} + } + return TestLinAcc, nil + } + movementSensor.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { + md := ctx.Value(contextutils.MetadataContextKey) + if mdMap, ok := md.(map[string][]string); ok { + mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} + } + return TestAngVel, nil + } + movementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + md := ctx.Value(contextutils.MetadataContextKey) + if mdMap, ok := md.(map[string][]string); ok { + mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} + } + return TestPosition, 0.0, nil + } + movementSensor.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + md := ctx.Value(contextutils.MetadataContextKey) + if mdMap, ok := md.(map[string][]string); ok { + mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime} + } + return TestOrientation, nil + } + movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + AngularVelocitySupported: true, + LinearAccelerationSupported: true, + PositionSupported: true, + OrientationSupported: true, + }, nil + } + return movementSensor +} + +func getFinishedReplayMovementSensor() *inject.MovementSensor { + movementSensor := &inject.MovementSensor{} + movementSensor.LinearAccelerationFunc = func(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { + return r3.Vector{}, replay.ErrEndOfDataset + } + movementSensor.AngularVelocityFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { + return spatialmath.AngularVelocity{}, replay.ErrEndOfDataset + } + movementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { + return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), replay.ErrEndOfDataset + } + movementSensor.OrientationFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { + return nil, replay.ErrEndOfDataset + } + movementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { + return &movementsensor.Properties{ + AngularVelocitySupported: true, + LinearAccelerationSupported: true, + PositionSupported: true, + OrientationSupported: true, + }, nil + } + return movementSensor +} diff --git a/testhelper/integrationtesthelper.go b/testhelper/integrationtesthelper.go index 5ad80e1da..5038f0854 100644 --- a/testhelper/integrationtesthelper.go +++ b/testhelper/integrationtesthelper.go @@ -51,7 +51,7 @@ const ( testTimeout = 20 * time.Second ) -var defaultTime = time.Time{} +var undefinedTime = time.Time{} // Test final position and orientation are at approximately the expected values. func testCartographerPosition(t *testing.T, svc slam.Service, useIMU bool, expectedComponentRef string) { @@ -197,7 +197,7 @@ func integrationTimedLidar( */ for { timeTracker.mu.Lock() - if timeTracker.imuTime == defaultTime { + if timeTracker.imuTime == undefinedTime { time.Sleep(sensorDataIngestionWaitTime) break } @@ -212,7 +212,7 @@ func integrationTimedLidar( // Communicate that all lidar readings have been sent to cartographer or if the last IMU reading has been sent, // checks if LastLidarTime has been defined. If so, simulate endOfDataSet error. t.Logf("TimedLidarReading Mock i: %d, closed: %v, readingTime: %s\n", i, closed, timeTracker.lidarTime.String()) - if i >= NumPointClouds || timeTracker.finalImuTime != defaultTime { + if i >= NumPointClouds || timeTracker.finalImuTime != undefinedTime { // Sends a signal to the integration sensor's done channel the first time end of dataset has been sent if !closed { done <- struct{}{} @@ -414,7 +414,7 @@ func integrationTimedIMU( // Communicate that all desired IMU readings have been sent or to cartographer or if the last lidar reading // has been sent by, checks if lastLidarTime has been defined. If so, simulate endOfDataSet error. t.Logf("TimedMovementSensorReading Mock i: %d, closed: %v, readingTime: %s\n", i, closed, timeTracker.imuTime.String()) - if int(i) >= len(mockDataset) || timeTracker.finalLidarTime != defaultTime { + if int(i) >= len(mockDataset) || timeTracker.finalLidarTime != undefinedTime { // Sends a signal to the integration sensor's done channel the first time end of dataset has been sent if !closed { done <- struct{}{} diff --git a/viam-cartographer/src/carto_facade/carto_facade.cc b/viam-cartographer/src/carto_facade/carto_facade.cc index b96764a85..a7f2dbbf6 100644 --- a/viam-cartographer/src/carto_facade/carto_facade.cc +++ b/viam-cartographer/src/carto_facade/carto_facade.cc @@ -9,7 +9,6 @@ #include #include "glog/logging.h" -#include "io.h" #include "map_builder.h" #include "util.h" @@ -113,22 +112,10 @@ 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.cloud_story_enabled = vcc.cloud_story_enabled; c.enable_mapping = vcc.enable_mapping; c.existing_map = to_std_string(vcc.existing_map); c.lidar_config = vcc.lidar_config; - if (!c.cloud_story_enabled) { - if (c.data_dir.size() == 0) { - throw VIAM_CARTO_DATA_DIR_NOT_PROVIDED; - } - if (vcc.map_rate_sec < 0) { - throw VIAM_CARTO_MAP_RATE_SEC_INVALID; - } - } - if (c.camera.empty()) { throw VIAM_CARTO_COMPONENT_REFERENCE_INVALID; } @@ -189,100 +176,21 @@ CartoFacade::CartoFacade(viam_carto_lib *pVCL, const viam_carto_config c, lib = pVCL; config = from_viam_carto_config(c); algo_config = ac; - path_to_internal_state = config.data_dir + "/internal_state"; path_to_internal_state_file = config.existing_map; }; CartoFacade::~CartoFacade() { bdestroy(config.component_reference); } -void setup_filesystem(std::string data_dir, - std::string path_to_internal_state) { - // setup internal state directory if it doesn't exist - auto perms = - fs::perms::group_all | fs::perms::owner_all | fs::perms::others_read; - try { - // if directory doesn't exist, create it with expected structure - if (!fs::is_directory(data_dir)) { - VLOG(1) << "data_dir doesn't exist. Creating " << data_dir; - fs::create_directory(data_dir); - VLOG(1) << "setting data_dir permissions"; - fs::permissions(data_dir, perms); - } - if (!fs::is_directory(path_to_internal_state)) { - VLOG(1) << "data_dir's internal_state directory, doesn't exist. " - "Creating " - << path_to_internal_state; - fs::create_directory(path_to_internal_state); - VLOG(1) - << "setting data_dir's internal_state directory's permissions"; - fs::permissions(path_to_internal_state, perms); - } - } catch (std::exception &e) { - LOG(ERROR) << e.what(); - throw VIAM_CARTO_DATA_DIR_FILE_SYSTEM_ERROR; - } -} - -std::vector list_sorted_files_in_directory(std::string directory) { - std::vector file_paths; - - for (const auto &entry : fs::directory_iterator(directory)) { - file_paths.push_back((entry.path()).string()); - } - - sort(file_paths.begin(), file_paths.end()); - return file_paths; -} - -std::string get_latest_internal_state_filename( - std::string path_to_internal_state) { - std::string latest_internal_state_filename; - - std::vector internal_state_filename = - list_sorted_files_in_directory(path_to_internal_state); - bool found_internal_state = false; - for (int i = internal_state_filename.size() - 1; i >= 0; i--) { - if (internal_state_filename.at(i).find(".pbstream") != - std::string::npos) { - latest_internal_state_filename = internal_state_filename.at(i); - found_internal_state = true; - break; - } - } - if (!found_internal_state) { - throw std::runtime_error( - "cannot find internal state but they should be present"); - } - - return latest_internal_state_filename; -} - void CartoFacade::IOInit() { if (state != CartoFacadeState::INITIALIZED) { LOG(ERROR) << "carto facade is in state: " << state << " expected " << CartoFacadeState::INITIALIZED; throw VIAM_CARTO_NOT_IN_INITIALIZED_STATE; } - if (config.cloud_story_enabled == true) { - slam_mode = determine_slam_mode_cloud_story_enabled( - path_to_internal_state_file, config.enable_mapping); - } else { - // Detect if data_dir has deprecated format - if (fs::is_directory(config.data_dir + "/data")) { - LOG(ERROR) - << "data directory " << config.data_dir - << " is invalid as it contains deprecated format i.e. /data " - "subdirectory"; - throw VIAM_CARTO_DATA_DIR_INVALID_DEPRECATED_STRUCTURE; - } - // Setup file system for saving internal state - setup_filesystem(config.data_dir, path_to_internal_state); - slam_mode = - determine_slam_mode(path_to_internal_state, config.map_rate_sec); - } + slam_mode = + determine_slam_mode(path_to_internal_state_file, config.enable_mapping); VLOG(1) << "slam mode: " << slam_mode; - // TODO: Make this API user configurable auto cd = find_lua_files(); if (cd.empty()) { throw VIAM_CARTO_LUA_CONFIG_NOT_FOUND; @@ -321,23 +229,13 @@ void CartoFacade::IOInit() { map_builder.BuildMapBuilder(); } - // TODO: google cartographer will terminate the program if - // the internal state is invalid - // see https://viam.atlassian.net/browse/RSDK-3553 if (slam_mode == viam::carto_facade::SlamMode::UPDATING || slam_mode == viam::carto_facade::SlamMode::LOCALIZING) { - // Check if there is an apriori map (internal state) in the - // path_to_internal_state directory or existing_map path - std::string latest_internal_state_filename; - if (config.cloud_story_enabled) { - latest_internal_state_filename = config.existing_map; - } else { - latest_internal_state_filename = - get_latest_internal_state_filename(path_to_internal_state); + // Check if apriori map file exists + std::ifstream f(path_to_internal_state_file); + if (!f.good()) { + throw VIAM_CARTO_INTERNAL_STATE_FILE_SYSTEM_ERROR; } - - VLOG(1) << "latest_internal_state_filename: " - << latest_internal_state_filename; // load_frozen_trajectory has to be true for LOCALIZING slam mode, // and false for UPDATING slam mode. bool load_frozen_trajectory = @@ -351,13 +249,13 @@ void CartoFacade::IOInit() { optimization_lock.lock(); // Load apriori map (internal state) std::lock_guard lk(map_builder_mutex); - map_builder.LoadMapFromFile(latest_internal_state_filename, + map_builder.LoadMapFromFile(config.existing_map, load_frozen_trajectory, algo_config.optimize_on_start); } else { // Load apriori map (internal state) std::lock_guard lk(map_builder_mutex); - map_builder.LoadMapFromFile(latest_internal_state_filename, + map_builder.LoadMapFromFile(config.existing_map, load_frozen_trajectory, algo_config.optimize_on_start); } @@ -682,14 +580,8 @@ void CartoFacade::GetInternalState(viam_carto_get_internal_state_response *r) { } boost::uuids::uuid uuid = boost::uuids::random_generator()(); - std::string filename; - if (config.cloud_story_enabled) { - filename = "temp_internal_state_" + boost::uuids::to_string(uuid) + - ".pbstream"; - } else { - filename = path_to_internal_state + "/" + "temp_internal_state_" + - boost::uuids::to_string(uuid) + ".pbstream"; - } + std::string filename = "/tmp/temp_internal_state_" + + boost::uuids::to_string(uuid) + ".pbstream"; { std::lock_guard lk(map_builder_mutex); @@ -719,68 +611,8 @@ void CartoFacade::Start() { throw VIAM_CARTO_NOT_IN_IO_INITIALIZED_STATE; } state = CartoFacadeState::STARTED; - if (!config.cloud_story_enabled) { - StartSaveInternalState(); - } }; -void CartoFacade::StartSaveInternalState() { - if (config.map_rate_sec == std::chrono::seconds(0)) { - return; - } - thread_save_internal_state = std::make_unique( - [&]() { this->SaveInternalStateOnInterval(); }); -} - -void CartoFacade::StopSaveInternalState() { - if (config.map_rate_sec == std::chrono::seconds(0)) { - return; - } - thread_save_internal_state->join(); -} - -void CartoFacade::SaveInternalStateOnInterval() { - auto check_for_shutdown_interval_usec = - std::chrono::microseconds(checkForShutdownIntervalMicroseconds); - while (state == CartoFacadeState::STARTED) { - auto start = std::chrono::high_resolution_clock::now(); - // Sleep for config.map_rate_sec duration, but check frequently for - // shutdown - while (state == CartoFacadeState::STARTED) { - std::chrono::duration time_elapsed_msec = - std::chrono::high_resolution_clock::now() - start; - if (time_elapsed_msec >= config.map_rate_sec) { - break; - } - if (config.map_rate_sec - time_elapsed_msec >= - check_for_shutdown_interval_usec) { - std::this_thread::sleep_for(check_for_shutdown_interval_usec); - } else { - std::this_thread::sleep_for(config.map_rate_sec - - time_elapsed_msec); - break; - } - } - - // Breakout without saving if the session has ended - - if (state != CartoFacadeState::STARTED) { - LOG(INFO) << "Saving final optimized internal state"; - } - std::time_t t = std::time(nullptr); - const std::string filename_with_timestamp = - viam::carto_facade::io::MakeFilenameWithTimestamp( - path_to_internal_state, t); - - std::lock_guard lk(map_builder_mutex); - map_builder.SaveMapToFile(true, filename_with_timestamp); - if (state != CartoFacadeState::STARTED) { - LOG(INFO) << "Finished saving final optimized internal state"; - break; - } - } -} - void CartoFacade::Stop() { if (state != CartoFacadeState::STARTED) { LOG(ERROR) << "carto facade is in state: " << state << " expected " @@ -788,9 +620,6 @@ void CartoFacade::Stop() { throw VIAM_CARTO_NOT_IN_STARTED_STATE; } state = CartoFacadeState::IO_INITIALIZED; - if (!config.cloud_story_enabled) { - StopSaveInternalState(); - } }; void CartoFacade::AddLidarReading(const viam_carto_lidar_reading *sr) { @@ -942,40 +771,6 @@ void CartoFacade::AddOdometerReading(const viam_carto_odometer_reading *sr) { }; viam::carto_facade::SlamMode determine_slam_mode( - std::string path_to_internal_state, std::chrono::seconds map_rate_sec) { - // Check if there is an apriori map (internal state) in the - // path_to_internal_state directory - std::vector internal_state_filenames = - list_sorted_files_in_directory(path_to_internal_state); - - // Check if there is a *.pbstream internal state in the - // path_to_internal_state directory - for (auto filename : internal_state_filenames) { - if (filename.find(".pbstream") != std::string::npos) { - // There is an apriori map (internal state) present, so we're - // running either in updating or localization mode. - if (map_rate_sec.count() == 0) { - // This log line is needed by rdk integration tests. - LOG(INFO) << "Running in localization only mode"; - return viam::carto_facade::SlamMode::LOCALIZING; - } - // This log line is needed by rdk integration tests. - LOG(INFO) << "Running in updating mode"; - return viam::carto_facade::SlamMode::UPDATING; - } - } - if (map_rate_sec.count() == 0) { - LOG(ERROR) - << "set to localization mode (map_rate_sec = 0) but " - "couldn't find apriori map (internal state) to localize on"; - throw VIAM_CARTO_SLAM_MODE_INVALID; - } - // This log line is needed by rdk integration tests. - LOG(INFO) << "Running in mapping mode"; - return viam::carto_facade::SlamMode::MAPPING; -} - -viam::carto_facade::SlamMode determine_slam_mode_cloud_story_enabled( std::string path_to_internal_state_file, bool enable_mapping) { // Check if an existing map has been provided if (path_to_internal_state_file.size() != 0) { diff --git a/viam-cartographer/src/carto_facade/carto_facade.h b/viam-cartographer/src/carto_facade/carto_facade.h index 862bb9cce..9e5b81e20 100644 --- a/viam-cartographer/src/carto_facade/carto_facade.h +++ b/viam-cartographer/src/carto_facade/carto_facade.h @@ -110,35 +110,32 @@ typedef struct viam_carto_odometer_reading { #define VIAM_CARTO_LIB_PLATFORM_INVALID 5 #define VIAM_CARTO_LIB_INVALID 6 #define VIAM_CARTO_LIB_NOT_INITIALIZED 7 -#define VIAM_CARTO_UNKNOWN_ERROR 9 -#define VIAM_CARTO_DATA_DIR_NOT_PROVIDED 10 -#define VIAM_CARTO_SLAM_MODE_INVALID 11 -#define VIAM_CARTO_LIDAR_CONFIG_INVALID 12 -#define VIAM_CARTO_MAP_RATE_SEC_INVALID 13 -#define VIAM_CARTO_COMPONENT_REFERENCE_INVALID 14 -#define VIAM_CARTO_LUA_CONFIG_NOT_FOUND 15 -#define VIAM_CARTO_DATA_DIR_INVALID_DEPRECATED_STRUCTURE 16 -#define VIAM_CARTO_DATA_DIR_FILE_SYSTEM_ERROR 17 -#define VIAM_CARTO_MAP_CREATION_ERROR 18 -#define VIAM_CARTO_UNKNOWN_SENSOR_NAME 19 -#define VIAM_CARTO_LIDAR_READING_EMPTY 20 -#define VIAM_CARTO_LIDAR_READING_INVALID 21 -#define VIAM_CARTO_GET_POSITION_RESPONSE_INVALID 22 -#define VIAM_CARTO_GET_POSITION_NOT_INITIALIZED 23 -#define VIAM_CARTO_POINTCLOUD_MAP_EMPTY 24 -#define VIAM_CARTO_GET_POINT_CLOUD_MAP_RESPONSE_INVALID 25 -#define VIAM_CARTO_LIB_ALREADY_INITIALIZED 26 -#define VIAM_CARTO_GET_INTERNAL_STATE_RESPONSE_INVALID 27 -#define VIAM_CARTO_GET_INTERNAL_STATE_FILE_WRITE_IO_ERROR 28 -#define VIAM_CARTO_GET_INTERNAL_STATE_FILE_READ_IO_ERROR 29 -#define VIAM_CARTO_NOT_IN_INITIALIZED_STATE 30 -#define VIAM_CARTO_NOT_IN_IO_INITIALIZED_STATE 31 -#define VIAM_CARTO_NOT_IN_STARTED_STATE 32 -#define VIAM_CARTO_NOT_IN_TERMINATABLE_STATE 33 -#define VIAM_CARTO_IMU_PROVIDED_AND_IMU_ENABLED_MISMATCH 34 -#define VIAM_CARTO_IMU_READING_EMPTY 35 -#define VIAM_CARTO_IMU_READING_INVALID 36 -#define VIAM_CARTO_ODOMETER_READING_INVALID 37 +#define VIAM_CARTO_UNKNOWN_ERROR 8 +#define VIAM_CARTO_SLAM_MODE_INVALID 9 +#define VIAM_CARTO_LIDAR_CONFIG_INVALID 10 +#define VIAM_CARTO_COMPONENT_REFERENCE_INVALID 11 +#define VIAM_CARTO_LUA_CONFIG_NOT_FOUND 12 +#define VIAM_CARTO_INTERNAL_STATE_FILE_SYSTEM_ERROR 13 +#define VIAM_CARTO_MAP_CREATION_ERROR 14 +#define VIAM_CARTO_UNKNOWN_SENSOR_NAME 15 +#define VIAM_CARTO_LIDAR_READING_EMPTY 16 +#define VIAM_CARTO_LIDAR_READING_INVALID 17 +#define VIAM_CARTO_GET_POSITION_RESPONSE_INVALID 18 +#define VIAM_CARTO_GET_POSITION_NOT_INITIALIZED 19 +#define VIAM_CARTO_POINTCLOUD_MAP_EMPTY 20 +#define VIAM_CARTO_GET_POINT_CLOUD_MAP_RESPONSE_INVALID 21 +#define VIAM_CARTO_LIB_ALREADY_INITIALIZED 22 +#define VIAM_CARTO_GET_INTERNAL_STATE_RESPONSE_INVALID 23 +#define VIAM_CARTO_GET_INTERNAL_STATE_FILE_WRITE_IO_ERROR 24 +#define VIAM_CARTO_GET_INTERNAL_STATE_FILE_READ_IO_ERROR 25 +#define VIAM_CARTO_NOT_IN_INITIALIZED_STATE 26 +#define VIAM_CARTO_NOT_IN_IO_INITIALIZED_STATE 27 +#define VIAM_CARTO_NOT_IN_STARTED_STATE 28 +#define VIAM_CARTO_NOT_IN_TERMINATABLE_STATE 29 +#define VIAM_CARTO_IMU_PROVIDED_AND_IMU_ENABLED_MISMATCH 30 +#define VIAM_CARTO_IMU_READING_EMPTY 31 +#define VIAM_CARTO_IMU_READING_INVALID 32 +#define VIAM_CARTO_ODOMETER_READING_INVALID 33 typedef struct viam_carto_algo_config { bool optimize_on_start; @@ -160,10 +157,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; - bool cloud_story_enabled; bool enable_mapping; bstring existing_map; } viam_carto_config; @@ -373,11 +367,8 @@ 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; viam_carto_LIDAR_CONFIG lidar_config; - bool cloud_story_enabled; bool enable_mapping; std::string existing_map; } config; @@ -393,10 +384,7 @@ const std::string configuration_localization_basename = "locating_in_map.lua"; const std::string configuration_update_basename = "updating_a_map.lua"; carto_facade::SlamMode determine_slam_mode(std::string path_to_map, - std::chrono::seconds map_rate_sec); - -carto_facade::SlamMode determine_slam_mode_cloud_story_enabled( - std::string path_to_map, bool enable_mapping); + bool enable_mapping); int slam_mode_to_vc_slam_mode(viam::carto_facade::SlamMode sm); @@ -408,12 +396,10 @@ class CartoFacade { ~CartoFacade(); // IOInit: - // 1. detects if the data_dir has a deprecated format & throws if it does - // 2. creates the data_dir with the correct format if it doesn't exist - // 3. sets the correct slam mode - // 4. creates & configures the map builder with the right hyperparameters + // 1. sets the correct slam mode + // 2. creates & configures the map builder with the right hyperparameters // based on the slam mode - // 5. starts the trajectory builder + // 3. starts the trajectory builder // // Needs to be first method called on newly instantiated CartoFacade object. void IOInit(); @@ -452,7 +438,6 @@ class CartoFacade { viam_carto_lib *lib; viam::carto_facade::config config; viam_carto_algo_config algo_config; - std::string path_to_internal_state; std::string path_to_internal_state_file; std::atomic state{CartoFacadeState::INITIALIZED}; std::string configuration_directory; @@ -467,18 +452,6 @@ class CartoFacade { private: // moved from namespace - // StartSaveInternalState starts the map saving process in a separate - // thread. - void StartSaveInternalState(); - - // StopSaveInternalState stops the map saving process that is running in a - // separate thread. - void StopSaveInternalState(); - - // SaveInternalStateOnInterval saves internal state with a filename that - // includes the timestamp of the time when the map is saved. - void SaveInternalStateOnInterval(); - std::shared_mutex optimization_shared_mutex; std::unique_ptr thread_save_internal_state; @@ -486,7 +459,7 @@ class CartoFacade { std::mutex viam_response_mutex; cartographer::transform::Rigid3d latest_global_pose = cartographer::transform::Rigid3d(); - // The latest_pointcloud_map variable is used enable GetPointCloudMap to + // The latest_pointcloud_map variable is used to enable GetPointCloudMap to // send the most recent map out while cartographer works on creating an // optimized map. It is only updated right before the optimization is // started. diff --git a/viam-cartographer/src/carto_facade/carto_facade_test.cc b/viam-cartographer/src/carto_facade/carto_facade_test.cc index 7dce48551..a8213739e 100644 --- a/viam-cartographer/src/carto_facade/carto_facade_test.cc +++ b/viam-cartographer/src/carto_facade/carto_facade_test.cc @@ -28,24 +28,21 @@ const auto tol = tt::tolerance(0.001); namespace viam { namespace carto_facade { -viam_carto_config viam_carto_config_setup( - int map_rate_sec, viam_carto_LIDAR_CONFIG lidar_config, - std::string data_dir, std::string camera, std::string movement_sensor, - bool cloud_story_enabled, bool enable_mapping, std::string existing_map) { +viam_carto_config viam_carto_config_setup(viam_carto_LIDAR_CONFIG lidar_config, + std::string camera, + std::string movement_sensor, + bool enable_mapping, + std::string existing_map) { struct viam_carto_config vcc; - vcc.map_rate_sec = map_rate_sec; vcc.lidar_config = lidar_config; - vcc.data_dir = bfromcstr(data_dir.c_str()); vcc.camera = bfromcstr(camera.c_str()); vcc.movement_sensor = bfromcstr(movement_sensor.c_str()); - vcc.cloud_story_enabled = cloud_story_enabled; vcc.enable_mapping = enable_mapping; vcc.existing_map = bfromcstr(existing_map.c_str()); return vcc; } void viam_carto_config_teardown(viam_carto_config vcc) { - BOOST_TEST(bdestroy(vcc.data_dir) == BSTR_OK); BOOST_TEST(bdestroy(vcc.camera) == BSTR_OK); BOOST_TEST(bdestroy(vcc.movement_sensor) == BSTR_OK); BOOST_TEST(bdestroy(vcc.existing_map) == BSTR_OK); @@ -172,44 +169,25 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_validate) { BOOST_TEST(viam_carto_lib_init(&lib, 0, 1) == VIAM_CARTO_SUCCESS); viam_carto *vc; - fs::path tmp_dir = - fs::temp_directory_path() / fs::path(bfs::unique_path().string()); std::string camera = "lidar"; std::string movement_sensor = "movement_sensor"; std::string no_camera = ""; std::string no_movement_sensor = ""; - // Test config validation with empty data_dir - struct viam_carto_algo_config ac = viam_carto_algo_config_setup(true); - struct viam_carto_config vcc_empty_data_dir = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, "", camera, movement_sensor, false, false, ""); - - BOOST_TEST(viam_carto_init(&vc, lib, vcc_empty_data_dir, ac) == - VIAM_CARTO_DATA_DIR_NOT_PROVIDED); - // Test config validation with no camera - ac = viam_carto_algo_config_setup(false); + struct viam_carto_algo_config ac = viam_carto_algo_config_setup(false); struct viam_carto_config vcc_empty_component_ref = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, tmp_dir.string(), no_camera, no_movement_sensor, - false, false, ""); + VIAM_CARTO_THREE_D, no_camera, no_movement_sensor, true, ""); BOOST_TEST(viam_carto_init(&vc, lib, vcc_empty_component_ref, ac) == VIAM_CARTO_COMPONENT_REFERENCE_INVALID); - // Test config validation with invalid map rate sec ac = viam_carto_algo_config_setup(true); - struct viam_carto_config vcc_invalid_map_rate_sec = - viam_carto_config_setup(-1, VIAM_CARTO_THREE_D, tmp_dir.string(), - camera, movement_sensor, false, false, ""); - - BOOST_TEST(viam_carto_init(&vc, lib, vcc_invalid_map_rate_sec, ac) == - VIAM_CARTO_MAP_RATE_SEC_INVALID); - // Test config validation with invalid lidar config - struct viam_carto_config vcc_invalid_lidar_config = viam_carto_config_setup( - 1, static_cast(-1), tmp_dir.string(), camera, - movement_sensor, false, false, ""); + struct viam_carto_config vcc_invalid_lidar_config = + viam_carto_config_setup(static_cast(-1), + camera, movement_sensor, true, ""); BOOST_TEST(viam_carto_init(&vc, lib, vcc_invalid_lidar_config, ac) == VIAM_CARTO_LIDAR_CONFIG_INVALID); @@ -217,38 +195,18 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_validate) { // Test config validation with invalid movement sensor config ac = viam_carto_algo_config_setup(true); struct viam_carto_config vcc_invalid_movement_sensor_config = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera, - no_movement_sensor, false, false, ""); + viam_carto_config_setup(VIAM_CARTO_THREE_D, camera, no_movement_sensor, + true, ""); BOOST_TEST( viam_carto_init(&vc, lib, vcc_invalid_movement_sensor_config, ac) == VIAM_CARTO_IMU_PROVIDED_AND_IMU_ENABLED_MISMATCH); - // Test config validation with deprecated config structure - fs::path deprecated_path = tmp_dir / fs::path(bfs::unique_path().string()); - fs::create_directories(deprecated_path.string() + "/data"); - ac = viam_carto_algo_config_setup(true); - struct viam_carto_config vcc_deprecated_path = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, deprecated_path.string(), - camera, movement_sensor, false, false, ""); - BOOST_TEST(viam_carto_init(&vc, lib, vcc_deprecated_path, ac) == - VIAM_CARTO_DATA_DIR_INVALID_DEPRECATED_STRUCTURE); - - // Test config validation with invalid path - fs::path invalid_path = tmp_dir / fs::path(bfs::unique_path().string()) / - fs::path(bfs::unique_path().string()); - - struct viam_carto_config vcc_invalid_path = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, invalid_path.string(), - camera, movement_sensor, false, false, ""); - BOOST_TEST(viam_carto_init(&vc, lib, vcc_invalid_path, ac) == - VIAM_CARTO_DATA_DIR_FILE_SYSTEM_ERROR); - // Test config validation with movement sensor (success) struct viam_carto_config vcc_with_movement_sensor_succ = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera, - movement_sensor, false, false, ""); + viam_carto_config_setup(VIAM_CARTO_THREE_D, camera, movement_sensor, + true, ""); BOOST_TEST(viam_carto_init(nullptr, lib, vcc_with_movement_sensor_succ, ac) == VIAM_CARTO_VC_INVALID); @@ -274,8 +232,8 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_validate) { viam_carto *vc2; ac = viam_carto_algo_config_setup(false); struct viam_carto_config vcc_without_movement_sensor_succ = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera, - no_movement_sensor, false, false, ""); + viam_carto_config_setup(VIAM_CARTO_THREE_D, camera, no_movement_sensor, + true, ""); BOOST_TEST(viam_carto_init(&vc2, lib, vcc_without_movement_sensor_succ, ac) == VIAM_CARTO_SUCCESS); @@ -285,25 +243,20 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_validate) { BOOST_TEST(viam_carto_terminate(&vc2) == VIAM_CARTO_VC_INVALID); // can't terminate same instance again + // TODO: Move all suite level setup & teardown to boost test hook // Teardown - viam_carto_config_teardown(vcc_empty_data_dir); viam_carto_config_teardown(vcc_empty_component_ref); - viam_carto_config_teardown(vcc_invalid_map_rate_sec); viam_carto_config_teardown(vcc_invalid_lidar_config); viam_carto_config_teardown(vcc_invalid_movement_sensor_config); - viam_carto_config_teardown(vcc_deprecated_path); - viam_carto_config_teardown(vcc_invalid_path); viam_carto_config_teardown(vcc_with_movement_sensor_succ); viam_carto_config_teardown(vcc_without_movement_sensor_succ); - // TODO: Move all suite level setup & teardown to boost test hook - fs::remove_all(tmp_dir); BOOST_TEST(viam_carto_lib_terminate(&lib) == VIAM_CARTO_SUCCESS); BOOST_TEST(viam_carto_lib_terminate(&lib) == VIAM_CARTO_LIB_INVALID); // can't terminate same instance again } -BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode_cloud_story_enabled) { +BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode) { viam_carto_lib *lib; BOOST_TEST(viam_carto_lib_init(&lib, 0, 1) == VIAM_CARTO_SUCCESS); @@ -315,7 +268,7 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode_cloud_story_enabled) { // mapping viam_carto *vc1; struct viam_carto_config vcc_mapping = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, "", camera, movement_sensor, true, true, ""); + VIAM_CARTO_THREE_D, camera, movement_sensor, true, ""); BOOST_TEST(viam_carto_init(&vc1, lib, vcc_mapping, ac) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc1->slam_mode == VIAM_CARTO_SLAM_MODE_MAPPING); @@ -376,9 +329,9 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode_cloud_story_enabled) { // updating viam_carto *vc2; - struct viam_carto_config vcc_updating = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, "", camera, movement_sensor, true, true, - internal_state_file_path); + struct viam_carto_config vcc_updating = + viam_carto_config_setup(VIAM_CARTO_THREE_D, camera, movement_sensor, + true, internal_state_file_path); BOOST_TEST(viam_carto_init(&vc2, lib, vcc_updating, ac) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc2->slam_mode == VIAM_CARTO_SLAM_MODE_UPDATING); @@ -418,9 +371,9 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode_cloud_story_enabled) { { // updating optimize_on_start viam_carto *vc3; - struct viam_carto_config vcc_updating = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, "", camera, movement_sensor, true, true, - internal_state_file_path); + struct viam_carto_config vcc_updating = + viam_carto_config_setup(VIAM_CARTO_THREE_D, camera, movement_sensor, + true, internal_state_file_path); BOOST_TEST(viam_carto_init(&vc3, lib, vcc_updating, ac_optimize_on_start) == VIAM_CARTO_SUCCESS); @@ -435,9 +388,9 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode_cloud_story_enabled) { { // localizing viam_carto *vc4; - struct viam_carto_config vcc_localizing = viam_carto_config_setup( - 0, VIAM_CARTO_THREE_D, "", camera, movement_sensor, true, false, - internal_state_file_path); + struct viam_carto_config vcc_localizing = + viam_carto_config_setup(VIAM_CARTO_THREE_D, camera, movement_sensor, + false, internal_state_file_path); BOOST_TEST(viam_carto_init(&vc4, lib, vcc_localizing, ac) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc4->slam_mode == VIAM_CARTO_SLAM_MODE_LOCALIZING); @@ -470,9 +423,9 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode_cloud_story_enabled) { { // localizing optimize_on_start viam_carto *vc5; - struct viam_carto_config vcc_localizing = viam_carto_config_setup( - 0, VIAM_CARTO_THREE_D, "", camera, movement_sensor, true, false, - internal_state_file_path); + struct viam_carto_config vcc_localizing = + viam_carto_config_setup(VIAM_CARTO_THREE_D, camera, movement_sensor, + false, internal_state_file_path); BOOST_TEST(viam_carto_init(&vc5, lib, vcc_localizing, ac_optimize_on_start) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc5->slam_mode == VIAM_CARTO_SLAM_MODE_LOCALIZING); @@ -484,215 +437,13 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode_cloud_story_enabled) { } { - // invalid - auto empty_dir = tmp_dir / fs::path(bfs::unique_path().string()); - ; - viam_carto *vc6; - struct viam_carto_config vcc_invalid = viam_carto_config_setup( - 0, VIAM_CARTO_THREE_D, empty_dir.string(), camera, movement_sensor, - false, false, "test.pbstream"); - BOOST_TEST(viam_carto_init(&vc6, lib, vcc_invalid, ac) == - VIAM_CARTO_SLAM_MODE_INVALID); - viam_carto_config_teardown(vcc_invalid); - } - - // TODO: Move all suite level setup & teardown to boost test hook - fs::remove_all(tmp_dir); - - BOOST_TEST(viam_carto_lib_terminate(&lib) == VIAM_CARTO_SUCCESS); -} - -BOOST_AUTO_TEST_CASE(CartoFacade_init_derive_slam_mode) { - viam_carto_lib *lib; - BOOST_TEST(viam_carto_lib_init(&lib, 0, 1) == VIAM_CARTO_SUCCESS); - - std::string camera = "lidar"; - std::string movement_sensor = "movement_sensor"; - fs::path tmp_dir = - fs::temp_directory_path() / fs::path(bfs::unique_path().string()); - struct viam_carto_algo_config ac = viam_carto_algo_config_setup(true); - - fs::create_directory(tmp_dir); - { - // mapping - viam_carto *vc1; - auto mapping_dir = tmp_dir / fs::path("mapping_dir"); - struct viam_carto_config vcc_mapping = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, mapping_dir.string(), - camera, movement_sensor, false, false, ""); - BOOST_TEST(viam_carto_init(&vc1, lib, vcc_mapping, ac) == - VIAM_CARTO_SUCCESS); - BOOST_TEST(vc1->slam_mode == VIAM_CARTO_SLAM_MODE_MAPPING); - viam::carto_facade::CartoFacade *cf1 = - static_cast(vc1->carto_obj); - BOOST_TEST(cf1->slam_mode == SlamMode::MAPPING); - BOOST_TEST(cf1->map_builder.GetOptimizeEveryNNodes() == - ac.optimize_every_n_nodes); - BOOST_TEST(cf1->map_builder.GetNumRangeData() == ac.num_range_data); - BOOST_TEST(cf1->map_builder.GetMissingDataRayLength() == - ac.missing_data_ray_length, - tol); - BOOST_TEST(cf1->map_builder.GetMaxRange() == ac.max_range, tol); - BOOST_TEST(cf1->map_builder.GetMinRange() == ac.min_range, tol); - BOOST_TEST(cf1->map_builder.GetOccupiedSpaceWeight() == - ac.occupied_space_weight, - tol); - BOOST_TEST( - cf1->map_builder.GetTranslationWeight() == ac.translation_weight, - tol); - BOOST_TEST(cf1->map_builder.GetRotationWeight() == ac.rotation_weight, - tol); - // END TEST - BOOST_TEST(viam_carto_terminate(&vc1) == VIAM_CARTO_SUCCESS); - viam_carto_config_teardown(vcc_mapping); - } - - auto updating_dir = tmp_dir / fs::path("updating_dir"); - // updating setup - { - auto internal_state_dir = updating_dir / fs::path("internal_state"); - fs::create_directories(internal_state_dir); - // we need to copy a valid internal state when we boot in updating mode - // otherwise cartographer terminates the entire os process - // see: https://viam.atlassian.net/browse/RSDK-3553 - auto internal_state_artifact_source = - fs::current_path() / - fs::path( - ".artifact/data/viam-cartographer/outputs/viam-office-02-22-3/" - "internal_state/internal_state_0.pbstream"); - - VLOG(1) << "internal_state_artifact_source: " - << internal_state_artifact_source; - VLOG(1) << "exists(internal_state_artifact_source): " - << exists(internal_state_artifact_source); - VLOG(1) << "internal_state_dir: " << internal_state_dir; - VLOG(1) << "exists(internal_state_dir): " << exists(internal_state_dir); - auto internal_state_artifact_target = - internal_state_dir / - fs::path("map_data_2022-02-11T01:44:53.1903Z.pbstream"); - fs::copy_file(internal_state_artifact_source, - internal_state_artifact_target); - } - - { - // updating - viam_carto *vc2; - - struct viam_carto_config vcc_updating = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, updating_dir.string(), camera, - movement_sensor, false, false, ""); - BOOST_TEST(viam_carto_init(&vc2, lib, vcc_updating, ac) == - VIAM_CARTO_SUCCESS); - BOOST_TEST(vc2->slam_mode == VIAM_CARTO_SLAM_MODE_UPDATING); - viam::carto_facade::CartoFacade *cf2 = - static_cast(vc2->carto_obj); - BOOST_TEST(cf2->slam_mode == SlamMode::UPDATING); - BOOST_TEST(cf2->map_builder.GetOptimizeEveryNNodes() == - ac.optimize_every_n_nodes); - BOOST_TEST(cf2->map_builder.GetNumRangeData() == ac.num_range_data); - BOOST_TEST(cf2->map_builder.GetMissingDataRayLength() == - ac.missing_data_ray_length, - tol); - BOOST_TEST(cf2->map_builder.GetMaxRange() == ac.max_range, tol); - BOOST_TEST(cf2->map_builder.GetMinRange() == ac.min_range, tol); - BOOST_TEST(cf2->map_builder.GetFreshSubmapsCount() == - ac.fresh_submaps_count); - BOOST_TEST(cf2->map_builder.GetMinCoveredArea() == ac.min_covered_area, - tol); - BOOST_TEST(cf2->map_builder.GetMinAddedSubmapsCount() == - ac.min_added_submaps_count); - BOOST_TEST(cf2->map_builder.GetOccupiedSpaceWeight() == - ac.occupied_space_weight, - tol); - BOOST_TEST( - cf2->map_builder.GetTranslationWeight() == ac.translation_weight, - tol); - BOOST_TEST(cf2->map_builder.GetRotationWeight() == ac.rotation_weight, - tol); - BOOST_TEST(viam_carto_terminate(&vc2) == VIAM_CARTO_SUCCESS); - viam_carto_config_teardown(vcc_updating); - } - struct viam_carto_algo_config ac_optimize_on_start = - viam_carto_algo_config_setup(true); - ac_optimize_on_start.optimize_on_start = true; - - { - // updating optimize_on_start - viam_carto *vc3; - struct viam_carto_config vcc_updating = viam_carto_config_setup( - 1, VIAM_CARTO_THREE_D, updating_dir.string(), camera, - movement_sensor, false, false, ""); - - BOOST_TEST(viam_carto_init(&vc3, lib, vcc_updating, - ac_optimize_on_start) == VIAM_CARTO_SUCCESS); - BOOST_TEST(vc3->slam_mode == VIAM_CARTO_SLAM_MODE_UPDATING); - viam::carto_facade::CartoFacade *cf2 = - static_cast(vc3->carto_obj); - BOOST_TEST(cf2->slam_mode == SlamMode::UPDATING); - BOOST_TEST(viam_carto_terminate(&vc3) == VIAM_CARTO_SUCCESS); - viam_carto_config_teardown(vcc_updating); - } - - { - // localizing - viam_carto *vc4; - struct viam_carto_config vcc_localizing = viam_carto_config_setup( - 0, VIAM_CARTO_THREE_D, updating_dir.string(), camera, - movement_sensor, false, false, ""); - BOOST_TEST(viam_carto_init(&vc4, lib, vcc_localizing, ac) == - VIAM_CARTO_SUCCESS); - BOOST_TEST(vc4->slam_mode == VIAM_CARTO_SLAM_MODE_LOCALIZING); - viam::carto_facade::CartoFacade *cf3 = - static_cast(vc4->carto_obj); - BOOST_TEST(cf3->slam_mode == SlamMode::LOCALIZING); - BOOST_TEST(cf3->map_builder.GetOptimizeEveryNNodes() == - ac.optimize_every_n_nodes); - BOOST_TEST(cf3->map_builder.GetNumRangeData() == ac.num_range_data); - BOOST_TEST(cf3->map_builder.GetMissingDataRayLength() == - ac.missing_data_ray_length, - tol); - BOOST_TEST(cf3->map_builder.GetMaxRange() == ac.max_range, tol); - BOOST_TEST(cf3->map_builder.GetMinRange() == ac.min_range, tol); - BOOST_TEST(cf3->map_builder.GetMaxSubmapsToKeep() == - ac.max_submaps_to_keep); - BOOST_TEST(cf3->map_builder.GetOccupiedSpaceWeight() == - ac.occupied_space_weight, - tol); - BOOST_TEST( - cf3->map_builder.GetTranslationWeight() == ac.translation_weight, - tol); - BOOST_TEST(cf3->map_builder.GetRotationWeight() == ac.rotation_weight, - tol); - BOOST_TEST(viam_carto_terminate(&vc4) == VIAM_CARTO_SUCCESS); - viam_carto_config_teardown(vcc_localizing); - } - - { - // localizing optimize_on_start - viam_carto *vc5; - struct viam_carto_config vcc_localizing = viam_carto_config_setup( - 0, VIAM_CARTO_THREE_D, updating_dir.string(), camera, - movement_sensor, false, false, ""); - BOOST_TEST(viam_carto_init(&vc5, lib, vcc_localizing, - ac_optimize_on_start) == VIAM_CARTO_SUCCESS); - BOOST_TEST(vc5->slam_mode == VIAM_CARTO_SLAM_MODE_LOCALIZING); - viam::carto_facade::CartoFacade *cf3 = - static_cast(vc5->carto_obj); - BOOST_TEST(cf3->slam_mode == SlamMode::LOCALIZING); - BOOST_TEST(viam_carto_terminate(&vc5) == VIAM_CARTO_SUCCESS); - viam_carto_config_teardown(vcc_localizing); - } - - { - // invalid - auto empty_dir = tmp_dir / fs::path(bfs::unique_path().string()); - ; + // invalid file path viam_carto *vc6; struct viam_carto_config vcc_invalid = - viam_carto_config_setup(0, VIAM_CARTO_THREE_D, empty_dir.string(), - camera, movement_sensor, false, false, ""); + viam_carto_config_setup(VIAM_CARTO_THREE_D, camera, movement_sensor, + false, "test.pbstream"); BOOST_TEST(viam_carto_init(&vc6, lib, vcc_invalid, ac) == - VIAM_CARTO_SLAM_MODE_INVALID); + VIAM_CARTO_INTERNAL_STATE_FILE_SYSTEM_ERROR); viam_carto_config_teardown(vcc_invalid); } @@ -710,11 +461,8 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_terminate_without_movement_sensor) { viam_carto *vc; std::string camera = "lidar"; std::string movement_sensor = ""; - fs::path tmp_dir = - fs::temp_directory_path() / fs::path(bfs::unique_path().string()); - struct viam_carto_config vcc = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera, - movement_sensor, false, false, ""); + struct viam_carto_config vcc = viam_carto_config_setup( + VIAM_CARTO_THREE_D, camera, movement_sensor, true, ""); struct viam_carto_algo_config ac = viam_carto_algo_config_setup(false); BOOST_TEST(viam_carto_init(&vc, lib, vcc, ac) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc->slam_mode == VIAM_CARTO_SLAM_MODE_MAPPING); @@ -734,20 +482,15 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_terminate_without_movement_sensor) { BOOST_TEST((cf->algo_config.occupied_space_weight) == 20, tol); BOOST_TEST((cf->algo_config.translation_weight) == 10, tol); BOOST_TEST((cf->algo_config.rotation_weight) == 1, tol); - auto path_to_internal_state = tmp_dir / fs::path("internal_state"); - BOOST_TEST((cf->path_to_internal_state) == path_to_internal_state.string()); BOOST_TEST(((cf->state) == CartoFacadeState::IO_INITIALIZED)); BOOST_TEST((cf->config.camera) == camera); BOOST_TEST((cf->config.movement_sensor) == movement_sensor); - BOOST_TEST((cf->config.map_rate_sec).count() == 1); - BOOST_TEST((cf->config.data_dir) == tmp_dir.string()); BOOST_TEST(to_std_string(cf->config.component_reference) == "lidar"); BOOST_TEST((cf->config.lidar_config) == VIAM_CARTO_THREE_D); BOOST_TEST(viam_carto_terminate(&vc) == VIAM_CARTO_SUCCESS); viam_carto_config_teardown(vcc); - fs::remove_all(tmp_dir); // library terminate BOOST_TEST(viam_carto_lib_terminate(&lib) == VIAM_CARTO_SUCCESS); @@ -762,12 +505,8 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo_without_movement_sensor) { viam_carto *vc; std::string camera = "lidar"; std::string movement_sensor = ""; - fs::path tmp_dir = - fs::temp_directory_path() / fs::path(bfs::unique_path().string()); - struct viam_carto_config vcc = - viam_carto_config_setup(60, VIAM_CARTO_THREE_D, tmp_dir.string(), - - camera, movement_sensor, false, false, ""); + struct viam_carto_config vcc = viam_carto_config_setup( + VIAM_CARTO_THREE_D, camera, movement_sensor, true, ""); struct viam_carto_algo_config ac = viam_carto_algo_config_setup(false); BOOST_TEST(viam_carto_init(&vc, lib, vcc, ac) == VIAM_CARTO_SUCCESS); @@ -1147,7 +886,6 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo_without_movement_sensor) { // Terminate BOOST_TEST(viam_carto_terminate(&vc) == VIAM_CARTO_SUCCESS); viam_carto_config_teardown(vcc); - fs::remove_all(tmp_dir); // library terminate BOOST_TEST(viam_carto_lib_terminate(&lib) == VIAM_CARTO_SUCCESS); @@ -1160,28 +898,20 @@ BOOST_AUTO_TEST_CASE(CartoFacade_config_without_movement_sensor) { std::string camera = "lidar"; std::string movement_sensor = ""; - fs::path tmp_dir = - fs::temp_directory_path() / fs::path(bfs::unique_path().string()); - struct viam_carto_config vcc = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, tmp_dir.string(), - - camera, movement_sensor, true, true, ""); + struct viam_carto_config vcc = viam_carto_config_setup( + VIAM_CARTO_THREE_D, camera, movement_sensor, true, ""); struct config c = viam::carto_facade::from_viam_carto_config(vcc); BOOST_TEST(to_std_string(c.component_reference) == "lidar"); - BOOST_TEST(c.data_dir == tmp_dir.string()); BOOST_TEST(c.lidar_config == VIAM_CARTO_THREE_D); - BOOST_TEST(c.map_rate_sec.count() == 1); BOOST_TEST(c.camera == "lidar"); BOOST_TEST(c.movement_sensor == ""); - BOOST_TEST(c.cloud_story_enabled == true); BOOST_TEST(c.enable_mapping == true); viam_carto_config_teardown(vcc); BOOST_TEST(bdestroy(c.component_reference) == BSTR_OK); - fs::remove_all(tmp_dir); // library terminate BOOST_TEST(viam_carto_lib_terminate(&lib) == VIAM_CARTO_SUCCESS); } @@ -1199,12 +929,8 @@ BOOST_AUTO_TEST_CASE(CartoFacade_start_stop_without_movement_sensor) { viam_carto *vc; std::string camera = "lidar"; std::string movement_sensor = ""; - fs::path tmp_dir = - fs::temp_directory_path() / fs::path(bfs::unique_path().string()); - struct viam_carto_config vcc = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, tmp_dir.string(), - - camera, movement_sensor, false, false, ""); + struct viam_carto_config vcc = viam_carto_config_setup( + VIAM_CARTO_THREE_D, camera, movement_sensor, true, ""); struct viam_carto_algo_config ac = viam_carto_algo_config_setup(false); BOOST_TEST(viam_carto_init(&vc, lib, vcc, ac) == VIAM_CARTO_SUCCESS); @@ -1213,22 +939,10 @@ BOOST_AUTO_TEST_CASE(CartoFacade_start_stop_without_movement_sensor) { static_cast(vc->carto_obj); BOOST_TEST(((cf->state) == CartoFacadeState::IO_INITIALIZED)); - BOOST_TEST(fs::is_directory(cf->path_to_internal_state)); - BOOST_TEST(fs::is_empty(cf->path_to_internal_state)); - // Start BOOST_TEST(viam_carto_start(vc) == VIAM_CARTO_SUCCESS); BOOST_TEST(((cf->state) == CartoFacadeState::STARTED)); - // Confirm at least one map is persisted within the map_rate_sec - VLOG(1) << "path_to_internal_state: " << cf->path_to_internal_state; - // TODO: This should busy wait until this condition happens & the tests - // passes or 2 seconds goes by & the test fails. - std::this_thread::sleep_for(cf->config.map_rate_sec + - std::chrono::seconds(1)); - BOOST_TEST(fs::is_directory(cf->path_to_internal_state)); - BOOST_TEST(!fs::is_empty(cf->path_to_internal_state)); - // Stop BOOST_TEST(viam_carto_stop(vc) == VIAM_CARTO_SUCCESS); BOOST_TEST(((cf->state) == CartoFacadeState::IO_INITIALIZED)); @@ -1236,7 +950,6 @@ BOOST_AUTO_TEST_CASE(CartoFacade_start_stop_without_movement_sensor) { // Terminate BOOST_TEST(viam_carto_terminate(&vc) == VIAM_CARTO_SUCCESS); viam_carto_config_teardown(vcc); - fs::remove_all(tmp_dir); // library terminate BOOST_TEST(viam_carto_lib_terminate(&lib) == VIAM_CARTO_SUCCESS); @@ -1250,11 +963,8 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_terminate_with_movement_sensor) { viam_carto *vc; std::string camera = "lidar"; std::string movement_sensor = "movement_sensor"; - fs::path tmp_dir = - fs::temp_directory_path() / fs::path(bfs::unique_path().string()); - struct viam_carto_config vcc = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera, - movement_sensor, false, false, ""); + struct viam_carto_config vcc = viam_carto_config_setup( + VIAM_CARTO_THREE_D, camera, movement_sensor, true, ""); struct viam_carto_algo_config ac = viam_carto_algo_config_setup(true); BOOST_TEST(viam_carto_init(&vc, lib, vcc, ac) == VIAM_CARTO_SUCCESS); BOOST_TEST(vc->slam_mode == VIAM_CARTO_SLAM_MODE_MAPPING); @@ -1275,20 +985,15 @@ BOOST_AUTO_TEST_CASE(CartoFacade_init_terminate_with_movement_sensor) { BOOST_TEST((cf->algo_config.occupied_space_weight) == 20, tol); BOOST_TEST((cf->algo_config.translation_weight) == 10, tol); BOOST_TEST((cf->algo_config.rotation_weight) == 1, tol); - auto path_to_internal_state = tmp_dir / fs::path("internal_state"); - BOOST_TEST((cf->path_to_internal_state) == path_to_internal_state.string()); BOOST_TEST(((cf->state) == CartoFacadeState::IO_INITIALIZED)); BOOST_TEST((cf->config.camera) == camera); BOOST_TEST((cf->config.movement_sensor) == movement_sensor); - BOOST_TEST((cf->config.map_rate_sec).count() == 1); - BOOST_TEST((cf->config.data_dir) == tmp_dir.string()); BOOST_TEST(to_std_string(cf->config.component_reference) == "lidar"); BOOST_TEST((cf->config.lidar_config) == VIAM_CARTO_THREE_D); BOOST_TEST(viam_carto_terminate(&vc) == VIAM_CARTO_SUCCESS); viam_carto_config_teardown(vcc); - fs::remove_all(tmp_dir); // library terminate BOOST_TEST(viam_carto_lib_terminate(&lib) == VIAM_CARTO_SUCCESS); @@ -1303,12 +1008,8 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo_with_movement_sensor) { viam_carto *vc; std::string camera = "lidar"; std::string movement_sensor = "movement_sensor"; - fs::path tmp_dir = - fs::temp_directory_path() / fs::path(bfs::unique_path().string()); - struct viam_carto_config vcc = - viam_carto_config_setup(60, VIAM_CARTO_THREE_D, tmp_dir.string(), - - camera, movement_sensor, false, false, ""); + struct viam_carto_config vcc = viam_carto_config_setup( + VIAM_CARTO_THREE_D, camera, movement_sensor, true, ""); struct viam_carto_algo_config ac = viam_carto_algo_config_setup(true); BOOST_TEST(viam_carto_init(&vc, lib, vcc, ac) == VIAM_CARTO_SUCCESS); @@ -1867,7 +1568,6 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo_with_movement_sensor) { // Terminate BOOST_TEST(viam_carto_terminate(&vc) == VIAM_CARTO_SUCCESS); viam_carto_config_teardown(vcc); - fs::remove_all(tmp_dir); // library terminate BOOST_TEST(viam_carto_lib_terminate(&lib) == VIAM_CARTO_SUCCESS); @@ -1880,27 +1580,20 @@ BOOST_AUTO_TEST_CASE(CartoFacade_config_with_movement_sensor) { std::string camera = "lidar"; std::string movement_sensor = "movement_sensor"; - fs::path tmp_dir = - fs::temp_directory_path() / fs::path(bfs::unique_path().string()); - struct viam_carto_config vcc = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera, - movement_sensor, true, true, ""); + struct viam_carto_config vcc = viam_carto_config_setup( + VIAM_CARTO_THREE_D, camera, movement_sensor, true, ""); struct config c = viam::carto_facade::from_viam_carto_config(vcc); BOOST_TEST(to_std_string(c.component_reference) == "lidar"); - BOOST_TEST(c.data_dir == tmp_dir.string()); BOOST_TEST(c.lidar_config == VIAM_CARTO_THREE_D); - BOOST_TEST(c.map_rate_sec.count() == 1); BOOST_TEST(c.camera == "lidar"); BOOST_TEST(c.movement_sensor == "movement_sensor"); - BOOST_TEST(c.cloud_story_enabled == true); BOOST_TEST(c.enable_mapping == true); viam_carto_config_teardown(vcc); BOOST_TEST(bdestroy(c.component_reference) == BSTR_OK); - fs::remove_all(tmp_dir); // library terminate BOOST_TEST(viam_carto_lib_terminate(&lib) == VIAM_CARTO_SUCCESS); } @@ -1918,11 +1611,8 @@ BOOST_AUTO_TEST_CASE(CartoFacade_start_stop_with_movement_sensor) { viam_carto *vc; std::string camera = "lidar"; std::string movement_sensor = "movement_sensor"; - fs::path tmp_dir = - fs::temp_directory_path() / fs::path(bfs::unique_path().string()); - struct viam_carto_config vcc = - viam_carto_config_setup(1, VIAM_CARTO_THREE_D, tmp_dir.string(), camera, - movement_sensor, false, false, ""); + struct viam_carto_config vcc = viam_carto_config_setup( + VIAM_CARTO_THREE_D, camera, movement_sensor, true, ""); struct viam_carto_algo_config ac = viam_carto_algo_config_setup(true); BOOST_TEST(viam_carto_init(&vc, lib, vcc, ac) == VIAM_CARTO_SUCCESS); @@ -1931,22 +1621,10 @@ BOOST_AUTO_TEST_CASE(CartoFacade_start_stop_with_movement_sensor) { static_cast(vc->carto_obj); BOOST_TEST(((cf->state) == CartoFacadeState::IO_INITIALIZED)); - BOOST_TEST(fs::is_directory(cf->path_to_internal_state)); - BOOST_TEST(fs::is_empty(cf->path_to_internal_state)); - // Start BOOST_TEST(viam_carto_start(vc) == VIAM_CARTO_SUCCESS); BOOST_TEST(((cf->state) == CartoFacadeState::STARTED)); - // Confirm at least one map is persisted within the map_rate_sec - VLOG(1) << "path_to_internal_state: " << cf->path_to_internal_state; - // TODO: This should busy wait until this condition happens & the tests - // passes or 2 seconds goes by & the test fails. - std::this_thread::sleep_for(cf->config.map_rate_sec + - std::chrono::seconds(1)); - BOOST_TEST(fs::is_directory(cf->path_to_internal_state)); - BOOST_TEST(!fs::is_empty(cf->path_to_internal_state)); - // Stop BOOST_TEST(viam_carto_stop(vc) == VIAM_CARTO_SUCCESS); BOOST_TEST(((cf->state) == CartoFacadeState::IO_INITIALIZED)); @@ -1954,7 +1632,6 @@ BOOST_AUTO_TEST_CASE(CartoFacade_start_stop_with_movement_sensor) { // Terminate BOOST_TEST(viam_carto_terminate(&vc) == VIAM_CARTO_SUCCESS); viam_carto_config_teardown(vcc); - fs::remove_all(tmp_dir); // library terminate BOOST_TEST(viam_carto_lib_terminate(&lib) == VIAM_CARTO_SUCCESS); diff --git a/viam-cartographer/src/carto_facade/io.cc b/viam-cartographer/src/carto_facade/io.cc deleted file mode 100644 index e7d38410a..000000000 --- a/viam-cartographer/src/carto_facade/io.cc +++ /dev/null @@ -1,110 +0,0 @@ -// This is an experimental integration of cartographer into RDK. -#include "io.h" - -#include // pcl::PCDReader -#include - -#include -#include // std::ifstream -#include -#include - -#include "glog/logging.h" - -namespace viam { -namespace carto_facade { -namespace io { - -namespace fs = boost::filesystem; - -const std::string MakeFilenameWithTimestamp(std::string path_to_dir, - std::time_t t) { - char timestamp[100]; - std::strftime(timestamp, sizeof(timestamp), time_format.c_str(), - std::gmtime(&t)); - return path_to_dir + "/" + "map_data_" + timestamp + ".pbstream"; -} - -cartographer::sensor::TimedPointCloudData TimedPointCloudDataFromPCDBuilder( - std::string file_path, double start_time) { - pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); - - cartographer::sensor::TimedPointCloudData timed_pcd; - cartographer::sensor::TimedPointCloud ranges; - - // Open the point cloud file - pcl::PointCloud::Ptr cloud( - new pcl::PointCloud); - auto err = pcl::io::loadPCDFile(file_path, *cloud); - - if (err == -1) { - return timed_pcd; - } - - double current_time = ReadTimeFromTimestamp(file_path.substr( - file_path.find(filename_prefix) + filename_prefix.length(), - file_path.find(".pcd"))); - double time_delta = current_time - start_time; - - VLOG(1) << "Accessing file " << file_path << " ... "; - VLOG(1) << "Loaded " << cloud->width * cloud->height << " data points"; - - for (size_t i = 0; i < cloud->points.size(); ++i) { - cartographer::sensor::TimedRangefinderPoint timed_rangefinder_point; - timed_rangefinder_point.position = Eigen::Vector3f( - cloud->points[i].x, cloud->points[i].y, cloud->points[i].z); - timed_rangefinder_point.time = 0 - i * 0.0001; - - ranges.push_back(timed_rangefinder_point); - } - - timed_pcd.time = cartographer::common::FromUniversal(123) + - cartographer::common::FromSeconds(double(time_delta)); - timed_pcd.origin = Eigen::Vector3f::Zero(); - timed_pcd.ranges = ranges; - - return timed_pcd; -} - -double ReadTimeFromTimestamp(std::string timestamp) { - std::string::size_type sz; - auto partial_time_format = time_format.substr(0, time_format.find(".")); - // Create a stream which we will use to parse the string - std::istringstream ss(timestamp); - - // Create a tm object to store the parsed date and time. - std::tm dt = {0}; - - // Now we read from buffer using get_time manipulator - // and formatting the input appropriately. - ss >> std::get_time(&dt, partial_time_format.c_str()); - if (ss.fail()) { - throw std::runtime_error( - "timestamp cannot be parsed into a std::tm object: " + timestamp); - } - double timestamp_time = (double)timegm(&dt); - if (timestamp_time == -1) { - throw std::runtime_error( - "timestamp cannot be represented as a std::time_t object: " + - timestamp); - } - auto sub_sec_index = timestamp.find("."); - if ((sub_sec_index != std::string::npos)) { - double sub_sec = 0; - try { - sub_sec = (double)std::stof(timestamp.substr(sub_sec_index), &sz); - } catch (std::exception &e) { - LOG(FATAL) << e.what(); - throw std::runtime_error( - "could not extract sub seconds from timestamp: " + timestamp); - } - double timestamp_time_w_sub_sec = timestamp_time + sub_sec; - return timestamp_time_w_sub_sec; - } else { - return timestamp_time; - } -} - -} // namespace io -} // namespace carto_facade -} // namespace viam diff --git a/viam-cartographer/src/carto_facade/io.h b/viam-cartographer/src/carto_facade/io.h deleted file mode 100644 index d8e231653..000000000 --- a/viam-cartographer/src/carto_facade/io.h +++ /dev/null @@ -1,39 +0,0 @@ -// This is an experimental integration of cartographer into RDK. -#ifndef VIAM_CARTO_FACADE_IO_H -#define VIAM_CARTO_FACADE_IO_H - -#include - -#include -#include -#include -#include -#include - -#include "cartographer/sensor/timed_point_cloud_data.h" - -namespace viam { -namespace carto_facade { -namespace io { -static const std::string filename_prefix = "_data_"; -static const std::string time_format = "%Y-%m-%dT%H:%M:%S.0000Z"; - -// MakeFilenameWithTimestamp creates a filename for a provided sensor with a -// timestamp. The filename includes the path to the file. Does not support -// millisecond resolution. -const std::string MakeFilenameWithTimestamp(std::string path_to_dir, - std::time_t t); - -// TimedPointCloudDataFromPCDBuilder creates a TimedPointCloudData object -// from a PCD file. -cartographer::sensor::TimedPointCloudData TimedPointCloudDataFromPCDBuilder( - std::string file_path, double start_time); - -// Converts UTC time string to a double value. -double ReadTimeFromTimestamp(std::string timestamp); - -} // namespace io -} // namespace carto_facade -} // namespace viam - -#endif // VIAM_CARTO_FACADE_IO_H diff --git a/viam-cartographer/src/carto_facade/io_test.cc b/viam-cartographer/src/carto_facade/io_test.cc deleted file mode 100644 index b69cfc17e..000000000 --- a/viam-cartographer/src/carto_facade/io_test.cc +++ /dev/null @@ -1,138 +0,0 @@ -#include "io.h" - -#include -#include -#include -#include -#include - -namespace viam { -namespace carto_facade { -namespace io { - -BOOST_AUTO_TEST_SUITE(CartoFacade_io) - -BOOST_AUTO_TEST_CASE(MakeFilenameWithTimestamp_success) { - std::string path_to_dir = "path_to_dir"; - - std::time_t t = std::time(nullptr); - std::string filename = MakeFilenameWithTimestamp(path_to_dir, t); - - // Check if the filename beginning is as expected - std::string path_prefix = "/map_data_"; - std::string filename_start = - filename.substr(0, path_to_dir.length() + path_prefix.length()); - BOOST_TEST(filename_start.compare(path_to_dir + path_prefix) == 0); - // Extract timestamp - double filename_time = ReadTimeFromTimestamp(filename.substr( - filename.find(filename_prefix) + filename_prefix.length(), - filename.find(".pcd"))); - - BOOST_TEST((double)t == filename_time); -} - -BOOST_AUTO_TEST_CASE(TimedPointCloudDataFromPCDBuilder_success) { - // Create a mini PCD file and save it in a tmp directory - std::string filename = "rplidar_data_2022-01-01T01:00:00.0001Z.pcd"; - std::vector> points = { - {-0.001000, 0.002000, 0.005000, 16711938}, - {0.582000, 0.012000, 0.000000, 16711938}, - {0.007000, 0.006000, 0.001000, 16711938}}; - std::string pcd = - "VERSION .7\n" - "FIELDS x y z rgb\n" - "SIZE 4 4 4 4\n" - "TYPE F F F I\n" - "COUNT 1 1 1 1\n" - "WIDTH 3\n" - "HEIGHT 1\n" - "VIEWPOINT 0 0 0 1 0 0 0\n" - "POINTS 3\n" - "DATA ascii\n"; - for (std::vector point : points) { - for (int i = 0; i < 3; i++) { - pcd = pcd + std::to_string(point.at(i)) + " "; - } - pcd = pcd + std::to_string(point.at(3)) + "\n"; - } - // Create a unique path in the temp directory and add the PCD file - boost::filesystem::path tmp_dir = boost::filesystem::temp_directory_path() / - boost::filesystem::unique_path(); - bool ok = boost::filesystem::create_directory(tmp_dir); - if (!ok) { - throw std::runtime_error("could not create directory: " + - tmp_dir.string()); - } - boost::filesystem::ofstream ofs(tmp_dir / filename); - ofs << pcd; - ofs.close(); - // Read it in and check if the data in the TimedPointCloudData is equivalent - // to what we had in the pcd file - cartographer::sensor::TimedPointCloudData timed_pcd = - TimedPointCloudDataFromPCDBuilder(tmp_dir.string() + "/" + filename, 0); - - auto tolerance = boost::test_tools::tolerance(0.00001); - BOOST_TEST(timed_pcd.ranges.size() == points.size()); - for (int i = 0; i < points.size(); i++) { - cartographer::sensor::TimedRangefinderPoint timed_rangefinder_point = - timed_pcd.ranges.at(i); - for (int j = 0; j < 3; j++) { - BOOST_TEST( - timed_rangefinder_point.position(j, 0) == points.at(i).at(j), - tolerance); - } - } - - // Remove the temporary directory and its contents - boost::filesystem::remove_all(tmp_dir); -} - -BOOST_AUTO_TEST_CASE(ReadTimeFromTimestamp_missing_timestamp) { - // Provide a filename with a missing timestamp - std::string timestamp = "no-timestamp"; - const std::string message = - "timestamp cannot be parsed into a std::tm object: " + timestamp; - BOOST_CHECK_EXCEPTION(ReadTimeFromTimestamp(timestamp), std::runtime_error, - [&message](const std::runtime_error& ex) { - BOOST_CHECK_EQUAL(ex.what(), message); - return true; - }); -} - -BOOST_AUTO_TEST_CASE(ReadTimeFromTimestamp_success) { - // Provide a filename with a timestamp - std::time_t t = std::time(nullptr); - char timestamp[100]; - std::strftime(timestamp, sizeof(timestamp), time_format.c_str(), - std::gmtime(&t)); - std::string filename_prefix = "rplidar_data_"; - std::string filename_type = ".pcd"; - std::string filename = - filename_prefix + std::string(timestamp) + filename_type; - // Read the time - std::string timestamp_str = filename.substr( - filename.find(filename_prefix) + filename_prefix.length(), - filename.find(filename_type)); - double filename_time = ReadTimeFromTimestamp(timestamp_str); - auto tolerance = boost::test_tools::tolerance(0.0001); - // Make sure the time read from the filename equals what we put into the - // filename - BOOST_TEST((double)t == filename_time, tolerance); -} - -BOOST_AUTO_TEST_CASE(ReadTimeFromTimestamp_comparison) { - const std::string timestamp_1 = "2022-01-01T01:00:00.0000Z"; - const std::string timestamp_2 = "2022-01-01T01:00:00.0001Z"; - const std::string timestamp_3 = "2022-01-01T01:00:01.0000Z"; - const auto time_1 = ReadTimeFromTimestamp(timestamp_1); - const auto time_2 = ReadTimeFromTimestamp(timestamp_2); - const auto time_3 = ReadTimeFromTimestamp(timestamp_3); - BOOST_TEST(time_1 < time_2); - BOOST_TEST(time_2 < time_3); -} - -BOOST_AUTO_TEST_SUITE_END() - -} // namespace io -} // namespace carto_facade -} // namespace viam diff --git a/viam-cartographer/src/carto_facade/map_builder.cc b/viam-cartographer/src/carto_facade/map_builder.cc index fe850dfc6..a0482e0b1 100644 --- a/viam-cartographer/src/carto_facade/map_builder.cc +++ b/viam-cartographer/src/carto_facade/map_builder.cc @@ -11,7 +11,6 @@ #include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "glog/logging.h" -#include "io.h" #include "map_builder.h" namespace viam { @@ -141,25 +140,6 @@ MapBuilder::GetLocalSlamResultCallback() { }; } -void MapBuilder::SetStartTime(double input_start_time) { - start_time = input_start_time; -} - -cartographer::sensor::TimedPointCloudData MapBuilder::GetDataFromFile( - std::string file) { - cartographer::sensor::TimedPointCloudData point_cloud; - - if (start_time == -1) { - throw std::runtime_error("start_time has not been initialized"); - } - point_cloud = viam::carto_facade::io::TimedPointCloudDataFromPCDBuilder( - file, start_time); - - return point_cloud; -} - -// TODO: There might still be a lot of room to improve accuracy & speed. -// Might be worth investigating in the future. cartographer::transform::Rigid3d MapBuilder::GetGlobalPose() { auto local_transform = map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id); diff --git a/viam-cartographer/src/carto_facade/map_builder.h b/viam-cartographer/src/carto_facade/map_builder.h index ae10389ef..cc2304bfd 100644 --- a/viam-cartographer/src/carto_facade/map_builder.h +++ b/viam-cartographer/src/carto_facade/map_builder.h @@ -28,6 +28,7 @@ const SensorId kOdometerSensorId{SensorId::SensorType::ODOMETRY, "odometry"}; class MapBuilder { public: + MapBuilder() : trajectory_builder(){}; ~MapBuilder(); // SetUp reads in the cartographer parameters via reading in the lua files. void SetUp(std::string configuration_directory, @@ -49,25 +50,8 @@ class MapBuilder { bool SaveMapToFile(bool include_unfinished_submaps, const std::string filename_with_timestamp); - // SaveMapToStream converted the saved pbstream to a stream and deletes the - // file. - std::string ConvertSavedMapToStream( - const std::string filename_with_timestamp, std::string *buffer); - - // TryFileClose attempts to close an opened ifstream, returning an error - // string if it fails. - std::string TryFileClose(std::ifstream &file, std::string filename); - void StartTrajectoryBuilder(bool use_imu_data); - // SetStartTime sets the start_time to the time stamp from the first sensor - // file that is being read in. - void SetStartTime(double input_start_time); - - // GetDataFromFile creates a TimedPointCloudData object from reading in - // a PCD file. - cartographer::sensor::TimedPointCloudData GetDataFromFile(std::string file); - // GetGlobalPose returns the local pose based on the provided a local pose. cartographer::transform::Rigid3d GetGlobalPose(); @@ -128,7 +112,6 @@ class MapBuilder { ::cartographer::transform::Rigid3d local_slam_result_pose = cartographer::transform::Rigid3d(); ; - double start_time = -1; }; } // namespace carto_facade } // namespace viam diff --git a/viam-cartographer/src/carto_facade/test_helpers.cc b/viam-cartographer/src/carto_facade/test_helpers.cc index 6f2b792e5..72af01e20 100644 --- a/viam-cartographer/src/carto_facade/test_helpers.cc +++ b/viam-cartographer/src/carto_facade/test_helpers.cc @@ -76,17 +76,6 @@ std::string ascii_pcd(std::vector> points) { return pcd; } -boost::filesystem::path make_tmp_dir() { - boost::filesystem::path tmp_dir = boost::filesystem::temp_directory_path() / - boost::filesystem::unique_path(); - bool ok = boost::filesystem::create_directory(tmp_dir); - if (!ok) { - throw std::runtime_error("could not create directory: " + - tmp_dir.string()); - } - return tmp_dir; -} - } // namespace test_helpers } // namespace carto_facade } // namespace viam diff --git a/viam-cartographer/src/carto_facade/test_helpers.h b/viam-cartographer/src/carto_facade/test_helpers.h index 7234ca66a..2f5812718 100644 --- a/viam-cartographer/src/carto_facade/test_helpers.h +++ b/viam-cartographer/src/carto_facade/test_helpers.h @@ -18,7 +18,6 @@ std::string binary_pcd(std::vector> points); std::string ascii_pcd(std::vector> points); -boost::filesystem::path make_tmp_dir(); } // namespace test_helpers } // namespace carto_facade } // namespace viam diff --git a/viam-cartographer/src/carto_facade/util_test.cc b/viam-cartographer/src/carto_facade/util_test.cc index 5ae76bbb6..4e7757abf 100644 --- a/viam-cartographer/src/carto_facade/util_test.cc +++ b/viam-cartographer/src/carto_facade/util_test.cc @@ -8,7 +8,6 @@ #include #include -#include "io.h" #include "test_helpers.h" namespace help = viam::carto_facade::test_helpers; @@ -133,32 +132,11 @@ BOOST_AUTO_TEST_CASE(carto_lidar_reading_wrong_shape_binary_failure) { } BOOST_AUTO_TEST_CASE(carto_lidar_reading_binary_success) { - // Create a mini PCD file and save it in a tmp directory - std::string filename = "rplidar_data_2022-01-01T01:00:00.0001Z.pcd"; std::vector> points = { {-0.001000, 0.002000, 0.005000, 16711938}, {0.582000, 0.012000, 0.000000, 16711938}, {0.007000, 0.006000, 0.001000, 16711938}}; std::string pcd = help::binary_pcd(points); - boost::filesystem::path tmp_dir = help::make_tmp_dir(); - // Create a unique path in the temp directory and add the PCD file - boost::filesystem::ofstream ofs(tmp_dir / filename); - ofs << pcd; - ofs.close(); - // Read it in and check if the data in the TimedPointCloudData is equivalent - // to what we had in the pcd file - cartographer::sensor::TimedPointCloudData timed_pcd = - viam::carto_facade::io::TimedPointCloudDataFromPCDBuilder( - tmp_dir.string() + "/" + filename, 0); - - BOOST_TEST(timed_pcd.ranges.size() == points.size()); - help::timed_pcd_contains(timed_pcd, points); - BOOST_TEST(timed_pcd.origin == Eigen::Vector3f::Zero()); - BOOST_TEST(timed_pcd.time == - cartographer::common::FromUniversal(16409988000001121)); - // Remove the temporary directory and its contents - boost::filesystem::remove_all(tmp_dir); - auto [success, timed_pcd_from_string] = carto_lidar_reading(pcd, 16409988000001121); BOOST_TEST(success); @@ -170,32 +148,11 @@ BOOST_AUTO_TEST_CASE(carto_lidar_reading_binary_success) { } BOOST_AUTO_TEST_CASE(carto_lidar_reading_ascii_success) { - // Create a mini PCD file and save it in a tmp directory - std::string filename = "rplidar_data_2022-01-01T01:00:00.0001Z.pcd"; std::vector> points = { {-0.001000, 0.002000, 0.005000, 16711938}, {0.582000, 0.012000, 0.000000, 16711938}, {0.007000, 0.006000, 0.001000, 16711938}}; std::string pcd = help::ascii_pcd(points); - // Create a unique path in the temp directory and add the PCD file - boost::filesystem::path tmp_dir = help::make_tmp_dir(); - boost::filesystem::ofstream ofs(tmp_dir / filename); - ofs << pcd; - ofs.close(); - // Read it in and check if the data in the TimedPointCloudData is equivalent - // to what we had in the pcd file - cartographer::sensor::TimedPointCloudData timed_pcd = - viam::carto_facade::io::TimedPointCloudDataFromPCDBuilder( - tmp_dir.string() + "/" + filename, 0); - - BOOST_TEST(timed_pcd.ranges.size() == points.size()); - help::timed_pcd_contains(timed_pcd, points); - BOOST_TEST(timed_pcd.origin == Eigen::Vector3f::Zero()); - BOOST_TEST(timed_pcd.time == - cartographer::common::FromUniversal(16409988000001121)); - // Remove the temporary directory and its contents - boost::filesystem::remove_all(tmp_dir); - auto [success, timed_pcd_from_string] = carto_lidar_reading(pcd, 16409988000001121); BOOST_TEST(success); diff --git a/viam_cartographer.go b/viam_cartographer.go index 5e1f0eced..cfc33318f 100644 --- a/viam_cartographer.go +++ b/viam_cartographer.go @@ -5,6 +5,8 @@ package viamcartographer import ( "bytes" "context" + "os" + "path/filepath" "strconv" "sync" "sync/atomic" @@ -22,6 +24,7 @@ import ( "github.com/viamrobotics/viam-cartographer/cartofacade" vcConfig "github.com/viamrobotics/viam-cartographer/config" + "github.com/viamrobotics/viam-cartographer/postprocess" "github.com/viamrobotics/viam-cartographer/sensorprocess" s "github.com/viamrobotics/viam-cartographer/sensors" ) @@ -34,6 +37,12 @@ var ( ErrClosed = errors.Errorf("resource (%s) is closed", Model.String()) // ErrUseCloudSlamEnabled denotes that the slam service method was called while use_cloud_slam was set to true. ErrUseCloudSlamEnabled = errors.Errorf("resource (%s) unavailable, configured with use_cloud_slam set to true", Model.String()) + // ErrNoPostprocessingToUndo denotes that the points have not been properly formatted. + ErrNoPostprocessingToUndo = errors.New("there are no postprocessing tasks to undo") + // ErrBadPostprocessingPointsFormat denotest that the postprocesing points have not been correctly provided. + ErrBadPostprocessingPointsFormat = errors.New("invalid postprocessing points format") + // ErrBadPostprocessingPointsFormat denotest that the postprocesing points have not been correctly provided. + ErrBadPostprocessingPath = errors.New("could not parse path to pcd") ) const ( @@ -43,6 +52,13 @@ const ( defaultCartoFacadeTimeout = 5 * time.Minute defaultCartoFacadeInternalTimeout = 15 * time.Minute chunkSizeBytes = 1 * 1024 * 1024 + + // JobDoneCommand is the string that needs to be sent to DoCommand to find out if the job has finished. + JobDoneCommand = "job_done" + // SuccessMessage is sent back after a successful DoCommand request. + SuccessMessage = "success" + // PostprocessToggleResponseKey is the key sent back for the toggle postprocess command. + PostprocessToggleResponseKey = "postprocessed" ) var defaultCartoAlgoCfg = cartofacade.CartoAlgoConfig{ @@ -118,7 +134,7 @@ func initSensorProcesses(cancelCtx context.Context, cartoSvc *CartographerServic CartoFacade: cartoSvc.cartofacade, IsOnline: cartoSvc.lidar.DataFrequencyHz() != 0, Lidar: cartoSvc.lidar, - IMU: cartoSvc.movementSensor, + MovementSensor: cartoSvc.movementSensor, Timeout: cartoSvc.cartoFacadeTimeout, InternalTimeout: cartoSvc.cartoFacadeInternalTimeout, Logger: cartoSvc.logger, @@ -132,11 +148,11 @@ func initSensorProcesses(cancelCtx context.Context, cartoSvc *CartographerServic spConfig.StartLidar(cancelCtx) }() - if spConfig.IMU != nil { + if spConfig.MovementSensor != nil { cartoSvc.sensorProcessWorkers.Add(1) go func() { defer cartoSvc.sensorProcessWorkers.Done() - spConfig.StartIMU(cancelCtx) + spConfig.StartMovementSensor(cancelCtx) }() } } else { @@ -187,10 +203,19 @@ func New( return nil, err } + // Get the lidar for the Dim2D cartographer sub algorithm lidarName := svcConfig.Camera["name"] + timedLidar, err := s.NewLidar(ctx, deps, lidarName, optionalConfigParams.LidarDataFrequencyHz, logger) + if err != nil { + return nil, err + } + // Get the movement sensor if one is configured and check if it supports an IMU and/or odometer. movementSensorName := optionalConfigParams.MovementSensorName - if movementSensorName != "" { + var timedMovementSensor s.TimedMovementSensor + if movementSensorName == "" { + logger.Info("no movement sensor configured, proceeding without IMU and without odometer") + } else { if optionalConfigParams.LidarDataFrequencyHz == 0 && optionalConfigParams.MovementSensorDataFrequencyHz != 0 { return nil, errors.New("In offline mode, but movement sensor data frequency is nonzero") } @@ -198,21 +223,11 @@ func New( if optionalConfigParams.LidarDataFrequencyHz != 0 && optionalConfigParams.MovementSensorDataFrequencyHz == 0 { return nil, errors.New("In online mode, but movement sensor data frequency is zero") } - } - // Get the lidar for the Dim2D cartographer sub algorithm - timedLidar, err := s.NewLidar(ctx, deps, lidarName, optionalConfigParams.LidarDataFrequencyHz, logger) - if err != nil { - return nil, err - } - - // Get the movement sensor if one is configured and check if it supports an IMU and/or odometer. - var timedMovementSensor s.TimedMovementSensor - if movementSensorName == "" { - logger.Info("no movement sensor configured, proceeding without IMU and without odometer") - } else if timedMovementSensor, err = s.NewMovementSensor(ctx, deps, movementSensorName, - optionalConfigParams.MovementSensorDataFrequencyHz, logger); err != nil { - return nil, err + if timedMovementSensor, err = s.NewMovementSensor(ctx, deps, movementSensorName, + optionalConfigParams.MovementSensorDataFrequencyHz, logger); err != nil { + return nil, err + } } // Need to be able to shut down the sensor process before the cartoFacade @@ -232,7 +247,6 @@ func New( Named: c.ResourceName().AsNamed(), lidar: timedLidar, movementSensor: timedMovementSensor, - movementSensorName: movementSensorName, subAlgo: subAlgo, configParams: svcConfig.ConfigParams, cancelSensorProcessFunc: cancelSensorProcessFunc, @@ -395,18 +409,11 @@ func initCartoFacade(ctx context.Context, cartoSvc *CartographerService) error { return err } - cartoCfg := cartofacade.CartoConfig{ - Camera: cartoSvc.lidar.Name(), - MovementSensor: cartoSvc.movementSensorName, - ComponentReference: cartoSvc.lidar.Name(), - LidarConfig: cartofacade.TwoD, - EnableMapping: cartoSvc.enableMapping, - ExistingMap: cartoSvc.existingMap, - } - - if cartoSvc.movementSensorName == "" { + var movementSensorName string + if cartoSvc.movementSensor == nil { cartoSvc.logger.Debug("No movement sensor provided, setting use_imu_data to false") } else { + movementSensorName = cartoSvc.movementSensor.Name() movementSensorProperties := cartoSvc.movementSensor.Properties() if movementSensorProperties.IMUSupported { cartoSvc.logger.Warn("IMU configured, setting use_imu_data to true") @@ -414,6 +421,18 @@ func initCartoFacade(ctx context.Context, cartoSvc *CartographerService) error { } else { cartoSvc.logger.Warn("Movement sensor was provided but does not support IMU data, setting use_imu_data to false") } + if movementSensorProperties.OdometerSupported { + cartoSvc.logger.Debug("Odometer is supported") + } + } + + cartoCfg := cartofacade.CartoConfig{ + Camera: cartoSvc.lidar.Name(), + MovementSensor: movementSensorName, + ComponentReference: cartoSvc.lidar.Name(), + LidarConfig: cartofacade.TwoD, + EnableMapping: cartoSvc.enableMapping, + ExistingMap: cartoSvc.existingMap, } cf := cartofacade.New(&cartoLib, cartoCfg, cartoAlgoConfig) @@ -463,13 +482,12 @@ func terminateCartoFacade(ctx context.Context, cartoSvc *CartographerService) er type CartographerService struct { resource.Named resource.AlwaysRebuild - mu sync.Mutex - SlamMode cartofacade.SlamMode - closed bool - lidar s.TimedLidar - movementSensor s.TimedMovementSensor - movementSensorName string - subAlgo SubAlgo + mu sync.Mutex + SlamMode cartofacade.SlamMode + closed bool + lidar s.TimedLidar + movementSensor s.TimedMovementSensor + subAlgo SubAlgo configParams map[string]string @@ -486,6 +504,10 @@ type CartographerService struct { mapTimestamp time.Time jobDone atomic.Bool + postprocessed atomic.Bool + postprocessingTasks []postprocess.Task + postprocessedPointCloud *[]byte + useCloudSlam bool enableMapping bool existingMap string @@ -496,9 +518,20 @@ type CartographerService struct { func (cartoSvc *CartographerService) Position(ctx context.Context) (spatialmath.Pose, string, error) { ctx, span := trace.StartSpan(ctx, "viamcartographer::CartographerService::Position") defer span.End() +<<<<<<< HEAD if err := cartoSvc.isOpenAndRunningLocally("Position"); err != nil { return nil, "", err +======= + if cartoSvc.useCloudSlam { + cartoSvc.logger.Warn("Position called with use_cloud_slam set to true") + return nil, "", ErrUseCloudSlamEnabled + } + + if cartoSvc.closed { + cartoSvc.logger.Warn("Position called after shutting down of cartographer has been initiated") + return nil, "", ErrClosed +>>>>>>> 60b6e8a9e22011d13daba9cdce9512eef67aae19 } pos, err := cartoSvc.cartofacade.Position(ctx, cartoSvc.cartoFacadeTimeout) @@ -524,14 +557,40 @@ func (cartoSvc *CartographerService) PointCloudMap(ctx context.Context) (func() ctx, span := trace.StartSpan(ctx, "viamcartographer::CartographerService::PointCloudMap") defer span.End() +<<<<<<< HEAD if err := cartoSvc.isOpenAndRunningLocally("PointCloudMap"); err != nil { return nil, err +======= + if cartoSvc.closed { + cartoSvc.logger.Warn("PointCloudMap called after shutting down of cartographer has been initiated") + return nil, ErrClosed +>>>>>>> 60b6e8a9e22011d13daba9cdce9512eef67aae19 + } + + /* + cartoSvc.existingMap != "" && !cartoSvc.enableMapping to check if we are in localization mode. + cartoSvc.postprocessedPointCloud != nil to check that the pointcloud has been set. + cartoSvc.postprocessed.Load() to check if postprocessed has not been toggled off. + */ + if cartoSvc.existingMap != "" && !cartoSvc.enableMapping && cartoSvc.postprocessedPointCloud != nil && cartoSvc.postprocessed.Load() { + return toChunkedFunc(*cartoSvc.postprocessedPointCloud), nil } pc, err := cartoSvc.cartofacade.PointCloudMap(ctx, cartoSvc.cartoFacadeTimeout) if err != nil { return nil, err } + + if cartoSvc.postprocessed.Load() { + var updatedPc []byte + err = postprocess.UpdatePointCloud(pc, &updatedPc, cartoSvc.postprocessingTasks) + if err != nil { + return nil, err + } + + return toChunkedFunc(updatedPc), nil + } + return toChunkedFunc(pc), nil } @@ -541,8 +600,14 @@ func (cartoSvc *CartographerService) InternalState(ctx context.Context) (func() ctx, span := trace.StartSpan(ctx, "viamcartographer::CartographerService::InternalState") defer span.End() +<<<<<<< HEAD if err := cartoSvc.isOpenAndRunningLocally("InternalState"); err != nil { return nil, err +======= + if cartoSvc.closed { + cartoSvc.logger.Warn("InternalState called after shutting down of cartographer has been initiated") + return nil, ErrClosed +>>>>>>> 60b6e8a9e22011d13daba9cdce9512eef67aae19 } is, err := cartoSvc.cartofacade.InternalState(ctx, cartoSvc.cartoFacadeTimeout) @@ -574,8 +639,14 @@ func (cartoSvc *CartographerService) LatestMapInfo(ctx context.Context) (time.Ti _, span := trace.StartSpan(ctx, "viamcartographer::CartographerService::LatestMapInfo") defer span.End() +<<<<<<< HEAD if err := cartoSvc.isOpenAndRunningLocally("LatestMapInfo"); err != nil { return time.Time{}, err +======= + if cartoSvc.closed { + cartoSvc.logger.Warn("LatestMapInfo called after shutting down of cartographer has been initiated") + return time.Time{}, ErrClosed +>>>>>>> 60b6e8a9e22011d13daba9cdce9512eef67aae19 } if cartoSvc.SlamMode != cartofacade.LocalizingMode { @@ -614,15 +685,78 @@ func (cartoSvc *CartographerService) Properties(ctx context.Context) (slam.Prope // DoCommand receives arbitrary commands. func (cartoSvc *CartographerService) DoCommand(ctx context.Context, req map[string]interface{}) (map[string]interface{}, error) { +<<<<<<< HEAD _, span := trace.StartSpan(ctx, "viamcartographer::CartographerService::DoCommand") defer span.End() if err := cartoSvc.isOpenAndRunningLocally("DoCommand"); err != nil { return nil, err +======= + if cartoSvc.useCloudSlam { + cartoSvc.logger.Warn("DoCommand called with use_cloud_slam set to true") + return nil, ErrUseCloudSlamEnabled + } + + if cartoSvc.closed { + cartoSvc.logger.Warn("DoCommand called after shutting down of cartographer has been initiated") + return nil, ErrClosed +>>>>>>> 60b6e8a9e22011d13daba9cdce9512eef67aae19 + } + + if _, ok := req[JobDoneCommand]; ok { + return map[string]interface{}{JobDoneCommand: cartoSvc.jobDone.Load()}, nil + } + + if _, ok := req[postprocess.ToggleCommand]; ok { + cartoSvc.postprocessed.Store(!cartoSvc.postprocessed.Load()) + return map[string]interface{}{PostprocessToggleResponseKey: cartoSvc.postprocessed.Load()}, nil + } + + if points, ok := req[postprocess.AddCommand]; ok { + task, err := postprocess.ParseDoCommand(points, postprocess.Add) + if err != nil { + return nil, errors.Wrap(ErrBadPostprocessingPointsFormat, err.Error()) + } + + cartoSvc.postprocessingTasks = append(cartoSvc.postprocessingTasks, task) + cartoSvc.postprocessed.Store(true) + return map[string]interface{}{postprocess.AddCommand: SuccessMessage}, nil + } + + if points, ok := req[postprocess.RemoveCommand]; ok { + task, err := postprocess.ParseDoCommand(points, postprocess.Remove) + if err != nil { + return nil, errors.Wrap(ErrBadPostprocessingPointsFormat, err.Error()) + } + + cartoSvc.postprocessingTasks = append(cartoSvc.postprocessingTasks, task) + cartoSvc.postprocessed.Store(true) + return map[string]interface{}{postprocess.RemoveCommand: SuccessMessage}, nil } - if _, ok := req["job_done"]; ok { - return map[string]interface{}{"job_done": cartoSvc.jobDone.Load()}, nil + if _, ok := req[postprocess.UndoCommand]; ok { + if len(cartoSvc.postprocessingTasks) == 0 { + return nil, ErrNoPostprocessingToUndo + } + + cartoSvc.postprocessingTasks = cartoSvc.postprocessingTasks[:len(cartoSvc.postprocessingTasks)-1] + return map[string]interface{}{postprocess.UndoCommand: SuccessMessage}, nil + } + + if val, ok := req[postprocess.PathCommand]; ok { + path, ok := val.(string) + if !ok { + return nil, ErrBadPostprocessingPath + } + + path = filepath.Clean(path) + bytes, err := os.ReadFile(path) + if err != nil { + return nil, err + } + cartoSvc.postprocessedPointCloud = &bytes + cartoSvc.postprocessed.Store(true) + return map[string]interface{}{postprocess.PathCommand: SuccessMessage}, nil } return nil, viamgrpc.UnimplementedError @@ -631,13 +765,13 @@ 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 { cartoSvc.mu.Lock() + defer cartoSvc.mu.Unlock() if cartoSvc.useCloudSlam { return nil } cartoSvc.logger.Info("Closing cartographer module") - defer cartoSvc.mu.Unlock() if cartoSvc.closed { cartoSvc.logger.Warn("Close() called multiple times") return nil diff --git a/viam_cartographer_test.go b/viam_cartographer_test.go index c7d77f244..37bb8ff0d 100644 --- a/viam_cartographer_test.go +++ b/viam_cartographer_test.go @@ -16,10 +16,12 @@ import ( "go.viam.com/rdk/logging" "go.viam.com/rdk/services/slam" "go.viam.com/test" + "go.viam.com/utils/artifact" viamcartographer "github.com/viamrobotics/viam-cartographer" "github.com/viamrobotics/viam-cartographer/cartofacade" vcConfig "github.com/viamrobotics/viam-cartographer/config" + "github.com/viamrobotics/viam-cartographer/postprocess" s "github.com/viamrobotics/viam-cartographer/sensors" "github.com/viamrobotics/viam-cartographer/testhelper" ) @@ -387,5 +389,133 @@ func TestDoCommand(t *testing.T) { test.That(t, err, test.ShouldEqual, viamgrpc.UnimplementedError) test.That(t, resp, test.ShouldBeNil) }) + t.Run("returns false when given 'job_done'", func(t *testing.T) { + cmd := map[string]interface{}{viamcartographer.JobDoneCommand: ""} + resp, err := svc.DoCommand(context.Background(), cmd) + test.That(t, err, test.ShouldBeNil) + test.That( + t, + resp, test.ShouldResemble, + map[string]interface{}{viamcartographer.JobDoneCommand: false}, + ) + }) + t.Run("changes postprocess bool after 'postprocess_toggle'", func(t *testing.T) { + cmd := map[string]interface{}{postprocess.ToggleCommand: ""} + resp, err := svc.DoCommand(context.Background(), cmd) + test.That(t, err, test.ShouldBeNil) + test.That( + t, + resp, test.ShouldResemble, + map[string]interface{}{viamcartographer.PostprocessToggleResponseKey: true}, + ) + + cmd = map[string]interface{}{postprocess.ToggleCommand: ""} + resp, err = svc.DoCommand(context.Background(), cmd) + test.That(t, err, test.ShouldBeNil) + test.That( + t, + resp, test.ShouldResemble, + map[string]interface{}{viamcartographer.PostprocessToggleResponseKey: false}, + ) + }) + t.Run( + "errors if 'postprocess_undo' is called before any postprocessing has occurred", + func(t *testing.T) { + cmd := map[string]interface{}{postprocess.UndoCommand: ""} + resp, err := svc.DoCommand(context.Background(), cmd) + test.That(t, err, test.ShouldBeError, viamcartographer.ErrNoPostprocessingToUndo) + test.That(t, resp, test.ShouldBeNil) + }) + t.Run( + "succeeds if 'postprocess_undo' is called after any postprocessing has occurred", + func(t *testing.T) { + point := map[string]interface{}{"X": float64(1), "Y": float64(1)} + cmd := map[string]interface{}{postprocess.AddCommand: []interface{}{point}} + resp, err := svc.DoCommand(context.Background(), cmd) + test.That(t, err, test.ShouldBeNil) + test.That( + t, + resp, + test.ShouldResemble, + map[string]interface{}{postprocess.AddCommand: viamcartographer.SuccessMessage}, + ) + + cmd = map[string]interface{}{postprocess.UndoCommand: ""} + resp, err = svc.DoCommand(context.Background(), cmd) + test.That(t, err, test.ShouldBeNil) + test.That( + t, + resp, + test.ShouldResemble, + map[string]interface{}{postprocess.UndoCommand: viamcartographer.SuccessMessage}, + ) + }) + t.Run( + "success if 'postprocess_add' is called correctly", + func(t *testing.T) { + point := map[string]interface{}{"X": float64(1), "Y": float64(1)} + cmd := map[string]interface{}{postprocess.AddCommand: []interface{}{point}} + resp, err := svc.DoCommand(context.Background(), cmd) + test.That(t, err, test.ShouldBeNil) + test.That( + t, + resp, + test.ShouldResemble, + map[string]interface{}{postprocess.AddCommand: viamcartographer.SuccessMessage}, + ) + }) + t.Run( + "errors if 'postprocess_add' is called with incorrect format", + func(t *testing.T) { + cmd := map[string]interface{}{postprocess.AddCommand: "hello"} + resp, err := svc.DoCommand(context.Background(), cmd) + test.That(t, err.Error(), test.ShouldContainSubstring, viamcartographer.ErrBadPostprocessingPointsFormat.Error()) + test.That(t, resp, test.ShouldBeNil) + }) + t.Run( + "success if 'postprocess_remove' is called correctly", + func(t *testing.T) { + point := map[string]interface{}{"X": float64(1), "Y": float64(1)} + cmd := map[string]interface{}{postprocess.RemoveCommand: []interface{}{point}} + resp, err := svc.DoCommand(context.Background(), cmd) + test.That(t, err, test.ShouldBeNil) + test.That( + t, + resp, + test.ShouldResemble, + map[string]interface{}{postprocess.RemoveCommand: viamcartographer.SuccessMessage}, + ) + }) + t.Run( + "errors if 'postprocess_remove' is called with incorrect format", + func(t *testing.T) { + cmd := map[string]interface{}{postprocess.RemoveCommand: "hello"} + resp, err := svc.DoCommand(context.Background(), cmd) + test.That(t, err.Error(), test.ShouldContainSubstring, viamcartographer.ErrBadPostprocessingPointsFormat.Error()) + test.That(t, resp, test.ShouldBeNil) + }) + t.Run( + "success if 'postprocess_path' is called correctly", + func(t *testing.T) { + path := artifact.MustPath("viam-cartographer/outputs/viam-office-02-22-3/pointcloud/pointcloud_1.pcd") + cmd := map[string]interface{}{postprocess.PathCommand: path} + resp, err := svc.DoCommand(context.Background(), cmd) + test.That(t, err, test.ShouldBeNil) + test.That( + t, + resp, + test.ShouldResemble, + map[string]interface{}{postprocess.PathCommand: viamcartographer.SuccessMessage}, + ) + }) + t.Run( + "errors if 'postprocess_path' is called with incorrect format", + func(t *testing.T) { + point := map[string]interface{}{"X": float64(1), "Y": float64(1)} + cmd := map[string]interface{}{postprocess.PathCommand: point} + resp, err := svc.DoCommand(context.Background(), cmd) + test.That(t, err.Error(), test.ShouldContainSubstring, viamcartographer.ErrBadPostprocessingPath.Error()) + test.That(t, resp, test.ShouldBeNil) + }) test.That(t, svc.Close(context.Background()), test.ShouldBeNil) }