From b39c81c57259936e94cc74b058077519336937ae Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Fri, 7 Jul 2023 16:25:33 -0400
Subject: [PATCH 01/32] RSDK-3547-use-cgo-api-methods2

---
 cartofacade/capi.go               |  60 +++----
 cartofacade/capi_test.go          |   2 +-
 cartofacade/testhelpers.go        |  30 ++--
 internal/testhelper/testhelper.go |   1 +
 module/main.go                    |   5 +
 viam-cartographer.go              | 269 ++++++++++++++++++++++++++----
 6 files changed, 286 insertions(+), 81 deletions(-)

diff --git a/cartofacade/capi.go b/cartofacade/capi.go
index 82c081c6..ba7984b2 100644
--- a/cartofacade/capi.go
+++ b/cartofacade/capi.go
@@ -77,8 +77,8 @@ type GetPosition struct {
 type LidarConfig int64
 
 const (
-	twoD LidarConfig = iota
-	threeD
+	TwoD LidarConfig = iota
+	ThreeD
 )
 
 // CartoConfig contains config values from app
@@ -92,19 +92,19 @@ type CartoConfig struct {
 
 // CartoAlgoConfig contains config values from app
 type CartoAlgoConfig struct {
-	optimizeOnStart      bool
-	optimizeEveryNNodes  int
-	numRangeData         int
-	missingDataRayLength float32
-	maxRange             float32
-	minRange             float32
-	maxSubmapsToKeep     int
-	freshSubmapsCount    int
-	minCoveredArea       float64
-	minAddedSubmapsCount int
-	occupiedSpaceWeight  float64
-	translationWeight    float64
-	rotationWeight       float64
+	OptimizeOnStart      bool
+	OptimizeEveryNNodes  int
+	NumRangeData         int
+	MissingDataRayLength float32
+	MaxRange             float32
+	MinRange             float32
+	MaxSubmapsToKeep     int
+	FreshSubmapsCount    int
+	MinCoveredArea       float64
+	MinAddedSubmapsCount int
+	OccupiedSpaceWeight  float64
+	TranslationWeight    float64
+	RotationWeight       float64
 }
 
 // NewLib calls viam_carto_lib_init and returns a pointer to a viam carto lib object.
@@ -311,9 +311,9 @@ func goStringToBstring(goStr string) C.bstring {
 
 func toLidarConfig(lidarConfig LidarConfig) (C.viam_carto_LIDAR_CONFIG, error) {
 	switch lidarConfig {
-	case twoD:
+	case TwoD:
 		return C.VIAM_CARTO_TWO_D, nil
-	case threeD:
+	case ThreeD:
 		return C.VIAM_CARTO_THREE_D, nil
 	default:
 		return 0, errors.New("invalid lidar config value")
@@ -349,19 +349,19 @@ func getConfig(cfg CartoConfig) (C.viam_carto_config, error) {
 
 func toAlgoConfig(acfg CartoAlgoConfig) C.viam_carto_algo_config {
 	vcac := C.viam_carto_algo_config{}
-	vcac.optimize_on_start = C.bool(acfg.optimizeOnStart)
-	vcac.optimize_every_n_nodes = C.int(acfg.optimizeEveryNNodes)
-	vcac.num_range_data = C.int(acfg.numRangeData)
-	vcac.missing_data_ray_length = C.float(acfg.missingDataRayLength)
-	vcac.max_range = C.float(acfg.maxRange)
-	vcac.min_range = C.float(acfg.minRange)
-	vcac.max_submaps_to_keep = C.int(acfg.maxSubmapsToKeep)
-	vcac.fresh_submaps_count = C.int(acfg.freshSubmapsCount)
-	vcac.min_covered_area = C.double(acfg.minCoveredArea)
-	vcac.min_added_submaps_count = C.int(acfg.minAddedSubmapsCount)
-	vcac.occupied_space_weight = C.double(acfg.occupiedSpaceWeight)
-	vcac.translation_weight = C.double(acfg.translationWeight)
-	vcac.rotation_weight = C.double(acfg.rotationWeight)
+	vcac.optimize_on_start = C.bool(acfg.OptimizeOnStart)
+	vcac.optimize_every_n_nodes = C.int(acfg.OptimizeEveryNNodes)
+	vcac.num_range_data = C.int(acfg.NumRangeData)
+	vcac.missing_data_ray_length = C.float(acfg.MissingDataRayLength)
+	vcac.max_range = C.float(acfg.MaxRange)
+	vcac.min_range = C.float(acfg.MinRange)
+	vcac.max_submaps_to_keep = C.int(acfg.MaxSubmapsToKeep)
+	vcac.fresh_submaps_count = C.int(acfg.FreshSubmapsCount)
+	vcac.min_covered_area = C.double(acfg.MinCoveredArea)
+	vcac.min_added_submaps_count = C.int(acfg.MinAddedSubmapsCount)
+	vcac.occupied_space_weight = C.double(acfg.OccupiedSpaceWeight)
+	vcac.translation_weight = C.double(acfg.TranslationWeight)
+	vcac.rotation_weight = C.double(acfg.RotationWeight)
 	return vcac
 }
 
diff --git a/cartofacade/capi_test.go b/cartofacade/capi_test.go
index b0a5dad5..4ad9a6b6 100644
--- a/cartofacade/capi_test.go
+++ b/cartofacade/capi_test.go
@@ -72,7 +72,7 @@ func TestGetConfig(t *testing.T) {
 
 		freeBstringArray(vcc.sensors, vcc.sensors_len)
 
-		test.That(t, vcc.lidar_config, test.ShouldEqual, twoD)
+		test.That(t, vcc.lidar_config, test.ShouldEqual, TwoD)
 	})
 }
 
diff --git a/cartofacade/testhelpers.go b/cartofacade/testhelpers.go
index 9ded50cc..366d906c 100644
--- a/cartofacade/testhelpers.go
+++ b/cartofacade/testhelpers.go
@@ -16,7 +16,7 @@ func GetTestConfig(sensor string) (CartoConfig, string, error) {
 		MapRateSecond:      5,
 		DataDir:            dir,
 		ComponentReference: "component",
-		LidarConfig:        twoD,
+		LidarConfig:        TwoD,
 	}, dir, nil
 }
 
@@ -24,25 +24,25 @@ func GetTestConfig(sensor string) (CartoConfig, string, error) {
 func GetBadTestConfig() CartoConfig {
 	return CartoConfig{
 		Sensors:     []string{"rplidar", "imu"},
-		LidarConfig: twoD,
+		LidarConfig: TwoD,
 	}
 }
 
 // GetTestAlgoConfig gets a sample algo config for testing purposes.
 func GetTestAlgoConfig() CartoAlgoConfig {
 	return CartoAlgoConfig{
-		optimizeOnStart:      false,
-		optimizeEveryNNodes:  3,
-		numRangeData:         100,
-		missingDataRayLength: 25.0,
-		maxRange:             25.0,
-		minRange:             0.2,
-		maxSubmapsToKeep:     3,
-		freshSubmapsCount:    3,
-		minCoveredArea:       1.0,
-		minAddedSubmapsCount: 1,
-		occupiedSpaceWeight:  20.0,
-		translationWeight:    10.0,
-		rotationWeight:       1.0,
+		OptimizeOnStart:      false,
+		OptimizeEveryNNodes:  3,
+		NumRangeData:         100,
+		MissingDataRayLength: 25.0,
+		MaxRange:             25.0,
+		MinRange:             0.2,
+		MaxSubmapsToKeep:     3,
+		FreshSubmapsCount:    3,
+		MinCoveredArea:       1.0,
+		MinAddedSubmapsCount: 1,
+		OccupiedSpaceWeight:  20.0,
+		TranslationWeight:    10.0,
+		RotationWeight:       1.0,
 	}
 }
diff --git a/internal/testhelper/testhelper.go b/internal/testhelper/testhelper.go
index cc524bd8..ede17f3b 100644
--- a/internal/testhelper/testhelper.go
+++ b/internal/testhelper/testhelper.go
@@ -218,6 +218,7 @@ func CreateSLAMService(
 		SensorValidationMaxTimeoutSecForTest,
 		SensorValidationIntervalSecForTest,
 		testDialMaxTimeoutSec,
+		viamcartographer.DefaultCartoFacadeTimeout,
 	)
 	if err != nil {
 		test.That(t, svc, test.ShouldBeNil)
diff --git a/module/main.go b/module/main.go
index 0f3e3d20..ec066cad 100644
--- a/module/main.go
+++ b/module/main.go
@@ -41,6 +41,11 @@ func mainWithArgs(ctx context.Context, args []string, logger golog.Logger) error
 		return nil
 	}
 
+	if err := viamcartographer.InitCartoLib(logger); err != nil {
+		return err
+	}
+	defer utils.UncheckedErrorFunc(viamcartographer.TerminateCartoLib)
+
 	// Instantiate the module
 	cartoModule, err := module.NewModuleFromArgs(ctx, logger)
 	if err != nil {
diff --git a/viam-cartographer.go b/viam-cartographer.go
index 04a47026..f082365f 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -15,6 +15,7 @@ import (
 	"github.com/edaniels/golog"
 	"github.com/pkg/errors"
 	"go.opencensus.io/trace"
+	"go.uber.org/zap/zapcore"
 	pb "go.viam.com/api/service/slam/v1"
 	viamgrpc "go.viam.com/rdk/grpc"
 	"go.viam.com/rdk/resource"
@@ -24,14 +25,17 @@ import (
 	goutils "go.viam.com/utils"
 	"go.viam.com/utils/pexec"
 
+	"github.com/viamrobotics/viam-cartographer/cartofacade"
 	vcConfig "github.com/viamrobotics/viam-cartographer/config"
 	dim2d "github.com/viamrobotics/viam-cartographer/internal/dim-2d"
+	"github.com/viamrobotics/viam-cartographer/sensorprocess"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
 	vcUtils "github.com/viamrobotics/viam-cartographer/utils"
 )
 
 // Model is the model name of cartographer.
 var Model = resource.NewModel("viam", "slam", "cartographer")
+var cartoLib cartofacade.CartoLib
 
 const (
 	// DefaultExecutableName is what this program expects to call to start the cartographer grpc server.
@@ -43,8 +47,25 @@ const (
 	defaultSensorValidationIntervalSec   = 1
 	parsePortMaxTimeoutSec               = 60
 	localhost0                           = "localhost:0"
+	DefaultCartoFacadeTimeout            = 5 * time.Second
 )
 
+var defaultCartoAlgoCfg = cartofacade.CartoAlgoConfig{
+	OptimizeOnStart:      false,
+	OptimizeEveryNNodes:  3,
+	NumRangeData:         30,
+	MissingDataRayLength: 25.0,
+	MaxRange:             25.0,
+	MinRange:             0.2,
+	MaxSubmapsToKeep:     3,
+	FreshSubmapsCount:    3,
+	MinCoveredArea:       1.0,
+	MinAddedSubmapsCount: 1,
+	OccupiedSpaceWeight:  20.0,
+	TranslationWeight:    10.0,
+	RotationWeight:       1.0,
+}
+
 // SubAlgo defines the cartographer specific sub-algorithms that we support.
 type SubAlgo string
 
@@ -69,11 +90,47 @@ func init() {
 				defaultSensorValidationMaxTimeoutSec,
 				defaultSensorValidationIntervalSec,
 				defaultDialMaxTimeoutSec,
+				DefaultCartoFacadeTimeout,
 			)
 		},
 	})
 }
 
+// InitCartoLib is run to initialize the cartographer library
+// must be called before module.AddModelFromRegistry is
+// called
+func InitCartoLib(logger golog.Logger) error {
+	minloglevel := 1 // warn
+	vlog := 0        //  disabled
+	if logger.Level() == zapcore.DebugLevel {
+		minloglevel = 0 // info
+		vlog = 1        // verbose enabled
+	}
+	lib, err := cartofacade.NewLib(minloglevel, vlog)
+	if err != nil {
+		return err
+	}
+	cartoLib = lib
+	return nil
+}
+
+func TerminateCartoLib() error {
+	return cartoLib.Terminate()
+}
+
+func initSensorProcess(cancelCtx context.Context, cartoSvc *cartographerService) {
+	spConfig := sensorprocess.Config{
+		CartoFacade:      &cartoSvc.cartofacade,
+		Lidar:            cartoSvc.lidar,
+		LidarName:        cartoSvc.primarySensorName,
+		DataRateMs:       cartoSvc.dataRateMs,
+		Timeout:          cartoSvc.cartoFacadeTimeout,
+		Logger:           cartoSvc.logger,
+		TelemetryEnabled: cartoSvc.logger.Level() == zapcore.DebugLevel,
+	}
+	sensorprocess.Start(cancelCtx, spConfig)
+}
+
 // New returns a new slam service for the given robot.
 func New(
 	ctx context.Context,
@@ -85,6 +142,7 @@ func New(
 	sensorValidationMaxTimeoutSec int,
 	sensorValidationIntervalSec int,
 	dialMaxTimeoutSec int,
+	cartoFacadeTimeout time.Duration,
 ) (slam.Service, error) {
 	ctx, span := trace.StartSpan(ctx, "viamcartographer::slamService::New")
 	defer span.End()
@@ -100,12 +158,7 @@ func New(
 			c.Model.Name, svcConfig.ConfigParams["mode"])
 	}
 
-	// Set up the data directories
-	if err := vcConfig.SetupDirectories(svcConfig.DataDirectory, logger); err != nil {
-		return nil, err
-	}
-
-	port, dataRateMsec, mapRateSec, useLiveData, deleteProcessedData, _, err := vcConfig.GetOptionalParameters(
+	port, dataRateMsec, mapRateSec, useLiveData, deleteProcessedData, modularizationV2Enabled, err := vcConfig.GetOptionalParameters(
 		svcConfig,
 		localhost0,
 		defaultDataRateMsec,
@@ -127,55 +180,192 @@ func New(
 
 	// Cartographer SLAM Service Object
 	cartoSvc := &cartographerService{
-		Named:                 c.ResourceName().AsNamed(),
-		primarySensorName:     lidar.Name,
-		executableName:        executableName,
-		subAlgo:               subAlgo,
-		slamProcess:           pexec.NewProcessManager(logger),
-		configParams:          svcConfig.ConfigParams,
-		dataDirectory:         svcConfig.DataDirectory,
-		useLiveData:           useLiveData,
-		deleteProcessedData:   deleteProcessedData,
-		port:                  port,
-		dataRateMs:            dataRateMsec,
-		mapRateSec:            mapRateSec,
-		cancelFunc:            cancelFunc,
-		logger:                logger,
-		bufferSLAMProcessLogs: bufferSLAMProcessLogs,
-		localizationMode:      mapRateSec == 0,
-		mapTimestamp:          time.Now().UTC(),
-	}
-
-	success := false
+		Named:                         c.ResourceName().AsNamed(),
+		primarySensorName:             lidar.Name,
+		lidar:                         lidar,
+		executableName:                executableName,
+		subAlgo:                       subAlgo,
+		slamProcess:                   pexec.NewProcessManager(logger),
+		configParams:                  svcConfig.ConfigParams,
+		dataDirectory:                 svcConfig.DataDirectory,
+		sensors:                       svcConfig.Sensors,
+		useLiveData:                   useLiveData,
+		deleteProcessedData:           deleteProcessedData,
+		port:                          port,
+		dataRateMs:                    dataRateMsec,
+		mapRateSec:                    mapRateSec,
+		cancelFunc:                    cancelFunc,
+		logger:                        logger,
+		bufferSLAMProcessLogs:         bufferSLAMProcessLogs,
+		modularizationV2Enabled:       modularizationV2Enabled,
+		sensorValidationMaxTimeoutSec: sensorValidationMaxTimeoutSec,
+		sensorValidationIntervalSec:   sensorValidationMaxTimeoutSec,
+		dialMaxTimeoutSec:             dialMaxTimeoutSec,
+		cartoFacadeTimeout:            cartoFacadeTimeout,
+		localizationMode:              mapRateSec == 0,
+		mapTimestamp:                  time.Now().UTC(),
+	}
+
 	defer func() {
-		if !success {
+		if err != nil {
+			logger.Errorw("New() hit error, closing...", "error", err)
 			if err := cartoSvc.Close(ctx); err != nil {
 				logger.Errorw("error closing out after error", "error", err)
 			}
 		}
 	}()
 
+	if modularizationV2Enabled {
+		cartoSvc, err = initCartoFacade(cancelCtx, cartoSvc)
+		if err != nil {
+			return cartoSvc, err
+		}
+		initSensorProcess(cancelCtx, cartoSvc)
+		return cartoSvc, err
+	}
+	cartoSvc, err = initCartoGrpcServer(ctx, cancelCtx, cartoSvc)
+	return cartoSvc, err
+}
+
+// TODO: write a test for this
+func parseCartoAlgoConfig(configParams map[string]string, logger golog.Logger) (cartofacade.CartoAlgoConfig, error) {
+	cartoAlgoCfg := defaultCartoAlgoCfg
+	logger.Warnf("NICK: defaultCartoAlgoCfgPtr: %p, cartoAlgoCfgPtr: %p", &defaultCartoAlgoCfg, &cartoAlgoCfg)
+	for k, val := range configParams {
+		switch k {
+		case "optimize_on_start":
+			if val == "true" {
+				cartoAlgoCfg.OptimizeOnStart = true
+			}
+		case "optimize_every_n_nodes":
+			iVal, err := strconv.Atoi(val)
+			if err != nil {
+				return cartoAlgoCfg, err
+			}
+			cartoAlgoCfg.OptimizeEveryNNodes = iVal
+		case "num_range_data":
+			iVal, err := strconv.Atoi(val)
+			if err != nil {
+				return cartoAlgoCfg, err
+			}
+			cartoAlgoCfg.NumRangeData = iVal
+		case "missing_data_ray_length":
+			fVal, err := strconv.ParseFloat(val, 32)
+			if err != nil {
+				return cartoAlgoCfg, err
+			}
+			cartoAlgoCfg.MissingDataRayLength = float32(fVal)
+		case "max_range":
+			fVal, err := strconv.ParseFloat(val, 32)
+			if err != nil {
+				return cartoAlgoCfg, err
+			}
+			cartoAlgoCfg.MaxRange = float32(fVal)
+		case "min_range":
+			fVal, err := strconv.ParseFloat(val, 32)
+			if err != nil {
+				return cartoAlgoCfg, err
+			}
+			cartoAlgoCfg.MinRange = float32(fVal)
+		case "max_submaps_to_keep":
+			iVal, err := strconv.Atoi(val)
+			if err != nil {
+				return cartoAlgoCfg, err
+			}
+			cartoAlgoCfg.MaxSubmapsToKeep = iVal
+		case "fresh_submaps_count":
+			iVal, err := strconv.Atoi(val)
+			if err != nil {
+				return cartoAlgoCfg, err
+			}
+			cartoAlgoCfg.FreshSubmapsCount = iVal
+		case "min_covered_area":
+			fVal, err := strconv.ParseFloat(val, 64)
+			if err != nil {
+				return cartoAlgoCfg, err
+			}
+			cartoAlgoCfg.MinCoveredArea = fVal
+		case "min_added_submaps_count":
+			iVal, err := strconv.Atoi(val)
+			if err != nil {
+				return cartoAlgoCfg, err
+			}
+			cartoAlgoCfg.MinAddedSubmapsCount = iVal
+		case "occupied_space_weight":
+			fVal, err := strconv.ParseFloat(val, 64)
+			if err != nil {
+				return cartoAlgoCfg, err
+			}
+			cartoAlgoCfg.OccupiedSpaceWeight = fVal
+		case "translation_weight":
+			fVal, err := strconv.ParseFloat(val, 64)
+			if err != nil {
+				return cartoAlgoCfg, err
+			}
+			cartoAlgoCfg.TranslationWeight = fVal
+		case "rotation_weight":
+			fVal, err := strconv.ParseFloat(val, 64)
+			if err != nil {
+				return cartoAlgoCfg, err
+			}
+			cartoAlgoCfg.RotationWeight = fVal
+		default:
+			logger.Warn("unused config param: %s: %s", k, val)
+		}
+	}
+	return cartoAlgoCfg, nil
+}
+
+func initCartoFacade(cancelCtx context.Context, cartoSvc *cartographerService) (*cartographerService, error) {
+	cartoAlgoConfig, err := parseCartoAlgoConfig(cartoSvc.configParams, cartoSvc.logger)
+	if err != nil {
+		return nil, err
+	}
+
+	cartoCfg := cartofacade.CartoConfig{
+		Sensors:            cartoSvc.sensors,
+		MapRateSecond:      cartoSvc.mapRateSec,
+		DataDir:            cartoSvc.dataDirectory,
+		ComponentReference: cartoSvc.primarySensorName,
+		LidarConfig:        cartofacade.TwoD,
+	}
+
+	cf := cartofacade.New(&cartoLib, cartoCfg, cartoAlgoConfig)
+	err = cf.Initialize(cancelCtx, cartoSvc.cartoFacadeTimeout, &cartoSvc.activeBackgroundWorkers)
+	if err != nil {
+		return nil, err
+	}
+	cartoSvc.cartofacade = cf
+
+	return cartoSvc, nil
+}
+
+func initCartoGrpcServer(ctx context.Context, cancelCtx context.Context, cartoSvc *cartographerService) (*cartographerService, error) {
+	// Set up the data directories
+	if err := vcConfig.SetupDirectories(cartoSvc.dataDirectory, cartoSvc.logger); err != nil {
+		return nil, err
+	}
+
 	if cartoSvc.primarySensorName != "" {
-		if err := dim2d.ValidateGetAndSaveData(cancelCtx, cartoSvc.dataDirectory, lidar,
-			sensorValidationMaxTimeoutSec, sensorValidationIntervalSec, cartoSvc.logger); err != nil {
+		if err := dim2d.ValidateGetAndSaveData(cancelCtx, cartoSvc.dataDirectory, cartoSvc.lidar,
+			cartoSvc.sensorValidationMaxTimeoutSec, cartoSvc.sensorValidationIntervalSec, cartoSvc.logger); err != nil {
 			return nil, errors.Wrap(err, "getting and saving data failed")
 		}
-		cartoSvc.StartDataProcess(cancelCtx, lidar, nil)
-		logger.Debugf("Reading data from sensor: %v", cartoSvc.primarySensorName)
+		cartoSvc.StartDataProcess(cancelCtx, cartoSvc.lidar, nil)
+		cartoSvc.logger.Debugf("Reading data from sensor: %v", cartoSvc.primarySensorName)
 	}
 
 	if err := cartoSvc.StartSLAMProcess(ctx); err != nil {
 		return nil, errors.Wrap(err, "error with slam service slam process")
 	}
 
-	client, clientClose, err := vcConfig.SetupGRPCConnection(ctx, cartoSvc.port, dialMaxTimeoutSec, logger)
+	client, clientClose, err := vcConfig.SetupGRPCConnection(ctx, cartoSvc.port, cartoSvc.dialMaxTimeoutSec, cartoSvc.logger)
 	if err != nil {
 		return nil, errors.Wrap(err, "error with initial grpc client to slam algorithm")
 	}
 	cartoSvc.clientAlgo = client
 	cartoSvc.clientAlgoClose = clientClose
 
-	success = true
 	return cartoSvc, nil
 }
 
@@ -184,6 +374,7 @@ type cartographerService struct {
 	resource.Named
 	resource.AlwaysRebuild
 	primarySensorName string
+	lidar             lidar.Lidar
 	executableName    string
 	subAlgo           SubAlgo
 	slamProcess       pexec.ProcessManager
@@ -192,9 +383,14 @@ type cartographerService struct {
 
 	configParams        map[string]string
 	dataDirectory       string
+	sensors             []string
 	deleteProcessedData bool
 	useLiveData         bool
 
+	modularizationV2Enabled bool
+	cartofacade             cartofacade.CartoFacade
+	cartoFacadeTimeout      time.Duration
+
 	port       string
 	dataRateMs int
 	mapRateSec int
@@ -208,8 +404,11 @@ type cartographerService struct {
 	slamProcessLogWriter         io.WriteCloser
 	slamProcessBufferedLogReader bufio.Reader
 
-	localizationMode bool
-	mapTimestamp     time.Time
+	localizationMode              bool
+	mapTimestamp                  time.Time
+	sensorValidationMaxTimeoutSec int
+	sensorValidationIntervalSec   int
+	dialMaxTimeoutSec             int
 }
 
 // GetPosition forwards the request for positional data to the slam library's gRPC service. Once a response is received,

From 717af1869eb62026ba3fb2ff76af60416f3251a1 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Fri, 7 Jul 2023 16:33:26 -0400
Subject: [PATCH 02/32] wip

---
 .../sensorprocess.go => sensorprocess.go      | 19 +++++++++----------
 ...orprocess_test.go => sensorprocess_test.go |  8 ++++----
 viam-cartographer.go                          |  5 ++---
 3 files changed, 15 insertions(+), 17 deletions(-)
 rename sensorprocess/sensorprocess.go => sensorprocess.go (88%)
 rename sensorprocess/sensorprocess_test.go => sensorprocess_test.go (99%)

diff --git a/sensorprocess/sensorprocess.go b/sensorprocess.go
similarity index 88%
rename from sensorprocess/sensorprocess.go
rename to sensorprocess.go
index 7bae2d09..0b2db990 100644
--- a/sensorprocess/sensorprocess.go
+++ b/sensorprocess.go
@@ -1,5 +1,4 @@
-// Package sensorprocess contains the logic to add lidar or replay sensor readings to cartographer's mapbuilder
-package sensorprocess
+package viamcartographer 
 
 import (
 	"bytes"
@@ -16,8 +15,8 @@ import (
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
 )
 
-// Config holds config needed throughout the process of adding a sensor reading to the mapbuilder.
-type Config struct {
+// SensorProcessConfig holds config needed throughout the process of adding a sensor reading to the mapbuilder.
+type SensorProcessConfig struct {
 	CartoFacade      cartofacade.Interface
 	Lidar            lidar.Lidar
 	LidarName        string
@@ -27,11 +26,11 @@ type Config struct {
 	TelemetryEnabled bool
 }
 
-// Start polls the lidar to get the next sensor reading and adds it to the mapBuilder.
+// StartSensorProcess polls the lidar to get the next sensor reading and adds it to the mapBuilder.
 // stops when the context is Done.
-func Start(
+func StartSensorProcess(
 	ctx context.Context,
-	config Config,
+	config SensorProcessConfig,
 ) {
 	for {
 		select {
@@ -46,7 +45,7 @@ func Start(
 // addSensorReading adds a lidar reading to the mapbuilder.
 func addSensorReading(
 	parentCtx context.Context,
-	config Config,
+	config SensorProcessConfig,
 ) {
 	ctxWithMetadata, md := contextutils.ContextWithMetadata(parentCtx)
 	readingPc, err := config.Lidar.GetData(ctxWithMetadata)
@@ -79,7 +78,7 @@ func addSensorReading(
 
 // addSensorReadingFromReplaySensor adds a reading from a replay sensor to the cartofacade
 // retries on error.
-func addSensorReadingFromReplaySensor(ctx context.Context, reading []byte, readingTime time.Time, config Config) {
+func addSensorReadingFromReplaySensor(ctx context.Context, reading []byte, readingTime time.Time, config SensorProcessConfig) {
 	/*
 		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
@@ -105,7 +104,7 @@ func addSensorReadingFromReplaySensor(ctx context.Context, reading []byte, readi
 
 // addSensorReadingFromLiveReadings adds a reading from a live lidar to the carto facade
 // does not retry.
-func addSensorReadingFromLiveReadings(ctx context.Context, reading []byte, readingTime time.Time, config Config) int {
+func addSensorReadingFromLiveReadings(ctx context.Context, reading []byte, readingTime time.Time, config SensorProcessConfig) int {
 	startTime := time.Now()
 	err := config.CartoFacade.AddSensorReading(ctx, config.Timeout, config.LidarName, reading, readingTime)
 	if err != nil {
diff --git a/sensorprocess/sensorprocess_test.go b/sensorprocess_test.go
similarity index 99%
rename from sensorprocess/sensorprocess_test.go
rename to sensorprocess_test.go
index 867c6b8e..5922506d 100644
--- a/sensorprocess/sensorprocess_test.go
+++ b/sensorprocess_test.go
@@ -1,4 +1,4 @@
-package sensorprocess
+package viamcartographer
 
 import (
 	"context"
@@ -41,7 +41,7 @@ func TestAddSensorReadingReplaySensor(t *testing.T) {
 	reading := []byte("12345")
 	readingTimestamp := time.Now().UTC()
 	cf := cartofacade.Mock{}
-	config := Config{
+	config := SensorProcessConfig{
 		Logger:      logger,
 		CartoFacade: &cf,
 		LidarName:   "good_lidar",
@@ -138,7 +138,7 @@ func TestAddSensorReadingLiveReadings(t *testing.T) {
 	cf := cartofacade.Mock{}
 	reading := []byte("12345")
 	readingTimestamp := time.Now().UTC()
-	config := Config{
+	config := SensorProcessConfig{
 		Logger:      logger,
 		CartoFacade: &cf,
 		LidarName:   "good_lidar",
@@ -247,7 +247,7 @@ func TestAddSensorReading(t *testing.T) {
 	logger := golog.NewTestLogger(t)
 	cf := cartofacade.Mock{}
 
-	config := Config{
+	config := SensorProcessConfig{
 		Logger:      logger,
 		CartoFacade: &cf,
 		DataRateMs:  200,
diff --git a/viam-cartographer.go b/viam-cartographer.go
index f082365f..2164b714 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -28,7 +28,6 @@ import (
 	"github.com/viamrobotics/viam-cartographer/cartofacade"
 	vcConfig "github.com/viamrobotics/viam-cartographer/config"
 	dim2d "github.com/viamrobotics/viam-cartographer/internal/dim-2d"
-	"github.com/viamrobotics/viam-cartographer/sensorprocess"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
 	vcUtils "github.com/viamrobotics/viam-cartographer/utils"
 )
@@ -119,7 +118,7 @@ func TerminateCartoLib() error {
 }
 
 func initSensorProcess(cancelCtx context.Context, cartoSvc *cartographerService) {
-	spConfig := sensorprocess.Config{
+	spConfig := SensorProcessConfig{
 		CartoFacade:      &cartoSvc.cartofacade,
 		Lidar:            cartoSvc.lidar,
 		LidarName:        cartoSvc.primarySensorName,
@@ -128,7 +127,7 @@ func initSensorProcess(cancelCtx context.Context, cartoSvc *cartographerService)
 		Logger:           cartoSvc.logger,
 		TelemetryEnabled: cartoSvc.logger.Level() == zapcore.DebugLevel,
 	}
-	sensorprocess.Start(cancelCtx, spConfig)
+	StartSensorProcess(cancelCtx, spConfig)
 }
 
 // New returns a new slam service for the given robot.

From 6de6b803d8f2a9d70103c41af14d79c67768a9e0 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Fri, 7 Jul 2023 16:54:19 -0400
Subject: [PATCH 03/32] wip

---
 .../sensorprocess.go                          |  2 +-
 .../sensorprocess_test.go                     |  5 +-
 testhelper/testhelper.go                      | 92 +++++++++++++++++++
 viam-cartographer.go                          | 12 ++-
 4 files changed, 104 insertions(+), 7 deletions(-)
 rename sensorprocess.go => sensorprocess/sensorprocess.go (99%)
 rename sensorprocess_test.go => sensorprocess/sensorprocess_test.go (99%)
 create mode 100644 testhelper/testhelper.go

diff --git a/sensorprocess.go b/sensorprocess/sensorprocess.go
similarity index 99%
rename from sensorprocess.go
rename to sensorprocess/sensorprocess.go
index 0b2db990..4bf4e030 100644
--- a/sensorprocess.go
+++ b/sensorprocess/sensorprocess.go
@@ -1,4 +1,4 @@
-package viamcartographer 
+package sensorprocess
 
 import (
 	"bytes"
diff --git a/sensorprocess_test.go b/sensorprocess/sensorprocess_test.go
similarity index 99%
rename from sensorprocess_test.go
rename to sensorprocess/sensorprocess_test.go
index 5922506d..28c74848 100644
--- a/sensorprocess_test.go
+++ b/sensorprocess/sensorprocess_test.go
@@ -1,4 +1,4 @@
-package viamcartographer
+package sensorprocess
 
 import (
 	"context"
@@ -10,7 +10,7 @@ import (
 	"go.viam.com/test"
 
 	"github.com/viamrobotics/viam-cartographer/cartofacade"
-	"github.com/viamrobotics/viam-cartographer/internal/testhelper"
+	"github.com/viamrobotics/viam-cartographer/testhelper"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
 )
 
@@ -409,3 +409,4 @@ func TestAddSensorReading(t *testing.T) {
 		test.That(t, calls[1].readingTimestamp.Before(calls[2].readingTimestamp), test.ShouldBeTrue)
 	})
 }
+
diff --git a/testhelper/testhelper.go b/testhelper/testhelper.go
new file mode 100644
index 00000000..b0318323
--- /dev/null
+++ b/testhelper/testhelper.go
@@ -0,0 +1,92 @@
+package testhelper
+
+import (
+	"context"
+	"errors"
+	"github.com/viamrobotics/gostream"
+	"go.viam.com/rdk/components/camera"
+	"go.viam.com/rdk/pointcloud"
+	"go.viam.com/rdk/resource"
+	"go.viam.com/rdk/rimage/transform"
+	"go.viam.com/rdk/testutils/inject"
+	"go.viam.com/rdk/utils/contextutils"
+)
+
+const (
+	// TestTime can be used to test specific timestamps provided by a replay sensor.
+	TestTime = "2006-01-02T15:04:05.9999Z"
+	// BadTime can be used to represent something that should cause an error while parsing it as a time.
+	BadTime = "NOT A TIME"
+)
+
+func SetupDeps(sensors []string) resource.Dependencies {
+	deps := make(resource.Dependencies)
+
+	for _, sensor := range sensors {
+		switch sensor {
+		case "good_lidar":
+			deps[camera.Named(sensor)] = getGoodLidar()
+		case "replay_sensor":
+			deps[camera.Named(sensor)] = getReplaySensor(TestTime)
+		case "invalid_replay_sensor":
+			deps[camera.Named(sensor)] = getReplaySensor(BadTime)
+		case "invalid_sensor":
+			deps[camera.Named(sensor)] = getInvalidSensor()
+    default:
+      continue
+		}
+	}
+	return deps
+}
+
+func getGoodLidar() *inject.Camera {
+	cam := &inject.Camera{}
+	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
+		return pointcloud.New(), nil
+	}
+	cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) {
+		return nil, errors.New("lidar not camera")
+	}
+	cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) {
+		return nil, transform.NewNoIntrinsicsError("")
+	}
+	cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) {
+		return camera.Properties{}, nil
+	}
+	return cam
+}
+
+func getReplaySensor(testTime string) *inject.Camera {
+	cam := &inject.Camera{}
+	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
+		md := ctx.Value(contextutils.MetadataContextKey)
+		if mdMap, ok := md.(map[string][]string); ok {
+			mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime}
+		}
+		return pointcloud.New(), nil
+	}
+	cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) {
+		return nil, errors.New("lidar not camera")
+	}
+	cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) {
+		return nil, transform.NewNoIntrinsicsError("")
+	}
+	cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) {
+		return camera.Properties{}, nil
+	}
+	return cam
+}
+
+func getInvalidSensor() *inject.Camera {
+	cam := &inject.Camera{}
+	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
+		return nil, errors.New("invalid sensor")
+	}
+	cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) {
+		return nil, errors.New("invalid sensor")
+	}
+	cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) {
+		return nil, transform.NewNoIntrinsicsError("")
+	}
+	return cam
+}
diff --git a/viam-cartographer.go b/viam-cartographer.go
index 2164b714..7a3863de 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -28,6 +28,7 @@ import (
 	"github.com/viamrobotics/viam-cartographer/cartofacade"
 	vcConfig "github.com/viamrobotics/viam-cartographer/config"
 	dim2d "github.com/viamrobotics/viam-cartographer/internal/dim-2d"
+	"github.com/viamrobotics/viam-cartographer/sensorprocess"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
 	vcUtils "github.com/viamrobotics/viam-cartographer/utils"
 )
@@ -118,7 +119,7 @@ func TerminateCartoLib() error {
 }
 
 func initSensorProcess(cancelCtx context.Context, cartoSvc *cartographerService) {
-	spConfig := SensorProcessConfig{
+	spConfig := sensorprocess.SensorProcessConfig{
 		CartoFacade:      &cartoSvc.cartofacade,
 		Lidar:            cartoSvc.lidar,
 		LidarName:        cartoSvc.primarySensorName,
@@ -127,7 +128,7 @@ func initSensorProcess(cancelCtx context.Context, cartoSvc *cartographerService)
 		Logger:           cartoSvc.logger,
 		TelemetryEnabled: cartoSvc.logger.Level() == zapcore.DebugLevel,
 	}
-	StartSensorProcess(cancelCtx, spConfig)
+	sensorprocess.StartSensorProcess(cancelCtx, spConfig)
 }
 
 // New returns a new slam service for the given robot.
@@ -204,9 +205,9 @@ func New(
 		localizationMode:              mapRateSec == 0,
 		mapTimestamp:                  time.Now().UTC(),
 	}
-
+	success := false
 	defer func() {
-		if err != nil {
+		if !success {
 			logger.Errorw("New() hit error, closing...", "error", err)
 			if err := cartoSvc.Close(ctx); err != nil {
 				logger.Errorw("error closing out after error", "error", err)
@@ -223,6 +224,9 @@ func New(
 		return cartoSvc, err
 	}
 	cartoSvc, err = initCartoGrpcServer(ctx, cancelCtx, cartoSvc)
+	if err != nil {
+		success = true
+	}
 	return cartoSvc, err
 }
 

From b7bc8b91d5ef1fec38f7b197365d45140d7d9b10 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Fri, 7 Jul 2023 16:57:32 -0400
Subject: [PATCH 04/32] wip

---
 sensorprocess/sensorprocess.go      | 17 +++++++++--------
 sensorprocess/sensorprocess_test.go | 10 +++++-----
 testhelper/testhelper.go            |  5 +++--
 viam-cartographer.go                | 16 +++++++++-------
 4 files changed, 26 insertions(+), 22 deletions(-)

diff --git a/sensorprocess/sensorprocess.go b/sensorprocess/sensorprocess.go
index 4bf4e030..7bae2d09 100644
--- a/sensorprocess/sensorprocess.go
+++ b/sensorprocess/sensorprocess.go
@@ -1,3 +1,4 @@
+// Package sensorprocess contains the logic to add lidar or replay sensor readings to cartographer's mapbuilder
 package sensorprocess
 
 import (
@@ -15,8 +16,8 @@ import (
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
 )
 
-// SensorProcessConfig holds config needed throughout the process of adding a sensor reading to the mapbuilder.
-type SensorProcessConfig struct {
+// Config holds config needed throughout the process of adding a sensor reading to the mapbuilder.
+type Config struct {
 	CartoFacade      cartofacade.Interface
 	Lidar            lidar.Lidar
 	LidarName        string
@@ -26,11 +27,11 @@ type SensorProcessConfig struct {
 	TelemetryEnabled bool
 }
 
-// StartSensorProcess polls the lidar to get the next sensor reading and adds it to the mapBuilder.
+// Start polls the lidar to get the next sensor reading and adds it to the mapBuilder.
 // stops when the context is Done.
-func StartSensorProcess(
+func Start(
 	ctx context.Context,
-	config SensorProcessConfig,
+	config Config,
 ) {
 	for {
 		select {
@@ -45,7 +46,7 @@ func StartSensorProcess(
 // addSensorReading adds a lidar reading to the mapbuilder.
 func addSensorReading(
 	parentCtx context.Context,
-	config SensorProcessConfig,
+	config Config,
 ) {
 	ctxWithMetadata, md := contextutils.ContextWithMetadata(parentCtx)
 	readingPc, err := config.Lidar.GetData(ctxWithMetadata)
@@ -78,7 +79,7 @@ func addSensorReading(
 
 // addSensorReadingFromReplaySensor adds a reading from a replay sensor to the cartofacade
 // retries on error.
-func addSensorReadingFromReplaySensor(ctx context.Context, reading []byte, readingTime time.Time, config SensorProcessConfig) {
+func addSensorReadingFromReplaySensor(ctx context.Context, reading []byte, readingTime time.Time, config Config) {
 	/*
 		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
@@ -104,7 +105,7 @@ func addSensorReadingFromReplaySensor(ctx context.Context, reading []byte, readi
 
 // addSensorReadingFromLiveReadings adds a reading from a live lidar to the carto facade
 // does not retry.
-func addSensorReadingFromLiveReadings(ctx context.Context, reading []byte, readingTime time.Time, config SensorProcessConfig) int {
+func addSensorReadingFromLiveReadings(ctx context.Context, reading []byte, readingTime time.Time, config Config) int {
 	startTime := time.Now()
 	err := config.CartoFacade.AddSensorReading(ctx, config.Timeout, config.LidarName, reading, readingTime)
 	if err != nil {
diff --git a/sensorprocess/sensorprocess_test.go b/sensorprocess/sensorprocess_test.go
index 28c74848..139ada1d 100644
--- a/sensorprocess/sensorprocess_test.go
+++ b/sensorprocess/sensorprocess_test.go
@@ -10,8 +10,9 @@ import (
 	"go.viam.com/test"
 
 	"github.com/viamrobotics/viam-cartographer/cartofacade"
-	"github.com/viamrobotics/viam-cartographer/testhelper"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
+	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
+	"github.com/viamrobotics/viam-cartographer/testhelper"
 )
 
 type addSensorReadingArgs struct {
@@ -41,7 +42,7 @@ func TestAddSensorReadingReplaySensor(t *testing.T) {
 	reading := []byte("12345")
 	readingTimestamp := time.Now().UTC()
 	cf := cartofacade.Mock{}
-	config := SensorProcessConfig{
+	config := Config{
 		Logger:      logger,
 		CartoFacade: &cf,
 		LidarName:   "good_lidar",
@@ -138,7 +139,7 @@ func TestAddSensorReadingLiveReadings(t *testing.T) {
 	cf := cartofacade.Mock{}
 	reading := []byte("12345")
 	readingTimestamp := time.Now().UTC()
-	config := SensorProcessConfig{
+	config := Config{
 		Logger:      logger,
 		CartoFacade: &cf,
 		LidarName:   "good_lidar",
@@ -247,7 +248,7 @@ func TestAddSensorReading(t *testing.T) {
 	logger := golog.NewTestLogger(t)
 	cf := cartofacade.Mock{}
 
-	config := SensorProcessConfig{
+	config := Config{
 		Logger:      logger,
 		CartoFacade: &cf,
 		DataRateMs:  200,
@@ -409,4 +410,3 @@ func TestAddSensorReading(t *testing.T) {
 		test.That(t, calls[1].readingTimestamp.Before(calls[2].readingTimestamp), test.ShouldBeTrue)
 	})
 }
-
diff --git a/testhelper/testhelper.go b/testhelper/testhelper.go
index b0318323..ce835b14 100644
--- a/testhelper/testhelper.go
+++ b/testhelper/testhelper.go
@@ -3,6 +3,7 @@ package testhelper
 import (
 	"context"
 	"errors"
+
 	"github.com/viamrobotics/gostream"
 	"go.viam.com/rdk/components/camera"
 	"go.viam.com/rdk/pointcloud"
@@ -32,8 +33,8 @@ func SetupDeps(sensors []string) resource.Dependencies {
 			deps[camera.Named(sensor)] = getReplaySensor(BadTime)
 		case "invalid_sensor":
 			deps[camera.Named(sensor)] = getInvalidSensor()
-    default:
-      continue
+		default:
+			continue
 		}
 	}
 	return deps
diff --git a/viam-cartographer.go b/viam-cartographer.go
index 7a3863de..68f8deba 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -34,8 +34,10 @@ import (
 )
 
 // Model is the model name of cartographer.
-var Model = resource.NewModel("viam", "slam", "cartographer")
-var cartoLib cartofacade.CartoLib
+var (
+	Model    = resource.NewModel("viam", "slam", "cartographer")
+	cartoLib cartofacade.CartoLib
+)
 
 const (
 	// DefaultExecutableName is what this program expects to call to start the cartographer grpc server.
@@ -98,7 +100,7 @@ func init() {
 
 // InitCartoLib is run to initialize the cartographer library
 // must be called before module.AddModelFromRegistry is
-// called
+// called.
 func InitCartoLib(logger golog.Logger) error {
 	minloglevel := 1 // warn
 	vlog := 0        //  disabled
@@ -119,7 +121,7 @@ func TerminateCartoLib() error {
 }
 
 func initSensorProcess(cancelCtx context.Context, cartoSvc *cartographerService) {
-	spConfig := sensorprocess.SensorProcessConfig{
+	spConfig := sensorprocess.Config{
 		CartoFacade:      &cartoSvc.cartofacade,
 		Lidar:            cartoSvc.lidar,
 		LidarName:        cartoSvc.primarySensorName,
@@ -128,7 +130,7 @@ func initSensorProcess(cancelCtx context.Context, cartoSvc *cartographerService)
 		Logger:           cartoSvc.logger,
 		TelemetryEnabled: cartoSvc.logger.Level() == zapcore.DebugLevel,
 	}
-	sensorprocess.StartSensorProcess(cancelCtx, spConfig)
+	sensorprocess.Start(cancelCtx, spConfig)
 }
 
 // New returns a new slam service for the given robot.
@@ -230,7 +232,7 @@ func New(
 	return cartoSvc, err
 }
 
-// TODO: write a test for this
+// TODO: write a test for this.
 func parseCartoAlgoConfig(configParams map[string]string, logger golog.Logger) (cartofacade.CartoAlgoConfig, error) {
 	cartoAlgoCfg := defaultCartoAlgoCfg
 	logger.Warnf("NICK: defaultCartoAlgoCfgPtr: %p, cartoAlgoCfgPtr: %p", &defaultCartoAlgoCfg, &cartoAlgoCfg)
@@ -343,7 +345,7 @@ func initCartoFacade(cancelCtx context.Context, cartoSvc *cartographerService) (
 	return cartoSvc, nil
 }
 
-func initCartoGrpcServer(ctx context.Context, cancelCtx context.Context, cartoSvc *cartographerService) (*cartographerService, error) {
+func initCartoGrpcServer(ctx, cancelCtx context.Context, cartoSvc *cartographerService) (*cartographerService, error) {
 	// Set up the data directories
 	if err := vcConfig.SetupDirectories(cartoSvc.dataDirectory, cartoSvc.logger); err != nil {
 		return nil, err

From e1189cb0d775c0000433d699911fe14a6dfa74a7 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Fri, 7 Jul 2023 17:09:23 -0400
Subject: [PATCH 05/32] wip

---
 sensorprocess/sensorprocess_test.go | 1 -
 viam-cartographer.go                | 9 +++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/sensorprocess/sensorprocess_test.go b/sensorprocess/sensorprocess_test.go
index 139ada1d..943749fd 100644
--- a/sensorprocess/sensorprocess_test.go
+++ b/sensorprocess/sensorprocess_test.go
@@ -11,7 +11,6 @@ import (
 
 	"github.com/viamrobotics/viam-cartographer/cartofacade"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
-	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
 	"github.com/viamrobotics/viam-cartographer/testhelper"
 )
 
diff --git a/viam-cartographer.go b/viam-cartographer.go
index 68f8deba..82423cce 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -160,6 +160,11 @@ func New(
 			c.Model.Name, svcConfig.ConfigParams["mode"])
 	}
 
+	// Set up the data directories
+	if err := vcConfig.SetupDirectories(svcConfig.DataDirectory, logger); err != nil {
+		return nil, err
+	}
+
 	port, dataRateMsec, mapRateSec, useLiveData, deleteProcessedData, modularizationV2Enabled, err := vcConfig.GetOptionalParameters(
 		svcConfig,
 		localhost0,
@@ -346,10 +351,6 @@ func initCartoFacade(cancelCtx context.Context, cartoSvc *cartographerService) (
 }
 
 func initCartoGrpcServer(ctx, cancelCtx context.Context, cartoSvc *cartographerService) (*cartographerService, error) {
-	// Set up the data directories
-	if err := vcConfig.SetupDirectories(cartoSvc.dataDirectory, cartoSvc.logger); err != nil {
-		return nil, err
-	}
 
 	if cartoSvc.primarySensorName != "" {
 		if err := dim2d.ValidateGetAndSaveData(cancelCtx, cartoSvc.dataDirectory, cartoSvc.lidar,

From 79dfb996667e75ca311d0c7dbac12a096cf63f21 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Fri, 7 Jul 2023 17:21:43 -0400
Subject: [PATCH 06/32] fix tests

---
 viam-cartographer.go | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/viam-cartographer.go b/viam-cartographer.go
index 82423cce..68990235 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -232,8 +232,9 @@ func New(
 	}
 	cartoSvc, err = initCartoGrpcServer(ctx, cancelCtx, cartoSvc)
 	if err != nil {
-		success = true
+		return nil, err
 	}
+	success = true
 	return cartoSvc, err
 }
 
@@ -351,23 +352,22 @@ func initCartoFacade(cancelCtx context.Context, cartoSvc *cartographerService) (
 }
 
 func initCartoGrpcServer(ctx, cancelCtx context.Context, cartoSvc *cartographerService) (*cartographerService, error) {
-
 	if cartoSvc.primarySensorName != "" {
 		if err := dim2d.ValidateGetAndSaveData(cancelCtx, cartoSvc.dataDirectory, cartoSvc.lidar,
 			cartoSvc.sensorValidationMaxTimeoutSec, cartoSvc.sensorValidationIntervalSec, cartoSvc.logger); err != nil {
-			return nil, errors.Wrap(err, "getting and saving data failed")
+			return cartoSvc, errors.Wrap(err, "getting and saving data failed")
 		}
 		cartoSvc.StartDataProcess(cancelCtx, cartoSvc.lidar, nil)
 		cartoSvc.logger.Debugf("Reading data from sensor: %v", cartoSvc.primarySensorName)
 	}
 
 	if err := cartoSvc.StartSLAMProcess(ctx); err != nil {
-		return nil, errors.Wrap(err, "error with slam service slam process")
+		return cartoSvc, errors.Wrap(err, "error with slam service slam process")
 	}
 
 	client, clientClose, err := vcConfig.SetupGRPCConnection(ctx, cartoSvc.port, cartoSvc.dialMaxTimeoutSec, cartoSvc.logger)
 	if err != nil {
-		return nil, errors.Wrap(err, "error with initial grpc client to slam algorithm")
+		return cartoSvc, errors.Wrap(err, "error with initial grpc client to slam algorithm")
 	}
 	cartoSvc.clientAlgo = client
 	cartoSvc.clientAlgoClose = clientClose

From da0e6bcf17cf231be9137c144bb41f871ad56a3f Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Fri, 7 Jul 2023 17:22:40 -0400
Subject: [PATCH 07/32] wip

---
 viam-cartographer.go | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/viam-cartographer.go b/viam-cartographer.go
index 68990235..15a1592f 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -235,7 +235,7 @@ func New(
 		return nil, err
 	}
 	success = true
-	return cartoSvc, err
+	return cartoSvc, nil
 }
 
 // TODO: write a test for this.

From 73a15b4e8d5d1655f03dcbf9673724d1b1ea6161 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Mon, 10 Jul 2023 10:41:14 -0400
Subject: [PATCH 08/32] clean up tests

---
 cartofacade/capi.go               |   2 +
 integration_test.go               |  25 +++---
 internal/dim-2d/dim-2d_test.go    |  13 +--
 internal/testhelper/testhelper.go | 140 ++----------------------------
 testhelper/testhelper.go          |  54 +++++++++++-
 viam-cartographer.go              |   5 +-
 viam-cartographer_test.go         |   9 +-
 7 files changed, 88 insertions(+), 160 deletions(-)

diff --git a/cartofacade/capi.go b/cartofacade/capi.go
index ba7984b2..e74742a2 100644
--- a/cartofacade/capi.go
+++ b/cartofacade/capi.go
@@ -77,7 +77,9 @@ type GetPosition struct {
 type LidarConfig int64
 
 const (
+	// TwoD LidarConfig denotes a 2d lidar
 	TwoD LidarConfig = iota
+	// ThreeD LidarConfig denotes a 3d lidar
 	ThreeD
 )
 
diff --git a/integration_test.go b/integration_test.go
index e4c863b5..3621d0cf 100644
--- a/integration_test.go
+++ b/integration_test.go
@@ -27,6 +27,7 @@ import (
 	vcConfig "github.com/viamrobotics/viam-cartographer/config"
 	"github.com/viamrobotics/viam-cartographer/dataprocess"
 	"github.com/viamrobotics/viam-cartographer/internal/testhelper"
+	th "github.com/viamrobotics/viam-cartographer/testhelper"
 )
 
 const (
@@ -124,13 +125,13 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	}
 
 	// Release point cloud for service validation
-	testhelper.IntegrationLidarReleasePointCloudChan <- 1
+	th.IntegrationLidarReleasePointCloudChan <- 1
 	// Create slam service using a real cartographer binary
 	svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, true, viamcartographer.DefaultExecutableName)
 	test.That(t, err, test.ShouldBeNil)
 
 	// Release point cloud, since cartographer looks for the second most recent point cloud
-	testhelper.IntegrationLidarReleasePointCloudChan <- 1
+	th.IntegrationLidarReleasePointCloudChan <- 1
 
 	// Make sure we initialize in mapping mode
 	logReader := svc.(testhelper.Service).GetSLAMProcessBufferedLogReader()
@@ -146,9 +147,9 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	}
 
 	// Wait for cartographer to finish processing data
-	for i := 0; i < testhelper.NumPointClouds-2; i++ {
+	for i := 0; i < th.NumPointClouds-2; i++ {
 		t.Logf("Find log line for point cloud %v", i)
-		testhelper.IntegrationLidarReleasePointCloudChan <- 1
+		th.IntegrationLidarReleasePointCloudChan <- 1
 		for {
 			line, err := logReader.ReadString('\n')
 			test.That(t, err, test.ShouldBeNil)
@@ -278,7 +279,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	}
 
 	// Release point cloud for service validation
-	testhelper.IntegrationLidarReleasePointCloudChan <- 1
+	th.IntegrationLidarReleasePointCloudChan <- 1
 	// Create slam service using a real cartographer binary
 	svc, err = testhelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName)
 	test.That(t, err, test.ShouldBeNil)
@@ -296,10 +297,10 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	}
 
 	// Release point cloud, since cartographer looks for the second most recent point cloud
-	testhelper.IntegrationLidarReleasePointCloudChan <- 1
-	for i := 0; i < testhelper.NumPointClouds-2; i++ {
+	th.IntegrationLidarReleasePointCloudChan <- 1
+	for i := 0; i < th.NumPointClouds-2; i++ {
 		t.Logf("Find log line for point cloud %v", i)
-		testhelper.IntegrationLidarReleasePointCloudChan <- 1
+		th.IntegrationLidarReleasePointCloudChan <- 1
 		for {
 			line, err := logReader.ReadString('\n')
 			test.That(t, err, test.ShouldBeNil)
@@ -354,7 +355,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	}
 
 	// Release point cloud for service validation
-	testhelper.IntegrationLidarReleasePointCloudChan <- 1
+	th.IntegrationLidarReleasePointCloudChan <- 1
 	// Create slam service using a real cartographer binary
 	svc, err = testhelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName)
 	test.That(t, err, test.ShouldBeNil)
@@ -372,10 +373,10 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	}
 
 	// Release point cloud, since cartographer looks for the second most recent point cloud
-	testhelper.IntegrationLidarReleasePointCloudChan <- 1
-	for i := 0; i < testhelper.NumPointClouds-2; i++ {
+	th.IntegrationLidarReleasePointCloudChan <- 1
+	for i := 0; i < th.NumPointClouds-2; i++ {
 		t.Logf("Find log line for point cloud %v", i)
-		testhelper.IntegrationLidarReleasePointCloudChan <- 1
+		th.IntegrationLidarReleasePointCloudChan <- 1
 		for {
 			line, err := logReader.ReadString('\n')
 			test.That(t, err, test.ShouldBeNil)
diff --git a/internal/dim-2d/dim-2d_test.go b/internal/dim-2d/dim-2d_test.go
index e2ce2aca..44794b8c 100644
--- a/internal/dim-2d/dim-2d_test.go
+++ b/internal/dim-2d/dim-2d_test.go
@@ -13,6 +13,7 @@ import (
 	dim2d "github.com/viamrobotics/viam-cartographer/internal/dim-2d"
 	"github.com/viamrobotics/viam-cartographer/internal/testhelper"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
+	th "github.com/viamrobotics/viam-cartographer/testhelper"
 )
 
 func TestNewLidar(t *testing.T) {
@@ -20,7 +21,7 @@ func TestNewLidar(t *testing.T) {
 
 	t.Run("No sensor provided", func(t *testing.T) {
 		sensors := []string{}
-		deps := testhelper.SetupDeps(sensors)
+		deps := th.SetupDeps(sensors)
 		actualLidar, err := dim2d.NewLidar(context.Background(), deps, sensors, logger)
 		expectedLidar := lidar.Lidar{}
 		test.That(t, actualLidar, test.ShouldResemble, expectedLidar)
@@ -29,7 +30,7 @@ func TestNewLidar(t *testing.T) {
 
 	t.Run("Failed lidar creation due to more than one sensor provided", func(t *testing.T) {
 		sensors := []string{"lidar", "one-too-many"}
-		deps := testhelper.SetupDeps(sensors)
+		deps := th.SetupDeps(sensors)
 		actualLidar, err := dim2d.NewLidar(context.Background(), deps, sensors, logger)
 		expectedLidar := lidar.Lidar{}
 		test.That(t, actualLidar, test.ShouldResemble, expectedLidar)
@@ -39,7 +40,7 @@ func TestNewLidar(t *testing.T) {
 
 	t.Run("Failed lidar creation with non-existing sensor", func(t *testing.T) {
 		sensors := []string{"gibberish"}
-		deps := testhelper.SetupDeps(sensors)
+		deps := th.SetupDeps(sensors)
 		actualLidar, err := dim2d.NewLidar(context.Background(), deps, sensors, logger)
 		expectedLidar := lidar.Lidar{}
 		test.That(t, actualLidar, test.ShouldResemble, expectedLidar)
@@ -51,7 +52,7 @@ func TestNewLidar(t *testing.T) {
 	t.Run("Successful lidar creation", func(t *testing.T) {
 		sensors := []string{"good_lidar"}
 		ctx := context.Background()
-		deps := testhelper.SetupDeps(sensors)
+		deps := th.SetupDeps(sensors)
 		actualLidar, err := dim2d.NewLidar(ctx, deps, sensors, logger)
 		test.That(t, actualLidar.Name, test.ShouldEqual, sensors[0])
 		test.That(t, err, test.ShouldBeNil)
@@ -70,7 +71,7 @@ func TestGetAndSaveData(t *testing.T) {
 
 	t.Run("Successful call to GetAndSaveData", func(t *testing.T) {
 		sensors := []string{"good_lidar"}
-		deps := testhelper.SetupDeps(sensors)
+		deps := th.SetupDeps(sensors)
 		actualLidar, err := dim2d.NewLidar(ctx, deps, sensors, logger)
 		test.That(t, actualLidar.Name, test.ShouldEqual, sensors[0])
 		test.That(t, err, test.ShouldBeNil)
@@ -94,7 +95,7 @@ func TestValidateGetAndSaveData(t *testing.T) {
 
 	t.Run("Successful call to ValidateGetAndSaveData", func(t *testing.T) {
 		sensors := []string{"good_lidar"}
-		deps := testhelper.SetupDeps(sensors)
+		deps := th.SetupDeps(sensors)
 		actualLidar, err := dim2d.NewLidar(ctx, deps, sensors, logger)
 		test.That(t, actualLidar.Name, test.ShouldEqual, sensors[0])
 		test.That(t, err, test.ShouldBeNil)
diff --git a/internal/testhelper/testhelper.go b/internal/testhelper/testhelper.go
index ede17f3b..4d6196fc 100644
--- a/internal/testhelper/testhelper.go
+++ b/internal/testhelper/testhelper.go
@@ -7,34 +7,24 @@ import (
 	"bufio"
 	"context"
 	"os"
-	"strconv"
-	"sync/atomic"
 	"testing"
+	"time"
 
 	"github.com/edaniels/golog"
 	"github.com/pkg/errors"
-	"github.com/viamrobotics/gostream"
-	"go.viam.com/rdk/components/camera"
-	"go.viam.com/rdk/pointcloud"
 	"go.viam.com/rdk/resource"
-	"go.viam.com/rdk/rimage/transform"
 	"go.viam.com/rdk/services/slam"
-	"go.viam.com/rdk/testutils/inject"
-	"go.viam.com/rdk/utils/contextutils"
 	"go.viam.com/test"
-	"go.viam.com/utils/artifact"
 	"go.viam.com/utils/pexec"
 
 	viamcartographer "github.com/viamrobotics/viam-cartographer"
 	vcConfig "github.com/viamrobotics/viam-cartographer/config"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
+	th "github.com/viamrobotics/viam-cartographer/testhelper"
 )
 
 const (
-	// NumPointClouds is the number of pointclouds saved in artifact
-	// for the cartographer integration tests.
-	NumPointClouds = 15
-	// SensorValidationMaxTimeoutSecForTest is used in the ValidateGetAndSaveData
+	// SensorValidationMaxTimeoutSecForTest is used in the ValidateGetAndSaveDa
 	// function to ensure that the sensor in the GetAndSaveData function
 	// returns data within an acceptable time.
 	SensorValidationMaxTimeoutSecForTest = 1
@@ -43,16 +33,8 @@ const (
 	// sensor that is used in the GetAndSaveData function.
 	SensorValidationIntervalSecForTest = 1
 	testDialMaxTimeoutSec              = 1
-	// TestTime can be used to test specific timestamps provided by a replay sensor.
-	TestTime = "2006-01-02T15:04:05.9999Z"
-	// BadTime can be used to represent something that should cause an error while parsing it as a time.
-	BadTime = "NOT A TIME"
 )
 
-// IntegrationLidarReleasePointCloudChan is the lidar pointcloud release
-// channel for the integration tests.
-var IntegrationLidarReleasePointCloudChan = make(chan int, 1)
-
 // Service in the internal package includes additional exported functions relating to the data and
 // slam processes in the slam service. These functions are not exported to the user. This resolves
 // a circular import caused by the inject package.
@@ -65,118 +47,6 @@ type Service interface {
 	GetSLAMProcessBufferedLogReader() bufio.Reader
 }
 
-// SetupDeps returns the dependencies based on the sensors passed as arguments.
-func SetupDeps(sensors []string) resource.Dependencies {
-	deps := make(resource.Dependencies)
-
-	for _, sensor := range sensors {
-		switch sensor {
-		case "good_lidar":
-			deps[camera.Named(sensor)] = getGoodLidar()
-		case "replay_sensor":
-			deps[camera.Named(sensor)] = getReplaySensor(TestTime)
-		case "invalid_replay_sensor":
-			deps[camera.Named(sensor)] = getReplaySensor(BadTime)
-		case "invalid_sensor":
-			deps[camera.Named(sensor)] = getInvalidSensor()
-		case "gibberish":
-			return deps
-		case "cartographer_int_lidar":
-			deps[camera.Named(sensor)] = getIntegrationLidar()
-		default:
-			continue
-		}
-	}
-	return deps
-}
-
-func getGoodLidar() *inject.Camera {
-	cam := &inject.Camera{}
-	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
-		return pointcloud.New(), nil
-	}
-	cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) {
-		return nil, errors.New("lidar not camera")
-	}
-	cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) {
-		return nil, transform.NewNoIntrinsicsError("")
-	}
-	cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) {
-		return camera.Properties{}, nil
-	}
-	return cam
-}
-
-func getReplaySensor(testTime string) *inject.Camera {
-	cam := &inject.Camera{}
-	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
-		md := ctx.Value(contextutils.MetadataContextKey)
-		if mdMap, ok := md.(map[string][]string); ok {
-			mdMap[contextutils.TimeRequestedMetadataKey] = []string{testTime}
-		}
-		return pointcloud.New(), nil
-	}
-	cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) {
-		return nil, errors.New("lidar not camera")
-	}
-	cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) {
-		return nil, transform.NewNoIntrinsicsError("")
-	}
-	cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) {
-		return camera.Properties{}, nil
-	}
-	return cam
-}
-
-func getInvalidSensor() *inject.Camera {
-	cam := &inject.Camera{}
-	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
-		return nil, errors.New("invalid sensor")
-	}
-	cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) {
-		return nil, errors.New("invalid sensor")
-	}
-	cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) {
-		return nil, transform.NewNoIntrinsicsError("")
-	}
-	return cam
-}
-
-func getIntegrationLidar() *inject.Camera {
-	cam := &inject.Camera{}
-	var index uint64
-	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
-		select {
-		case <-IntegrationLidarReleasePointCloudChan:
-			i := atomic.AddUint64(&index, 1) - 1
-			if i >= NumPointClouds {
-				return nil, errors.New("No more cartographer point clouds")
-			}
-			file, err := os.Open(artifact.MustPath("viam-cartographer/mock_lidar/" + strconv.FormatUint(i, 10) + ".pcd"))
-			if err != nil {
-				return nil, err
-			}
-			pointCloud, err := pointcloud.ReadPCD(file)
-			if err != nil {
-				return nil, err
-			}
-			return pointCloud, nil
-		default:
-			return nil, errors.Errorf("Lidar not ready to return point cloud %v", atomic.LoadUint64(&index))
-		}
-	}
-	cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) {
-		return nil, errors.New("lidar not camera")
-	}
-	cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) {
-		return nil, transform.NewNoIntrinsicsError("")
-	}
-	cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) {
-		return camera.Properties{}, nil
-	}
-	return cam
-}
-
 // ClearDirectory deletes the contents in the path directory
 // without deleting path itself.
 func ClearDirectory(t *testing.T, path string) {
@@ -200,7 +70,7 @@ func CreateSLAMService(
 	cfgService := resource.Config{Name: "test", API: slam.API, Model: viamcartographer.Model}
 	cfgService.ConvertedAttributes = cfg
 
-	deps := SetupDeps(cfg.Sensors)
+	deps := th.SetupDeps(cfg.Sensors)
 
 	sensorDeps, err := cfg.Validate("path")
 	if err != nil {
@@ -218,7 +88,7 @@ func CreateSLAMService(
 		SensorValidationMaxTimeoutSecForTest,
 		SensorValidationIntervalSecForTest,
 		testDialMaxTimeoutSec,
-		viamcartographer.DefaultCartoFacadeTimeout,
+		5*time.Second,
 	)
 	if err != nil {
 		test.That(t, svc, test.ShouldBeNil)
diff --git a/testhelper/testhelper.go b/testhelper/testhelper.go
index ce835b14..8c39aeea 100644
--- a/testhelper/testhelper.go
+++ b/testhelper/testhelper.go
@@ -1,9 +1,13 @@
+// Package testhelper provides test helpers which don't depend on viamcartographer
 package testhelper
 
 import (
 	"context"
-	"errors"
+	"os"
+	"strconv"
+	"sync/atomic"
 
+	"github.com/pkg/errors"
 	"github.com/viamrobotics/gostream"
 	"go.viam.com/rdk/components/camera"
 	"go.viam.com/rdk/pointcloud"
@@ -11,15 +15,24 @@ import (
 	"go.viam.com/rdk/rimage/transform"
 	"go.viam.com/rdk/testutils/inject"
 	"go.viam.com/rdk/utils/contextutils"
+	"go.viam.com/utils/artifact"
 )
 
 const (
+	// NumPointClouds is the number of pointclouds saved in artifact
+	// for the cartographer integration tests.
+	NumPointClouds = 15
 	// TestTime can be used to test specific timestamps provided by a replay sensor.
 	TestTime = "2006-01-02T15:04:05.9999Z"
 	// BadTime can be used to represent something that should cause an error while parsing it as a time.
 	BadTime = "NOT A TIME"
 )
 
+// IntegrationLidarReleasePointCloudChan is the lidar pointcloud release
+// channel for the integration tests.
+var IntegrationLidarReleasePointCloudChan = make(chan int, 1)
+
+// SetupDeps returns the dependencies based on the sensors passed as arguments.
 func SetupDeps(sensors []string) resource.Dependencies {
 	deps := make(resource.Dependencies)
 
@@ -33,6 +46,10 @@ func SetupDeps(sensors []string) resource.Dependencies {
 			deps[camera.Named(sensor)] = getReplaySensor(BadTime)
 		case "invalid_sensor":
 			deps[camera.Named(sensor)] = getInvalidSensor()
+		case "gibberish":
+			return deps
+		case "cartographer_int_lidar":
+			deps[camera.Named(sensor)] = getIntegrationLidar()
 		default:
 			continue
 		}
@@ -91,3 +108,38 @@ func getInvalidSensor() *inject.Camera {
 	}
 	return cam
 }
+
+func getIntegrationLidar() *inject.Camera {
+	cam := &inject.Camera{}
+	var index uint64
+	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
+		select {
+		case <-IntegrationLidarReleasePointCloudChan:
+			i := atomic.AddUint64(&index, 1) - 1
+			if i >= NumPointClouds {
+				return nil, errors.New("No more cartographer point clouds")
+			}
+			file, err := os.Open(artifact.MustPath("viam-cartographer/mock_lidar/" + strconv.FormatUint(i, 10) + ".pcd"))
+			if err != nil {
+				return nil, err
+			}
+			pointCloud, err := pointcloud.ReadPCD(file)
+			if err != nil {
+				return nil, err
+			}
+			return pointCloud, nil
+		default:
+			return nil, errors.Errorf("Lidar not ready to return point cloud %v", atomic.LoadUint64(&index))
+		}
+	}
+	cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) {
+		return nil, errors.New("lidar not camera")
+	}
+	cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) {
+		return nil, transform.NewNoIntrinsicsError("")
+	}
+	cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) {
+		return camera.Properties{}, nil
+	}
+	return cam
+}
diff --git a/viam-cartographer.go b/viam-cartographer.go
index 15a1592f..15fca908 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -49,7 +49,7 @@ const (
 	defaultSensorValidationIntervalSec   = 1
 	parsePortMaxTimeoutSec               = 60
 	localhost0                           = "localhost:0"
-	DefaultCartoFacadeTimeout            = 5 * time.Second
+	defaultCartoFacadeTimeout            = 5 * time.Second
 )
 
 var defaultCartoAlgoCfg = cartofacade.CartoAlgoConfig{
@@ -92,7 +92,7 @@ func init() {
 				defaultSensorValidationMaxTimeoutSec,
 				defaultSensorValidationIntervalSec,
 				defaultDialMaxTimeoutSec,
-				DefaultCartoFacadeTimeout,
+				defaultCartoFacadeTimeout,
 			)
 		},
 	})
@@ -116,6 +116,7 @@ func InitCartoLib(logger golog.Logger) error {
 	return nil
 }
 
+// TerminateCartoLib is run to terminate the cartographer library.
 func TerminateCartoLib() error {
 	return cartoLib.Terminate()
 }
diff --git a/viam-cartographer_test.go b/viam-cartographer_test.go
index f1ca0db8..8e108ff4 100644
--- a/viam-cartographer_test.go
+++ b/viam-cartographer_test.go
@@ -24,6 +24,7 @@ import (
 	vcConfig "github.com/viamrobotics/viam-cartographer/config"
 	"github.com/viamrobotics/viam-cartographer/internal/testhelper"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
+	th "github.com/viamrobotics/viam-cartographer/testhelper"
 )
 
 const (
@@ -215,7 +216,7 @@ func TestDataProcess(t *testing.T) {
 		defer testhelper.ClearDirectory(t, filepath.Join(dataDir, "data"))
 
 		sensors := []string{"good_lidar"}
-		lidar, err := lidar.New(testhelper.SetupDeps(sensors), sensors, 0)
+		lidar, err := lidar.New(th.SetupDeps(sensors), sensors, 0)
 		test.That(t, err, test.ShouldBeNil)
 
 		cancelCtx, cancelFunc := context.WithCancel(context.Background())
@@ -232,7 +233,7 @@ func TestDataProcess(t *testing.T) {
 	t.Run("Failed startup of data process with invalid sensor "+
 		"that errors during call to NextPointCloud", func(t *testing.T) {
 		sensors := []string{"invalid_sensor"}
-		lidar, err := lidar.New(testhelper.SetupDeps(sensors), sensors, 0)
+		lidar, err := lidar.New(th.SetupDeps(sensors), sensors, 0)
 		test.That(t, err, test.ShouldBeNil)
 
 		cancelCtx, cancelFunc := context.WithCancel(context.Background())
@@ -250,7 +251,7 @@ func TestDataProcess(t *testing.T) {
 		defer testhelper.ClearDirectory(t, filepath.Join(dataDir, "data"))
 
 		sensors := []string{"replay_sensor"}
-		lidar, err := lidar.New(testhelper.SetupDeps(sensors), sensors, 0)
+		lidar, err := lidar.New(th.SetupDeps(sensors), sensors, 0)
 		test.That(t, err, test.ShouldBeNil)
 
 		cancelCtx, cancelFunc := context.WithCancel(context.Background())
@@ -261,7 +262,7 @@ func TestDataProcess(t *testing.T) {
 		cancelFunc()
 		files, err := os.ReadDir(filepath.Join(dataDir, "data"))
 		test.That(t, len(files), test.ShouldBeGreaterThanOrEqualTo, 1)
-		test.That(t, files[0].Name(), test.ShouldContainSubstring, testhelper.TestTime)
+		test.That(t, files[0].Name(), test.ShouldContainSubstring, th.TestTime)
 		test.That(t, err, test.ShouldBeNil)
 	})
 

From ca859bb7d56b1bad13ef3d60248f59d1ff1e1c4d Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Mon, 10 Jul 2023 10:44:32 -0400
Subject: [PATCH 09/32] fix test helper names

---
 integration_test.go               | 62 ++++++++++++++--------------
 internal/dim-2d/dim-2d_test.go    | 28 ++++++-------
 internal/testhelper/testhelper.go |  4 +-
 viam-cartographer_test.go         | 68 +++++++++++++++----------------
 4 files changed, 81 insertions(+), 81 deletions(-)

diff --git a/integration_test.go b/integration_test.go
index 3621d0cf..9854778a 100644
--- a/integration_test.go
+++ b/integration_test.go
@@ -26,8 +26,8 @@ import (
 	viamcartographer "github.com/viamrobotics/viam-cartographer"
 	vcConfig "github.com/viamrobotics/viam-cartographer/config"
 	"github.com/viamrobotics/viam-cartographer/dataprocess"
-	"github.com/viamrobotics/viam-cartographer/internal/testhelper"
-	th "github.com/viamrobotics/viam-cartographer/testhelper"
+	internaltesthelper "github.com/viamrobotics/viam-cartographer/internal/testhelper"
+	"github.com/viamrobotics/viam-cartographer/testhelper"
 )
 
 const (
@@ -100,7 +100,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 		t.Skip()
 	}
 
-	dataDir, err := testhelper.CreateTempFolderArchitecture(logger)
+	dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger)
 	test.That(t, err, test.ShouldBeNil)
 
 	prevNumFiles := 0
@@ -125,16 +125,16 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	}
 
 	// Release point cloud for service validation
-	th.IntegrationLidarReleasePointCloudChan <- 1
+	testhelper.IntegrationLidarReleasePointCloudChan <- 1
 	// Create slam service using a real cartographer binary
-	svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, true, viamcartographer.DefaultExecutableName)
+	svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, true, viamcartographer.DefaultExecutableName)
 	test.That(t, err, test.ShouldBeNil)
 
 	// Release point cloud, since cartographer looks for the second most recent point cloud
-	th.IntegrationLidarReleasePointCloudChan <- 1
+	testhelper.IntegrationLidarReleasePointCloudChan <- 1
 
 	// Make sure we initialize in mapping mode
-	logReader := svc.(testhelper.Service).GetSLAMProcessBufferedLogReader()
+	logReader := svc.(internaltesthelper.Service).GetSLAMProcessBufferedLogReader()
 	for {
 		line, err := logReader.ReadString('\n')
 		test.That(t, err, test.ShouldBeNil)
@@ -147,14 +147,14 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	}
 
 	// Wait for cartographer to finish processing data
-	for i := 0; i < th.NumPointClouds-2; i++ {
+	for i := 0; i < testhelper.NumPointClouds-2; i++ {
 		t.Logf("Find log line for point cloud %v", i)
-		th.IntegrationLidarReleasePointCloudChan <- 1
+		testhelper.IntegrationLidarReleasePointCloudChan <- 1
 		for {
 			line, err := logReader.ReadString('\n')
 			test.That(t, err, test.ShouldBeNil)
 			if strings.Contains(line, "Passed sensor data to SLAM") {
-				prevNumFiles = testhelper.CheckDeleteProcessedData(
+				prevNumFiles = internaltesthelper.CheckDeleteProcessedData(
 					t,
 					subAlgo,
 					dataDir,
@@ -207,11 +207,11 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	}
 
 	// Create slam service using a real cartographer binary
-	svc, err = testhelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName)
+	svc, err = internaltesthelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName)
 	test.That(t, err, test.ShouldBeNil)
 
 	// Make sure we initialize in mapping mode
-	logReader = svc.(testhelper.Service).GetSLAMProcessBufferedLogReader()
+	logReader = svc.(internaltesthelper.Service).GetSLAMProcessBufferedLogReader()
 	for {
 		line, err := logReader.ReadString('\n')
 		test.That(t, err, test.ShouldBeNil)
@@ -227,7 +227,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 		line, err := logReader.ReadString('\n')
 		test.That(t, err, test.ShouldBeNil)
 		if strings.Contains(line, "Passed sensor data to SLAM") {
-			prevNumFiles = testhelper.CheckDeleteProcessedData(
+			prevNumFiles = internaltesthelper.CheckDeleteProcessedData(
 				t,
 				subAlgo,
 				dataDir,
@@ -253,7 +253,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	time.Sleep(time.Millisecond * cartoSleepMsec)
 
 	// Remove existing pointclouds, but leave maps and config (so we keep the lua files).
-	test.That(t, testhelper.ResetFolder(dataDir+"/data"), test.ShouldBeNil)
+	test.That(t, internaltesthelper.ResetFolder(dataDir+"/data"), test.ShouldBeNil)
 	prevNumFiles = 0
 
 	// Count the initial number of maps in the map directory (should equal 1)
@@ -279,13 +279,13 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	}
 
 	// Release point cloud for service validation
-	th.IntegrationLidarReleasePointCloudChan <- 1
+	testhelper.IntegrationLidarReleasePointCloudChan <- 1
 	// Create slam service using a real cartographer binary
-	svc, err = testhelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName)
+	svc, err = internaltesthelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName)
 	test.That(t, err, test.ShouldBeNil)
 
 	// Make sure we initialize in localization mode
-	logReader = svc.(testhelper.Service).GetSLAMProcessBufferedLogReader()
+	logReader = svc.(internaltesthelper.Service).GetSLAMProcessBufferedLogReader()
 	for {
 		line, err := logReader.ReadString('\n')
 		test.That(t, err, test.ShouldBeNil)
@@ -297,15 +297,15 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	}
 
 	// Release point cloud, since cartographer looks for the second most recent point cloud
-	th.IntegrationLidarReleasePointCloudChan <- 1
-	for i := 0; i < th.NumPointClouds-2; i++ {
+	testhelper.IntegrationLidarReleasePointCloudChan <- 1
+	for i := 0; i < testhelper.NumPointClouds-2; i++ {
 		t.Logf("Find log line for point cloud %v", i)
-		th.IntegrationLidarReleasePointCloudChan <- 1
+		testhelper.IntegrationLidarReleasePointCloudChan <- 1
 		for {
 			line, err := logReader.ReadString('\n')
 			test.That(t, err, test.ShouldBeNil)
 			if strings.Contains(line, "Passed sensor data to SLAM") {
-				prevNumFiles = testhelper.CheckDeleteProcessedData(
+				prevNumFiles = internaltesthelper.CheckDeleteProcessedData(
 					t,
 					subAlgo,
 					dataDir,
@@ -321,7 +321,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	testCartographerMap(t, svc, true)
 
 	// Remove maps so that testing is done on the map generated by the internal map
-	test.That(t, testhelper.ResetFolder(dataDir+"/map"), test.ShouldBeNil)
+	test.That(t, internaltesthelper.ResetFolder(dataDir+"/map"), test.ShouldBeNil)
 
 	testCartographerInternalState(t, svc, dataDir)
 
@@ -335,7 +335,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	time.Sleep(time.Millisecond * cartoSleepMsec)
 
 	// Remove existing pointclouds, but leave maps and config (so we keep the lua files).
-	test.That(t, testhelper.ResetFolder(dataDir+"/data"), test.ShouldBeNil)
+	test.That(t, internaltesthelper.ResetFolder(dataDir+"/data"), test.ShouldBeNil)
 	prevNumFiles = 0
 
 	// Test online mode using the map generated in the offline test
@@ -355,13 +355,13 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	}
 
 	// Release point cloud for service validation
-	th.IntegrationLidarReleasePointCloudChan <- 1
+	testhelper.IntegrationLidarReleasePointCloudChan <- 1
 	// Create slam service using a real cartographer binary
-	svc, err = testhelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName)
+	svc, err = internaltesthelper.CreateSLAMService(t, attrCfg, golog.NewTestLogger(t), true, viamcartographer.DefaultExecutableName)
 	test.That(t, err, test.ShouldBeNil)
 
 	// Make sure we initialize in updating mode
-	logReader = svc.(testhelper.Service).GetSLAMProcessBufferedLogReader()
+	logReader = svc.(internaltesthelper.Service).GetSLAMProcessBufferedLogReader()
 	for {
 		line, err := logReader.ReadString('\n')
 		test.That(t, err, test.ShouldBeNil)
@@ -373,15 +373,15 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	}
 
 	// Release point cloud, since cartographer looks for the second most recent point cloud
-	th.IntegrationLidarReleasePointCloudChan <- 1
-	for i := 0; i < th.NumPointClouds-2; i++ {
+	testhelper.IntegrationLidarReleasePointCloudChan <- 1
+	for i := 0; i < testhelper.NumPointClouds-2; i++ {
 		t.Logf("Find log line for point cloud %v", i)
-		th.IntegrationLidarReleasePointCloudChan <- 1
+		testhelper.IntegrationLidarReleasePointCloudChan <- 1
 		for {
 			line, err := logReader.ReadString('\n')
 			test.That(t, err, test.ShouldBeNil)
 			if strings.Contains(line, "Passed sensor data to SLAM") {
-				prevNumFiles = testhelper.CheckDeleteProcessedData(
+				prevNumFiles = internaltesthelper.CheckDeleteProcessedData(
 					t,
 					subAlgo,
 					dataDir,
@@ -405,7 +405,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
 	testCartographerDir(t, dataDir, 2)
 
 	// Clear out directory
-	testhelper.ClearDirectory(t, dataDir)
+	internaltesthelper.ClearDirectory(t, dataDir)
 }
 
 // Checks the current slam directory to see if the number of files matches the expected amount.
diff --git a/internal/dim-2d/dim-2d_test.go b/internal/dim-2d/dim-2d_test.go
index 44794b8c..a42a0550 100644
--- a/internal/dim-2d/dim-2d_test.go
+++ b/internal/dim-2d/dim-2d_test.go
@@ -11,9 +11,9 @@ import (
 	"go.viam.com/test"
 
 	dim2d "github.com/viamrobotics/viam-cartographer/internal/dim-2d"
-	"github.com/viamrobotics/viam-cartographer/internal/testhelper"
+	internaltesthelper "github.com/viamrobotics/viam-cartographer/internal/testhelper"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
-	th "github.com/viamrobotics/viam-cartographer/testhelper"
+	"github.com/viamrobotics/viam-cartographer/testhelper"
 )
 
 func TestNewLidar(t *testing.T) {
@@ -21,7 +21,7 @@ func TestNewLidar(t *testing.T) {
 
 	t.Run("No sensor provided", func(t *testing.T) {
 		sensors := []string{}
-		deps := th.SetupDeps(sensors)
+		deps := testhelper.SetupDeps(sensors)
 		actualLidar, err := dim2d.NewLidar(context.Background(), deps, sensors, logger)
 		expectedLidar := lidar.Lidar{}
 		test.That(t, actualLidar, test.ShouldResemble, expectedLidar)
@@ -30,7 +30,7 @@ func TestNewLidar(t *testing.T) {
 
 	t.Run("Failed lidar creation due to more than one sensor provided", func(t *testing.T) {
 		sensors := []string{"lidar", "one-too-many"}
-		deps := th.SetupDeps(sensors)
+		deps := testhelper.SetupDeps(sensors)
 		actualLidar, err := dim2d.NewLidar(context.Background(), deps, sensors, logger)
 		expectedLidar := lidar.Lidar{}
 		test.That(t, actualLidar, test.ShouldResemble, expectedLidar)
@@ -40,7 +40,7 @@ func TestNewLidar(t *testing.T) {
 
 	t.Run("Failed lidar creation with non-existing sensor", func(t *testing.T) {
 		sensors := []string{"gibberish"}
-		deps := th.SetupDeps(sensors)
+		deps := testhelper.SetupDeps(sensors)
 		actualLidar, err := dim2d.NewLidar(context.Background(), deps, sensors, logger)
 		expectedLidar := lidar.Lidar{}
 		test.That(t, actualLidar, test.ShouldResemble, expectedLidar)
@@ -52,7 +52,7 @@ func TestNewLidar(t *testing.T) {
 	t.Run("Successful lidar creation", func(t *testing.T) {
 		sensors := []string{"good_lidar"}
 		ctx := context.Background()
-		deps := th.SetupDeps(sensors)
+		deps := testhelper.SetupDeps(sensors)
 		actualLidar, err := dim2d.NewLidar(ctx, deps, sensors, logger)
 		test.That(t, actualLidar.Name, test.ShouldEqual, sensors[0])
 		test.That(t, err, test.ShouldBeNil)
@@ -66,12 +66,12 @@ func TestNewLidar(t *testing.T) {
 func TestGetAndSaveData(t *testing.T) {
 	ctx := context.Background()
 	logger := golog.NewTestLogger(t)
-	dataDir, err := testhelper.CreateTempFolderArchitecture(logger)
+	dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger)
 	test.That(t, err, test.ShouldBeNil)
 
 	t.Run("Successful call to GetAndSaveData", func(t *testing.T) {
 		sensors := []string{"good_lidar"}
-		deps := th.SetupDeps(sensors)
+		deps := testhelper.SetupDeps(sensors)
 		actualLidar, err := dim2d.NewLidar(ctx, deps, sensors, logger)
 		test.That(t, actualLidar.Name, test.ShouldEqual, sensors[0])
 		test.That(t, err, test.ShouldBeNil)
@@ -84,18 +84,18 @@ func TestGetAndSaveData(t *testing.T) {
 		test.That(t, err, test.ShouldBeNil)
 	})
 
-	testhelper.ClearDirectory(t, dataDir)
+	internaltesthelper.ClearDirectory(t, dataDir)
 }
 
 func TestValidateGetAndSaveData(t *testing.T) {
 	ctx := context.Background()
 	logger := golog.NewTestLogger(t)
-	dataDir, err := testhelper.CreateTempFolderArchitecture(logger)
+	dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger)
 	test.That(t, err, test.ShouldBeNil)
 
 	t.Run("Successful call to ValidateGetAndSaveData", func(t *testing.T) {
 		sensors := []string{"good_lidar"}
-		deps := th.SetupDeps(sensors)
+		deps := testhelper.SetupDeps(sensors)
 		actualLidar, err := dim2d.NewLidar(ctx, deps, sensors, logger)
 		test.That(t, actualLidar.Name, test.ShouldEqual, sensors[0])
 		test.That(t, err, test.ShouldBeNil)
@@ -103,8 +103,8 @@ func TestValidateGetAndSaveData(t *testing.T) {
 		err = dim2d.ValidateGetAndSaveData(ctx,
 			dataDir,
 			actualLidar,
-			testhelper.SensorValidationMaxTimeoutSecForTest,
-			testhelper.SensorValidationMaxTimeoutSecForTest,
+			internaltesthelper.SensorValidationMaxTimeoutSecForTest,
+			internaltesthelper.SensorValidationMaxTimeoutSecForTest,
 			logger,
 		)
 		test.That(t, err, test.ShouldBeNil)
@@ -114,5 +114,5 @@ func TestValidateGetAndSaveData(t *testing.T) {
 		test.That(t, err, test.ShouldBeNil)
 	})
 
-	testhelper.ClearDirectory(t, dataDir)
+	internaltesthelper.ClearDirectory(t, dataDir)
 }
diff --git a/internal/testhelper/testhelper.go b/internal/testhelper/testhelper.go
index 4d6196fc..10703abe 100644
--- a/internal/testhelper/testhelper.go
+++ b/internal/testhelper/testhelper.go
@@ -20,7 +20,7 @@ import (
 	viamcartographer "github.com/viamrobotics/viam-cartographer"
 	vcConfig "github.com/viamrobotics/viam-cartographer/config"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
-	th "github.com/viamrobotics/viam-cartographer/testhelper"
+	externaltesthelper "github.com/viamrobotics/viam-cartographer/testhelper"
 )
 
 const (
@@ -70,7 +70,7 @@ func CreateSLAMService(
 	cfgService := resource.Config{Name: "test", API: slam.API, Model: viamcartographer.Model}
 	cfgService.ConvertedAttributes = cfg
 
-	deps := th.SetupDeps(cfg.Sensors)
+	deps := externaltesthelper.SetupDeps(cfg.Sensors)
 
 	sensorDeps, err := cfg.Validate("path")
 	if err != nil {
diff --git a/viam-cartographer_test.go b/viam-cartographer_test.go
index 8e108ff4..cc54503b 100644
--- a/viam-cartographer_test.go
+++ b/viam-cartographer_test.go
@@ -22,9 +22,9 @@ import (
 	"google.golang.org/grpc"
 
 	vcConfig "github.com/viamrobotics/viam-cartographer/config"
-	"github.com/viamrobotics/viam-cartographer/internal/testhelper"
+	internaltesthelper "github.com/viamrobotics/viam-cartographer/internal/testhelper"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
-	th "github.com/viamrobotics/viam-cartographer/testhelper"
+	"github.com/viamrobotics/viam-cartographer/testhelper"
 )
 
 const (
@@ -42,7 +42,7 @@ var (
 
 func TestNew(t *testing.T) {
 	logger := golog.NewTestLogger(t)
-	dataDir, err := testhelper.CreateTempFolderArchitecture(logger)
+	dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger)
 	test.That(t, err, test.ShouldBeNil)
 
 	t.Run("Successful creation of cartographer slam service with no sensor", func(t *testing.T) {
@@ -56,7 +56,7 @@ func TestNew(t *testing.T) {
 			UseLiveData:   &_false,
 		}
 
-		svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 		test.That(t, err, test.ShouldBeNil)
 
 		grpcServer.Stop()
@@ -74,7 +74,7 @@ func TestNew(t *testing.T) {
 			UseLiveData:   &_false,
 		}
 
-		_, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 		test.That(t, err, test.ShouldBeError,
 			errors.New("configuring lidar camera error: 'sensors' must contain only one "+
 				"lidar camera, but is 'sensors: [lidar, one-too-many]'"))
@@ -91,7 +91,7 @@ func TestNew(t *testing.T) {
 			UseLiveData:   &_true,
 		}
 
-		_, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 		test.That(t, err, test.ShouldBeError,
 			errors.New("configuring lidar camera error: error getting lidar camera "+
 				"gibberish for slam service: \"rdk:component:camera/gibberish\" missing from dependencies"))
@@ -108,7 +108,7 @@ func TestNew(t *testing.T) {
 			UseLiveData:   &_true,
 		}
 
-		svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 		test.That(t, err, test.ShouldBeNil)
 
 		grpcServer.Stop()
@@ -125,12 +125,12 @@ func TestNew(t *testing.T) {
 			UseLiveData:   &_true,
 		}
 
-		_, err = testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		_, err = internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 		test.That(t, err, test.ShouldBeError,
 			errors.New("getting and saving data failed: error getting data from sensor: invalid sensor"))
 	})
 
-	testhelper.ClearDirectory(t, dataDir)
+	internaltesthelper.ClearDirectory(t, dataDir)
 
 	t.Run("Successful creation of cartographer slam service in localization mode", func(t *testing.T) {
 		grpcServer, port := setupTestGRPCServer(t)
@@ -144,7 +144,7 @@ func TestNew(t *testing.T) {
 			MapRateSec:    &_zeroInt,
 		}
 
-		svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 		test.That(t, err, test.ShouldBeNil)
 
 		timestamp1, err := svc.GetLatestMapInfo(context.Background())
@@ -171,7 +171,7 @@ func TestNew(t *testing.T) {
 			MapRateSec:    &testMapRateSec,
 		}
 
-		svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 		test.That(t, err, test.ShouldBeNil)
 
 		timestamp1, err := svc.GetLatestMapInfo(context.Background())
@@ -191,7 +191,7 @@ func TestNew(t *testing.T) {
 
 func TestDataProcess(t *testing.T) {
 	logger, obs := golog.NewObservedTestLogger(t)
-	dataDir, err := testhelper.CreateTempFolderArchitecture(logger)
+	dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger)
 	test.That(t, err, test.ShouldBeNil)
 
 	grpcServer, port := setupTestGRPCServer(t)
@@ -204,19 +204,19 @@ func TestDataProcess(t *testing.T) {
 		UseLiveData:   &_true,
 	}
 
-	svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 	test.That(t, err, test.ShouldBeNil)
 
 	grpcServer.Stop()
 	test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
 
-	slamSvc := svc.(testhelper.Service)
+	slamSvc := svc.(internaltesthelper.Service)
 
 	t.Run("Successful startup of data process with good lidar", func(t *testing.T) {
-		defer testhelper.ClearDirectory(t, filepath.Join(dataDir, "data"))
+		defer internaltesthelper.ClearDirectory(t, filepath.Join(dataDir, "data"))
 
 		sensors := []string{"good_lidar"}
-		lidar, err := lidar.New(th.SetupDeps(sensors), sensors, 0)
+		lidar, err := lidar.New(testhelper.SetupDeps(sensors), sensors, 0)
 		test.That(t, err, test.ShouldBeNil)
 
 		cancelCtx, cancelFunc := context.WithCancel(context.Background())
@@ -233,7 +233,7 @@ func TestDataProcess(t *testing.T) {
 	t.Run("Failed startup of data process with invalid sensor "+
 		"that errors during call to NextPointCloud", func(t *testing.T) {
 		sensors := []string{"invalid_sensor"}
-		lidar, err := lidar.New(th.SetupDeps(sensors), sensors, 0)
+		lidar, err := lidar.New(testhelper.SetupDeps(sensors), sensors, 0)
 		test.That(t, err, test.ShouldBeNil)
 
 		cancelCtx, cancelFunc := context.WithCancel(context.Background())
@@ -248,10 +248,10 @@ func TestDataProcess(t *testing.T) {
 	})
 
 	t.Run("When replay sensor is configured, we read timestamps from the context", func(t *testing.T) {
-		defer testhelper.ClearDirectory(t, filepath.Join(dataDir, "data"))
+		defer internaltesthelper.ClearDirectory(t, filepath.Join(dataDir, "data"))
 
 		sensors := []string{"replay_sensor"}
-		lidar, err := lidar.New(th.SetupDeps(sensors), sensors, 0)
+		lidar, err := lidar.New(testhelper.SetupDeps(sensors), sensors, 0)
 		test.That(t, err, test.ShouldBeNil)
 
 		cancelCtx, cancelFunc := context.WithCancel(context.Background())
@@ -262,18 +262,18 @@ func TestDataProcess(t *testing.T) {
 		cancelFunc()
 		files, err := os.ReadDir(filepath.Join(dataDir, "data"))
 		test.That(t, len(files), test.ShouldBeGreaterThanOrEqualTo, 1)
-		test.That(t, files[0].Name(), test.ShouldContainSubstring, th.TestTime)
+		test.That(t, files[0].Name(), test.ShouldContainSubstring, testhelper.TestTime)
 		test.That(t, err, test.ShouldBeNil)
 	})
 
 	test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
 
-	testhelper.ClearDirectory(t, dataDir)
+	internaltesthelper.ClearDirectory(t, dataDir)
 }
 
 func TestEndpointFailures(t *testing.T) {
 	logger := golog.NewTestLogger(t)
-	dataDir, err := testhelper.CreateTempFolderArchitecture(logger)
+	dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger)
 	test.That(t, err, test.ShouldBeNil)
 
 	grpcServer, port := setupTestGRPCServer(t)
@@ -287,7 +287,7 @@ func TestEndpointFailures(t *testing.T) {
 		UseLiveData:   &_true,
 	}
 
-	svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 	test.That(t, err, test.ShouldBeNil)
 
 	pNew, frame, err := svc.GetPosition(context.Background())
@@ -312,12 +312,12 @@ func TestEndpointFailures(t *testing.T) {
 	grpcServer.Stop()
 	test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
 
-	testhelper.ClearDirectory(t, dataDir)
+	internaltesthelper.ClearDirectory(t, dataDir)
 }
 
 func TestSLAMProcess(t *testing.T) {
 	logger := golog.NewTestLogger(t)
-	dataDir, err := testhelper.CreateTempFolderArchitecture(logger)
+	dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger)
 	test.That(t, err, test.ShouldBeNil)
 
 	t.Run("Successful start of live SLAM process with default parameters", func(t *testing.T) {
@@ -330,10 +330,10 @@ func TestSLAMProcess(t *testing.T) {
 			UseLiveData:   &_true,
 		}
 
-		svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 		test.That(t, err, test.ShouldBeNil)
 
-		slamSvc := svc.(testhelper.Service)
+		slamSvc := svc.(internaltesthelper.Service)
 		processCfg := slamSvc.GetSLAMProcessConfig()
 		cmd := append([]string{processCfg.Name}, processCfg.Args...)
 
@@ -370,10 +370,10 @@ func TestSLAMProcess(t *testing.T) {
 			UseLiveData:   &_false,
 		}
 
-		svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 		test.That(t, err, test.ShouldBeNil)
 
-		slamSvc := svc.(testhelper.Service)
+		slamSvc := svc.(internaltesthelper.Service)
 		processCfg := slamSvc.GetSLAMProcessConfig()
 		cmd := append([]string{processCfg.Name}, processCfg.Args...)
 
@@ -411,17 +411,17 @@ func TestSLAMProcess(t *testing.T) {
 			Port:          "localhost:" + strconv.Itoa(port),
 			UseLiveData:   &_true,
 		}
-		_, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, "hokus_pokus_does_not_exist_filename")
+		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, "hokus_pokus_does_not_exist_filename")
 		test.That(t, fmt.Sprint(err), test.ShouldContainSubstring, "executable file not found in $PATH")
 		grpcServer.Stop()
 	})
 
-	testhelper.ClearDirectory(t, dataDir)
+	internaltesthelper.ClearDirectory(t, dataDir)
 }
 
 func TestDoCommand(t *testing.T) {
 	logger := golog.NewTestLogger(t)
-	dataDir, err := testhelper.CreateTempFolderArchitecture(logger)
+	dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger)
 	test.That(t, err, test.ShouldBeNil)
 	grpcServer, port := setupTestGRPCServer(t)
 	attrCfg := &vcConfig.Config{
@@ -433,7 +433,7 @@ func TestDoCommand(t *testing.T) {
 		Port:          "localhost:" + strconv.Itoa(port),
 		UseLiveData:   &_true,
 	}
-	svc, err := testhelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 	test.That(t, err, test.ShouldBeNil)
 	t.Run("returns UnimplementedError when given other parmeters", func(t *testing.T) {
 		cmd := map[string]interface{}{"fake_flag": true}
@@ -449,7 +449,7 @@ func TestDoCommand(t *testing.T) {
 	})
 	grpcServer.Stop()
 	test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
-	testhelper.ClearDirectory(t, dataDir)
+	internaltesthelper.ClearDirectory(t, dataDir)
 }
 
 // SetupTestGRPCServer sets up and starts a grpc server.

From 4b1c2fb1f9678a6a9753e462d0abba4dd9b53f38 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Mon, 10 Jul 2023 13:14:50 -0400
Subject: [PATCH 10/32] wip

---
 internal/dim-2d/dim-2d.go | 50 ++++++++++++++++++++
 viam-cartographer.go      | 97 ++++++++++++++++++++++++++++++++-------
 2 files changed, 131 insertions(+), 16 deletions(-)

diff --git a/internal/dim-2d/dim-2d.go b/internal/dim-2d/dim-2d.go
index 9028c2cf..7e1f23f1 100644
--- a/internal/dim-2d/dim-2d.go
+++ b/internal/dim-2d/dim-2d.go
@@ -11,6 +11,7 @@ import (
 	"github.com/edaniels/golog"
 	"github.com/pkg/errors"
 	"go.opencensus.io/trace"
+	"go.viam.com/rdk/pointcloud"
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/utils/contextutils"
 	goutils "go.viam.com/utils"
@@ -54,6 +55,55 @@ func NewLidar(
 	return lidar, nil
 }
 
+// TODO: Move to 2d.
+func GetData(ctx context.Context, lidar lidar.Lidar) (time.Time, pointcloud.PointCloud, error) {
+	ctx, md := contextutils.ContextWithMetadata(ctx)
+	reqTime := time.Now().UTC()
+	pointcloud, err := lidar.GetData(ctx)
+	if err != nil {
+		return reqTime, nil, err
+	}
+
+	timeRequestedMetadata, ok := md[contextutils.TimeRequestedMetadataKey]
+	if ok {
+		reqTime, err = time.Parse(time.RFC3339Nano, timeRequestedMetadata[0])
+		if err != nil {
+			return reqTime, nil, err
+		}
+	}
+	return reqTime, pointcloud, nil
+}
+
+func ValidateGetData(
+	ctx context.Context,
+	lidar lidar.Lidar,
+	sensorValidationMaxTimeoutSec int,
+	sensorValidationIntervalSec int,
+	logger golog.Logger,
+) error {
+	ctx, span := trace.StartSpan(ctx, "viamcartographer::internal::dim2d::ValidateGetData")
+	defer span.End()
+
+	startTime := time.Now().UTC()
+
+	for {
+		_, _, err := GetData(ctx, lidar)
+		if err == nil {
+			break
+		}
+
+		logger.Debugw("ValidateGetData hit error: ", "error", err)
+		if time.Since(startTime) >= time.Duration(sensorValidationMaxTimeoutSec)*time.Second {
+			return errors.Wrap(err, "ValidateGetData timeout")
+		}
+		if !goutils.SelectContextOrWait(ctx, time.Duration(sensorValidationIntervalSec)*time.Second) {
+			return ctx.Err()
+		}
+	}
+
+	return nil
+}
+
 // ValidateGetAndSaveData makes sure that the provided sensor is actually a lidar and can
 // return pointclouds. It also ensures that saving the data to files works as intended.
 func ValidateGetAndSaveData(
diff --git a/viam-cartographer.go b/viam-cartographer.go
index 15fca908..ed3987f9 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -123,7 +123,7 @@ func TerminateCartoLib() error {
 
 func initSensorProcess(cancelCtx context.Context, cartoSvc *cartographerService) {
 	spConfig := sensorprocess.Config{
-		CartoFacade:      &cartoSvc.cartofacade,
+		CartoFacade:      cartoSvc.cartofacade,
 		Lidar:            cartoSvc.lidar,
 		LidarName:        cartoSvc.primarySensorName,
 		DataRateMs:       cartoSvc.dataRateMs,
@@ -131,7 +131,11 @@ func initSensorProcess(cancelCtx context.Context, cartoSvc *cartographerService)
 		Logger:           cartoSvc.logger,
 		TelemetryEnabled: cartoSvc.logger.Level() == zapcore.DebugLevel,
 	}
-	sensorprocess.Start(cancelCtx, spConfig)
+	cartoSvc.activeBackgroundWorkers.Add(1)
+	go func() {
+		defer cartoSvc.activeBackgroundWorkers.Done()
+		sensorprocess.Start(cancelCtx, spConfig)
+	}()
 }
 
 // New returns a new slam service for the given robot.
@@ -162,6 +166,7 @@ func New(
 	}
 
 	// Set up the data directories
+	// TODO: Figure out how to not do this if v2 is enabled
 	if err := vcConfig.SetupDirectories(svcConfig.DataDirectory, logger); err != nil {
 		return nil, err
 	}
@@ -217,6 +222,7 @@ func New(
 	defer func() {
 		if !success {
 			logger.Errorw("New() hit error, closing...", "error", err)
+			logger.Errorf("cartoSvc %p", cartoSvc)
 			if err := cartoSvc.Close(ctx); err != nil {
 				logger.Errorw("error closing out after error", "error", err)
 			}
@@ -224,14 +230,19 @@ func New(
 	}()
 
 	if modularizationV2Enabled {
-		cartoSvc, err = initCartoFacade(cancelCtx, cartoSvc)
+		if err := dim2d.ValidateGetData(cancelCtx, cartoSvc.lidar, cartoSvc.sensorValidationMaxTimeoutSec, cartoSvc.sensorValidationIntervalSec, cartoSvc.logger); err != nil {
+			err := errors.Wrap(err, "failed to get data from lidar")
+			return nil, err
+		}
+
+		err = initCartoFacade(cancelCtx, cartoSvc)
 		if err != nil {
-			return cartoSvc, err
+			return nil, err
 		}
 		initSensorProcess(cancelCtx, cartoSvc)
 		return cartoSvc, err
 	}
-	cartoSvc, err = initCartoGrpcServer(ctx, cancelCtx, cartoSvc)
+	err = initCartoGrpcServer(ctx, cancelCtx, cartoSvc)
 	if err != nil {
 		return nil, err
 	}
@@ -328,10 +339,14 @@ func parseCartoAlgoConfig(configParams map[string]string, logger golog.Logger) (
 	return cartoAlgoCfg, nil
 }
 
-func initCartoFacade(cancelCtx context.Context, cartoSvc *cartographerService) (*cartographerService, error) {
+// initCartoFacade
+// 1. creates a new initCartoFacade
+// 2. initializes it and starts it
+// 3. terminates it if start fails.
+func initCartoFacade(cancelCtx context.Context, cartoSvc *cartographerService) error {
 	cartoAlgoConfig, err := parseCartoAlgoConfig(cartoSvc.configParams, cartoSvc.logger)
 	if err != nil {
-		return nil, err
+		return err
 	}
 
 	cartoCfg := cartofacade.CartoConfig{
@@ -345,35 +360,67 @@ func initCartoFacade(cancelCtx context.Context, cartoSvc *cartographerService) (
 	cf := cartofacade.New(&cartoLib, cartoCfg, cartoAlgoConfig)
 	err = cf.Initialize(cancelCtx, cartoSvc.cartoFacadeTimeout, &cartoSvc.activeBackgroundWorkers)
 	if err != nil {
-		return nil, err
+		cartoSvc.logger.Errorw("cartofacade initialize failed", "error", err)
+		return err
 	}
-	cartoSvc.cartofacade = cf
+	err = cf.Start(cancelCtx, cartoSvc.cartoFacadeTimeout)
+	if err != nil {
+		cartoSvc.logger.Errorw("cartofacade start failed", "error", err)
+		termErr := cf.Terminate(cancelCtx, cartoSvc.cartoFacadeTimeout)
+		if termErr != nil {
+			cartoSvc.logger.Errorw("cartofacade terminate failed", "error", termErr)
+			return termErr
+		}
+		return err
+	}
+	cartoSvc.cartofacade = &cf
 
-	return cartoSvc, nil
+	return nil
 }
 
-func initCartoGrpcServer(ctx, cancelCtx context.Context, cartoSvc *cartographerService) (*cartographerService, error) {
+// terminateCartoFacade
+// 1. stops & terminates carto facade
+// TODO: How does this work if we are closing down the context which manages the carto facade?
+func terminateCartoFacade(ctx context.Context, cartoSvc *cartographerService) error {
+	if cartoSvc.cartofacade == nil {
+		cartoSvc.logger.Debug("terminateCartoFacade called when cartoSvc.cartofacade is nil")
+		return nil
+	}
+	stopErr := cartoSvc.cartofacade.Stop(ctx, cartoSvc.cartoFacadeTimeout)
+	if stopErr != nil {
+		cartoSvc.logger.Errorw("cartofacade stop failed", "error", stopErr)
+	}
+
+	err := cartoSvc.cartofacade.Terminate(ctx, cartoSvc.cartoFacadeTimeout)
+	if err != nil {
+		cartoSvc.logger.Errorw("cartofacade terminate failed", "error", err)
+		return err
+	}
+	return stopErr
+}
+
+func initCartoGrpcServer(ctx, cancelCtx context.Context, cartoSvc *cartographerService) error {
 	if cartoSvc.primarySensorName != "" {
 		if err := dim2d.ValidateGetAndSaveData(cancelCtx, cartoSvc.dataDirectory, cartoSvc.lidar,
 			cartoSvc.sensorValidationMaxTimeoutSec, cartoSvc.sensorValidationIntervalSec, cartoSvc.logger); err != nil {
-			return cartoSvc, errors.Wrap(err, "getting and saving data failed")
+			return errors.Wrap(err, "getting and saving data failed")
 		}
 		cartoSvc.StartDataProcess(cancelCtx, cartoSvc.lidar, nil)
 		cartoSvc.logger.Debugf("Reading data from sensor: %v", cartoSvc.primarySensorName)
 	}
 
 	if err := cartoSvc.StartSLAMProcess(ctx); err != nil {
-		return cartoSvc, errors.Wrap(err, "error with slam service slam process")
+		return errors.Wrap(err, "error with slam service slam process")
 	}
 
 	client, clientClose, err := vcConfig.SetupGRPCConnection(ctx, cartoSvc.port, cartoSvc.dialMaxTimeoutSec, cartoSvc.logger)
 	if err != nil {
-		return cartoSvc, errors.Wrap(err, "error with initial grpc client to slam algorithm")
+		return errors.Wrap(err, "error with initial grpc client to slam algorithm")
 	}
 	cartoSvc.clientAlgo = client
 	cartoSvc.clientAlgoClose = clientClose
 
-	return cartoSvc, nil
+	return nil
 }
 
 // cartographerService is the structure of the slam service.
@@ -395,7 +442,7 @@ type cartographerService struct {
 	useLiveData         bool
 
 	modularizationV2Enabled bool
-	cartofacade             cartofacade.CartoFacade
+	cartofacade             *cartofacade.CartoFacade
 	cartoFacadeTimeout      time.Duration
 
 	port       string
@@ -555,6 +602,23 @@ 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.logger.Infof("cartoSvc.modularizationV2Enabled: %b", cartoSvc.modularizationV2Enabled)
+	// if v2 not enabled terminate carto facade & clean up
+	// background go routines
+	if cartoSvc.modularizationV2Enabled {
+		// TODO: Make this atomic & idempotent
+		cartoSvc.logger.Info("about to call terminateCartoFacade")
+		err := terminateCartoFacade(ctx, cartoSvc)
+		if err != nil {
+			cartoSvc.logger.Errorw("close hit error", "error", err)
+		}
+		cartoSvc.cancelFunc()
+		cartoSvc.activeBackgroundWorkers.Wait()
+		return nil
+	}
+	cartoSvc.logger.Info("past if branch")
+
+	// if v2 not enabled
 	defer func() {
 		if cartoSvc.clientAlgoClose != nil {
 			goutils.UncheckedErrorFunc(cartoSvc.clientAlgoClose)
@@ -577,6 +641,7 @@ func (cartoSvc *cartographerService) Close(ctx context.Context) error {
 		return errors.Wrap(err, "error occurred during closeout of process")
 	}
 	cartoSvc.activeBackgroundWorkers.Wait()
+	cartoSvc.logger.Info("before close ended")
 	return nil
 }
 

From 9305149c7bf914c9c3ace14034cd1196dbbc828a Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Mon, 10 Jul 2023 15:07:35 -0400
Subject: [PATCH 11/32] wip

---
 {internal => sensors/lidar}/dim-2d/dim-2d.go        | 13 ++++++++++---
 {internal => sensors/lidar}/dim-2d/dim-2d_test.go   |  2 +-
 .../lidar}/dim-2d/sensor-indices.go                 |  0
 {internal => testhelper}/inject/slam_service.go     |  0
 viam-cartographer.go                                |  9 +++++++--
 viam-cartographer_internal_test.go                  |  2 +-
 6 files changed, 19 insertions(+), 7 deletions(-)
 rename {internal => sensors/lidar}/dim-2d/dim-2d.go (89%)
 rename {internal => sensors/lidar}/dim-2d/dim-2d_test.go (98%)
 rename {internal => sensors/lidar}/dim-2d/sensor-indices.go (100%)
 rename {internal => testhelper}/inject/slam_service.go (100%)

diff --git a/internal/dim-2d/dim-2d.go b/sensors/lidar/dim-2d/dim-2d.go
similarity index 89%
rename from internal/dim-2d/dim-2d.go
rename to sensors/lidar/dim-2d/dim-2d.go
index 7e1f23f1..47958961 100644
--- a/internal/dim-2d/dim-2d.go
+++ b/sensors/lidar/dim-2d/dim-2d.go
@@ -55,8 +55,11 @@ func NewLidar(
 	return lidar, nil
 }
 
-// TODO: Move to 2d.
-func GetData(ctx context.Context, lidar lidar.Lidar) (time.Time, pointcloud.PointCloud, error) {
+// GetTimedData returns a 2d lidar reading, the timestamp from when it wask taken
+// (eitehr in live mode or offline mode)
+// and an error if an error occurred getting the lidar data or parsing the offline
+// timestamp.
+func GetTimedData(ctx context.Context, lidar lidar.Lidar) (time.Time, pointcloud.PointCloud, error) {
 	ctx, md := contextutils.ContextWithMetadata(ctx)
 	reqTime := time.Now().UTC()
 	pointcloud, err := lidar.GetData(ctx)
@@ -74,6 +77,10 @@ func GetData(ctx context.Context, lidar lidar.Lidar) (time.Time, pointcloud.Poin
 	return reqTime, pointcloud, nil
 }
 
+// ValidateGetData checks every sensorValidationIntervalSec if the provided lidar
+// returned a valid timed lidar readings every sensorValidationIntervalSec
+// until either success or sensorValidationMaxTimeoutSec has elapsed.
+// returns an error if no valid lidar readings were returned.
 func ValidateGetData(
 	ctx context.Context,
 	lidar lidar.Lidar,
@@ -87,7 +94,7 @@ func ValidateGetData(
 	startTime := time.Now().UTC()
 
 	for {
-		_, _, err := GetData(ctx, lidar)
+		_, _, err := GetTimedData(ctx, lidar)
 		if err == nil {
 			break
 		}
diff --git a/internal/dim-2d/dim-2d_test.go b/sensors/lidar/dim-2d/dim-2d_test.go
similarity index 98%
rename from internal/dim-2d/dim-2d_test.go
rename to sensors/lidar/dim-2d/dim-2d_test.go
index a42a0550..e9c21b79 100644
--- a/internal/dim-2d/dim-2d_test.go
+++ b/sensors/lidar/dim-2d/dim-2d_test.go
@@ -10,9 +10,9 @@ import (
 	"github.com/pkg/errors"
 	"go.viam.com/test"
 
-	dim2d "github.com/viamrobotics/viam-cartographer/internal/dim-2d"
 	internaltesthelper "github.com/viamrobotics/viam-cartographer/internal/testhelper"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
+	dim2d "github.com/viamrobotics/viam-cartographer/sensors/lidar/dim-2d"
 	"github.com/viamrobotics/viam-cartographer/testhelper"
 )
 
diff --git a/internal/dim-2d/sensor-indices.go b/sensors/lidar/dim-2d/sensor-indices.go
similarity index 100%
rename from internal/dim-2d/sensor-indices.go
rename to sensors/lidar/dim-2d/sensor-indices.go
diff --git a/internal/inject/slam_service.go b/testhelper/inject/slam_service.go
similarity index 100%
rename from internal/inject/slam_service.go
rename to testhelper/inject/slam_service.go
diff --git a/viam-cartographer.go b/viam-cartographer.go
index ed3987f9..3e4930c4 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -27,9 +27,9 @@ import (
 
 	"github.com/viamrobotics/viam-cartographer/cartofacade"
 	vcConfig "github.com/viamrobotics/viam-cartographer/config"
-	dim2d "github.com/viamrobotics/viam-cartographer/internal/dim-2d"
 	"github.com/viamrobotics/viam-cartographer/sensorprocess"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
+	dim2d "github.com/viamrobotics/viam-cartographer/sensors/lidar/dim-2d"
 	vcUtils "github.com/viamrobotics/viam-cartographer/utils"
 )
 
@@ -230,7 +230,12 @@ func New(
 	}()
 
 	if modularizationV2Enabled {
-		if err := dim2d.ValidateGetData(cancelCtx, cartoSvc.lidar, cartoSvc.sensorValidationMaxTimeoutSec, cartoSvc.sensorValidationIntervalSec, cartoSvc.logger); err != nil {
+		if err := dim2d.ValidateGetData(
+			cancelCtx,
+			cartoSvc.lidar,
+			cartoSvc.sensorValidationMaxTimeoutSec,
+			cartoSvc.sensorValidationIntervalSec,
+			cartoSvc.logger); err != nil {
 			err := errors.Wrap(err, "failed to get data from lidar")
 			return nil, err
 		}
diff --git a/viam-cartographer_internal_test.go b/viam-cartographer_internal_test.go
index df8c85ae..fac5a1ce 100644
--- a/viam-cartographer_internal_test.go
+++ b/viam-cartographer_internal_test.go
@@ -16,7 +16,7 @@ import (
 	"google.golang.org/grpc"
 	"google.golang.org/protobuf/types/known/structpb"
 
-	inject "github.com/viamrobotics/viam-cartographer/internal/inject"
+	inject "github.com/viamrobotics/viam-cartographer/testhelper/inject"
 )
 
 type pointCloudClientMock struct {

From 3202c73de73c4bbbbe1a32a46da7d280f698a16f Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Mon, 10 Jul 2023 16:31:13 -0400
Subject: [PATCH 12/32] add tests

---
 sensors/lidar/dim-2d/dim-2d.go      |  8 +--
 sensors/lidar/dim-2d/dim-2d_test.go | 98 +++++++++++++++++++++++++++++
 testhelper/testhelper.go            | 24 +++++++
 viam-cartographer.go                |  4 +-
 4 files changed, 128 insertions(+), 6 deletions(-)

diff --git a/sensors/lidar/dim-2d/dim-2d.go b/sensors/lidar/dim-2d/dim-2d.go
index 47958961..96456db2 100644
--- a/sensors/lidar/dim-2d/dim-2d.go
+++ b/sensors/lidar/dim-2d/dim-2d.go
@@ -84,8 +84,8 @@ func GetTimedData(ctx context.Context, lidar lidar.Lidar) (time.Time, pointcloud
 func ValidateGetData(
 	ctx context.Context,
 	lidar lidar.Lidar,
-	sensorValidationMaxTimeoutSec int,
-	sensorValidationIntervalSec int,
+	sensorValidationMaxTimeout time.Duration,
+	sensorValidationInterval time.Duration,
 	logger golog.Logger,
 ) error {
 	ctx, span := trace.StartSpan(ctx, "viamcartographer::internal::dim2d::ValidateGetData")
@@ -100,10 +100,10 @@ func ValidateGetData(
 		}
 
 		logger.Debugw("ValidateGetData hit error: ", "error", err)
-		if time.Since(startTime) >= time.Duration(sensorValidationMaxTimeoutSec)*time.Second {
+		if time.Since(startTime) >= sensorValidationMaxTimeout {
 			return errors.Wrap(err, "ValidateGetData timeout")
 		}
-		if !goutils.SelectContextOrWait(ctx, time.Duration(sensorValidationIntervalSec)*time.Second) {
+		if !goutils.SelectContextOrWait(ctx, sensorValidationInterval) {
 			return ctx.Err()
 		}
 	}
diff --git a/sensors/lidar/dim-2d/dim-2d_test.go b/sensors/lidar/dim-2d/dim-2d_test.go
index e9c21b79..991f26dc 100644
--- a/sensors/lidar/dim-2d/dim-2d_test.go
+++ b/sensors/lidar/dim-2d/dim-2d_test.go
@@ -5,6 +5,7 @@ import (
 	"context"
 	"os"
 	"testing"
+	"time"
 
 	"github.com/edaniels/golog"
 	"github.com/pkg/errors"
@@ -116,3 +117,100 @@ func TestValidateGetAndSaveData(t *testing.T) {
 
 	internaltesthelper.ClearDirectory(t, dataDir)
 }
+
+func TestGetTimedData(t *testing.T) {
+	logger := golog.NewTestLogger(t)
+	ctx := context.Background()
+
+	sensors := []string{"invalid_sensor"}
+	invalidLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger)
+	test.That(t, err, test.ShouldBeNil)
+
+	sensors = []string{"invalid_replay_sensor"}
+	invalidReplayLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger)
+	test.That(t, err, test.ShouldBeNil)
+
+	sensors = []string{"good_lidar"}
+	goodLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger)
+	test.That(t, err, test.ShouldBeNil)
+
+	sensors = []string{"replay_sensor"}
+	goodReplayLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger)
+	test.That(t, err, test.ShouldBeNil)
+
+	t.Run("when the lidar returns an error, returns that error", func(t *testing.T) {
+		_, pc, err := dim2d.GetTimedData(ctx, invalidLidar)
+		test.That(t, err, test.ShouldBeError, errors.New("invalid sensor"))
+		test.That(t, pc, test.ShouldBeNil)
+	})
+
+	t.Run("when the replay lidar succeeds but the timestamp is invalid, returns an error", func(t *testing.T) {
+		_, pc, err := dim2d.GetTimedData(ctx, invalidReplayLidar)
+		msg := "parsing time \"NOT A TIME\" as \"2006-01-02T15:04:05.999999999Z07:00\": cannot parse \"NOT A TIME\" as \"2006\""
+		test.That(t, err, test.ShouldBeError, errors.New(msg))
+		test.That(t, pc, test.ShouldBeNil)
+	})
+
+	t.Run("when a live lidar succeeds, returns current time in UTC and the reading", func(t *testing.T) {
+		beforeReading := time.Now().UTC()
+		pcTime, pc, err := dim2d.GetTimedData(ctx, goodLidar)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, pc, test.ShouldNotBeNil)
+		test.That(t, pcTime.After(beforeReading), test.ShouldBeTrue)
+		test.That(t, pcTime.Location(), test.ShouldEqual, time.UTC)
+	})
+
+	t.Run("when a replay lidar succeeds, returns the replay sensor time and the reading", func(t *testing.T) {
+		pcTime, pc, err := dim2d.GetTimedData(ctx, goodReplayLidar)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, pc, test.ShouldNotBeNil)
+		test.That(t, pcTime, test.ShouldEqual, time.Date(2006, 1, 2, 15, 4, 5, 999900000, time.UTC))
+	})
+}
+
+func TestValidateGetData(t *testing.T) {
+	logger := golog.NewTestLogger(t)
+	ctx := context.Background()
+
+	sensors := []string{"good_lidar"}
+	goodLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger)
+	test.That(t, err, test.ShouldBeNil)
+
+	sensors = []string{"invalid_sensor"}
+	invalidLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger)
+	test.That(t, err, test.ShouldBeNil)
+
+	sensorValidationMaxTimeout := time.Duration(50) * time.Millisecond
+	sensorValidationInterval := time.Duration(10) * time.Millisecond
+
+	t.Run("returns nil if a lidar reading succeeds immediately", func(t *testing.T) {
+		err := dim2d.ValidateGetData(ctx, goodLidar, sensorValidationMaxTimeout, sensorValidationInterval, logger)
+		test.That(t, err, test.ShouldBeNil)
+	})
+
+	t.Run("returns nil if a lidar reading succeeds within the timeout", func(t *testing.T) {
+		sensors = []string{"warming_up_lidar"}
+		warmingUpLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger)
+		test.That(t, err, test.ShouldBeNil)
+
+		err = dim2d.ValidateGetData(ctx, warmingUpLidar, sensorValidationMaxTimeout, sensorValidationInterval, logger)
+		test.That(t, err, test.ShouldBeNil)
+	})
+
+	t.Run("returns error if no lidar reading succeeds within the timeout", func(t *testing.T) {
+		err := dim2d.ValidateGetData(ctx, invalidLidar, sensorValidationMaxTimeout, sensorValidationInterval, logger)
+		test.That(t, err, test.ShouldBeError, errors.New("ValidateGetData timeout: invalid sensor"))
+	})
+
+	t.Run("returns error if no lidar reading succeeds by the time the context is cancelled", func(t *testing.T) {
+		cancelledCtx, cancelFunc := context.WithCancel(context.Background())
+		cancelFunc()
+
+		sensors = []string{"warming_up_lidar"}
+		warmingUpLidar, err := dim2d.NewLidar(ctx, testhelper.SetupDeps(sensors), sensors, logger)
+		test.That(t, err, test.ShouldBeNil)
+
+		err = dim2d.ValidateGetData(cancelledCtx, warmingUpLidar, sensorValidationMaxTimeout, sensorValidationInterval, logger)
+		test.That(t, err, test.ShouldBeError, context.Canceled)
+	})
+}
diff --git a/testhelper/testhelper.go b/testhelper/testhelper.go
index 8c39aeea..7e790d99 100644
--- a/testhelper/testhelper.go
+++ b/testhelper/testhelper.go
@@ -40,6 +40,8 @@ func SetupDeps(sensors []string) resource.Dependencies {
 		switch sensor {
 		case "good_lidar":
 			deps[camera.Named(sensor)] = getGoodLidar()
+		case "warming_up_lidar":
+			deps[camera.Named(sensor)] = getWarmingUpLidar()
 		case "replay_sensor":
 			deps[camera.Named(sensor)] = getReplaySensor(TestTime)
 		case "invalid_replay_sensor":
@@ -57,6 +59,28 @@ func SetupDeps(sensors []string) resource.Dependencies {
 	return deps
 }
 
+func getWarmingUpLidar() *inject.Camera {
+	cam := &inject.Camera{}
+	couter := 0
+	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
+		couter++
+		if couter == 1 {
+			return nil, errors.Errorf("warming up %d", couter)
+		}
+		return pointcloud.New(), nil
+	}
+	cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) {
+		return nil, errors.New("lidar not camera")
+	}
+	cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) {
+		return nil, transform.NewNoIntrinsicsError("")
+	}
+	cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) {
+		return camera.Properties{}, nil
+	}
+	return cam
+}
+
 func getGoodLidar() *inject.Camera {
 	cam := &inject.Camera{}
 	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
diff --git a/viam-cartographer.go b/viam-cartographer.go
index 3e4930c4..08cdaeb8 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -233,8 +233,8 @@ func New(
 		if err := dim2d.ValidateGetData(
 			cancelCtx,
 			cartoSvc.lidar,
-			cartoSvc.sensorValidationMaxTimeoutSec,
-			cartoSvc.sensorValidationIntervalSec,
+			time.Duration(sensorValidationMaxTimeoutSec)*time.Second,
+			time.Duration(cartoSvc.sensorValidationIntervalSec)*time.Second,
 			cartoSvc.logger); err != nil {
 			err := errors.Wrap(err, "failed to get data from lidar")
 			return nil, err

From e95fa5ff79799c900bd45fdb3dac0b9765e9cd22 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Mon, 10 Jul 2023 16:48:53 -0400
Subject: [PATCH 13/32] more tests

---
 viam-cartographer.go               |  2 -
 viam-cartographer_internal_test.go | 87 ++++++++++++++++++++++++++++++
 2 files changed, 87 insertions(+), 2 deletions(-)

diff --git a/viam-cartographer.go b/viam-cartographer.go
index 08cdaeb8..e043afac 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -222,7 +222,6 @@ func New(
 	defer func() {
 		if !success {
 			logger.Errorw("New() hit error, closing...", "error", err)
-			logger.Errorf("cartoSvc %p", cartoSvc)
 			if err := cartoSvc.Close(ctx); err != nil {
 				logger.Errorw("error closing out after error", "error", err)
 			}
@@ -258,7 +257,6 @@ func New(
 // TODO: write a test for this.
 func parseCartoAlgoConfig(configParams map[string]string, logger golog.Logger) (cartofacade.CartoAlgoConfig, error) {
 	cartoAlgoCfg := defaultCartoAlgoCfg
-	logger.Warnf("NICK: defaultCartoAlgoCfgPtr: %p, cartoAlgoCfgPtr: %p", &defaultCartoAlgoCfg, &cartoAlgoCfg)
 	for k, val := range configParams {
 		switch k {
 		case "optimize_on_start":
diff --git a/viam-cartographer_internal_test.go b/viam-cartographer_internal_test.go
index fac5a1ce..8ef8d9b8 100644
--- a/viam-cartographer_internal_test.go
+++ b/viam-cartographer_internal_test.go
@@ -5,6 +5,7 @@ import (
 	"context"
 	"testing"
 
+	"github.com/edaniels/golog"
 	"github.com/golang/geo/r3"
 	"github.com/pkg/errors"
 	commonv1 "go.viam.com/api/common/v1"
@@ -16,6 +17,7 @@ import (
 	"google.golang.org/grpc"
 	"google.golang.org/protobuf/types/known/structpb"
 
+	"github.com/viamrobotics/viam-cartographer/cartofacade"
 	inject "github.com/viamrobotics/viam-cartographer/testhelper/inject"
 )
 
@@ -397,3 +399,88 @@ func TestGetInternalStateEndpoint(t *testing.T) {
 		})
 	})
 }
+
+func TestParseCartoAlgoConfig(t *testing.T) {
+	logger := golog.NewTestLogger(t)
+
+	t.Run("returns default when config params are empty", func(t *testing.T) {
+		defaultCartoAlgoCfg := cartofacade.CartoAlgoConfig{
+			OptimizeOnStart:      false,
+			OptimizeEveryNNodes:  3,
+			NumRangeData:         30,
+			MissingDataRayLength: 25.0,
+			MaxRange:             25.0,
+			MinRange:             0.2,
+			MaxSubmapsToKeep:     3,
+			FreshSubmapsCount:    3,
+			MinCoveredArea:       1.0,
+			MinAddedSubmapsCount: 1,
+			OccupiedSpaceWeight:  20.0,
+			TranslationWeight:    10.0,
+			RotationWeight:       1.0,
+		}
+
+		configParams := map[string]string{}
+		cartoAlgoConfig, err := parseCartoAlgoConfig(configParams, logger)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, cartoAlgoConfig, test.ShouldResemble, defaultCartoAlgoCfg)
+	})
+
+	t.Run("returns overrides when config is non empty", func(t *testing.T) {
+		configParams := map[string]string{
+			"optimize_on_start":       "true",
+			"optimize_every_n_nodes":  "1",
+			"num_range_data":          "2",
+			"missing_data_ray_length": "3.0",
+			"max_range":               "4.0",
+			"min_range":               "5.0",
+			"max_submaps_to_keep":     "6",
+			"fresh_submaps_count":     "7",
+			"min_covered_area":        "8.0",
+			"min_added_submaps_count": "9",
+			"occupied_space_weight":   "10.0",
+			"translation_weight":      "11.0",
+			"rotation_weight":         "12.0",
+		}
+
+		overRidenCartoAlgoCfg := cartofacade.CartoAlgoConfig{
+			OptimizeOnStart:      true,
+			OptimizeEveryNNodes:  1,
+			NumRangeData:         2,
+			MissingDataRayLength: 3.0,
+			MaxRange:             4.0,
+			MinRange:             5.0,
+			MaxSubmapsToKeep:     6,
+			FreshSubmapsCount:    7,
+			MinCoveredArea:       8.0,
+			MinAddedSubmapsCount: 9,
+			OccupiedSpaceWeight:  10.0,
+			TranslationWeight:    11.0,
+			RotationWeight:       12.0,
+		}
+
+		cartoAlgoConfig, err := parseCartoAlgoConfig(configParams, logger)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, cartoAlgoConfig, test.ShouldResemble, overRidenCartoAlgoCfg)
+	})
+
+	t.Run("returns error when unsupported param provided", func(t *testing.T) {
+		configParams := map[string]string{
+			"optimize_on_start": "true",
+			"invalid_param":     "hihi",
+		}
+
+		_, err := parseCartoAlgoConfig(configParams, logger)
+		test.That(t, err, test.ShouldBeNil)
+	})
+
+	t.Run("returns error when param type is invalid", func(t *testing.T) {
+		configParams := map[string]string{
+			"optimize_on_start": "hi",
+		}
+
+		cartoAlgoConfig, err := parseCartoAlgoConfig(configParams, logger)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, cartoAlgoConfig, test.ShouldResemble, defaultCartoAlgoCfg)
+	})
+}

From 0f2f56e9df9b7f97861261f80be4131e2077324f Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Mon, 10 Jul 2023 16:51:33 -0400
Subject: [PATCH 14/32] more tests

---
 viam-cartographer_internal_test.go | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/viam-cartographer_internal_test.go b/viam-cartographer_internal_test.go
index 8ef8d9b8..f596549c 100644
--- a/viam-cartographer_internal_test.go
+++ b/viam-cartographer_internal_test.go
@@ -476,11 +476,11 @@ func TestParseCartoAlgoConfig(t *testing.T) {
 
 	t.Run("returns error when param type is invalid", func(t *testing.T) {
 		configParams := map[string]string{
-			"optimize_on_start": "hi",
+			"optimize_every_n_nodes": "hihi",
 		}
 
 		cartoAlgoConfig, err := parseCartoAlgoConfig(configParams, logger)
-		test.That(t, err, test.ShouldBeNil)
+		test.That(t, err, test.ShouldBeError, errors.New("strconv.Atoi: parsing \"hihi\": invalid syntax"))
 		test.That(t, cartoAlgoConfig, test.ShouldResemble, defaultCartoAlgoCfg)
 	})
 }

From 5b09e4697f0cbb90c17b2329c1e3a6665cab96b4 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Mon, 10 Jul 2023 16:53:52 -0400
Subject: [PATCH 15/32] wip

---
 viam-cartographer.go | 7 -------
 1 file changed, 7 deletions(-)

diff --git a/viam-cartographer.go b/viam-cartographer.go
index e043afac..6b5c7cdd 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -605,12 +605,8 @@ 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.logger.Infof("cartoSvc.modularizationV2Enabled: %b", cartoSvc.modularizationV2Enabled)
-	// if v2 not enabled terminate carto facade & clean up
-	// background go routines
 	if cartoSvc.modularizationV2Enabled {
 		// TODO: Make this atomic & idempotent
-		cartoSvc.logger.Info("about to call terminateCartoFacade")
 		err := terminateCartoFacade(ctx, cartoSvc)
 		if err != nil {
 			cartoSvc.logger.Errorw("close hit error", "error", err)
@@ -619,9 +615,7 @@ func (cartoSvc *cartographerService) Close(ctx context.Context) error {
 		cartoSvc.activeBackgroundWorkers.Wait()
 		return nil
 	}
-	cartoSvc.logger.Info("past if branch")
 
-	// if v2 not enabled
 	defer func() {
 		if cartoSvc.clientAlgoClose != nil {
 			goutils.UncheckedErrorFunc(cartoSvc.clientAlgoClose)
@@ -644,7 +638,6 @@ func (cartoSvc *cartographerService) Close(ctx context.Context) error {
 		return errors.Wrap(err, "error occurred during closeout of process")
 	}
 	cartoSvc.activeBackgroundWorkers.Wait()
-	cartoSvc.logger.Info("before close ended")
 	return nil
 }
 

From 803d2dd5a2f3058579b7852220ec527ae0f70a15 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Mon, 10 Jul 2023 17:57:36 -0400
Subject: [PATCH 16/32] wip

---
 internal/testhelper/testhelper.go  |   2 +-
 viam-cartographer.go               |  14 +-
 viam-cartographer_internal_test.go |   1 +
 viam-cartographer_test.go          | 416 ++++++++++++++++++++---------
 4 files changed, 301 insertions(+), 132 deletions(-)

diff --git a/internal/testhelper/testhelper.go b/internal/testhelper/testhelper.go
index 10703abe..31c0413a 100644
--- a/internal/testhelper/testhelper.go
+++ b/internal/testhelper/testhelper.go
@@ -134,7 +134,7 @@ const (
 func CreateTempFolderArchitecture(logger golog.Logger) (string, error) {
 	tmpDir, err := os.MkdirTemp("", "*")
 	if err != nil {
-		return "nil", err
+		return "", err
 	}
 	if err := vcConfig.SetupDirectories(tmpDir, logger); err != nil {
 		return "", err
diff --git a/viam-cartographer.go b/viam-cartographer.go
index 6b5c7cdd..418fb23c 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -167,9 +167,6 @@ func New(
 
 	// Set up the data directories
 	// TODO: Figure out how to not do this if v2 is enabled
-	if err := vcConfig.SetupDirectories(svcConfig.DataDirectory, logger); err != nil {
-		return nil, err
-	}
 
 	port, dataRateMsec, mapRateSec, useLiveData, deleteProcessedData, modularizationV2Enabled, err := vcConfig.GetOptionalParameters(
 		svcConfig,
@@ -178,10 +175,17 @@ func New(
 		defaultMapRateSec,
 		logger,
 	)
+
 	if err != nil {
 		return nil, err
 	}
 
+	if !modularizationV2Enabled {
+		if err := vcConfig.SetupDirectories(svcConfig.DataDirectory, logger); err != nil {
+			return nil, err
+		}
+	}
+
 	// Get the lidar for the Dim2D cartographer sub algorithm
 	lidar, err := dim2d.NewLidar(ctx, deps, svcConfig.Sensors, logger)
 	if err != nil {
@@ -335,8 +339,10 @@ func parseCartoAlgoConfig(configParams map[string]string, logger golog.Logger) (
 				return cartoAlgoCfg, err
 			}
 			cartoAlgoCfg.RotationWeight = fVal
+			// ignore mode as it is a special case
+		case "mode":
 		default:
-			logger.Warn("unused config param: %s: %s", k, val)
+			logger.Warnf("unused config param: %s: %s", k, val)
 		}
 	}
 	return cartoAlgoCfg, nil
diff --git a/viam-cartographer_internal_test.go b/viam-cartographer_internal_test.go
index f596549c..489f06bc 100644
--- a/viam-cartographer_internal_test.go
+++ b/viam-cartographer_internal_test.go
@@ -484,3 +484,4 @@ func TestParseCartoAlgoConfig(t *testing.T) {
 		test.That(t, cartoAlgoConfig, test.ShouldResemble, defaultCartoAlgoCfg)
 	})
 }
+
diff --git a/viam-cartographer_test.go b/viam-cartographer_test.go
index cc54503b..4ff33cfb 100644
--- a/viam-cartographer_test.go
+++ b/viam-cartographer_test.go
@@ -16,15 +16,18 @@ import (
 	"time"
 
 	"github.com/edaniels/golog"
-	"github.com/pkg/errors"
+	// "github.com/pkg/errors"
 	viamgrpc "go.viam.com/rdk/grpc"
 	"go.viam.com/test"
 	"google.golang.org/grpc"
 
+	viamcartographer "github.com/viamrobotics/viam-cartographer"
 	vcConfig "github.com/viamrobotics/viam-cartographer/config"
+	"github.com/viamrobotics/viam-cartographer/dataprocess"
 	internaltesthelper "github.com/viamrobotics/viam-cartographer/internal/testhelper"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
 	"github.com/viamrobotics/viam-cartographer/testhelper"
+	"go.viam.com/utils/artifact"
 )
 
 const (
@@ -42,151 +45,310 @@ var (
 
 func TestNew(t *testing.T) {
 	logger := golog.NewTestLogger(t)
-	dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger)
-	test.That(t, err, test.ShouldBeNil)
-
-	t.Run("Successful creation of cartographer slam service with no sensor", func(t *testing.T) {
-		grpcServer, port := setupTestGRPCServer(t)
-		test.That(t, err, test.ShouldBeNil)
-		attrCfg := &vcConfig.Config{
-			Sensors:       []string{},
-			ConfigParams:  map[string]string{"mode": "2d"},
-			DataDirectory: dataDir,
-			Port:          "localhost:" + strconv.Itoa(port),
-			UseLiveData:   &_false,
-		}
-
-		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger)
+	// test.That(t, err, test.ShouldBeNil)
+
+	// 	t.Run("Successful creation of cartographer slam service with no sensor", func(t *testing.T) {
+	// 		grpcServer, port := setupTestGRPCServer(t)
+	// 		test.That(t, err, test.ShouldBeNil)
+	// 		attrCfg := &vcConfig.Config{
+	// 			Sensors:       []string{},
+	// 			ConfigParams:  map[string]string{"mode": "2d"},
+	// 			DataDirectory: dataDir,
+	// 			Port:          "localhost:" + strconv.Itoa(port),
+	// 			UseLiveData:   &_false,
+	// 		}
+
+	// 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// 		test.That(t, err, test.ShouldBeNil)
+
+	// 		grpcServer.Stop()
+	// 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
+	// 	})
+
+	// 	t.Run("Failed creation of cartographer slam service with more than one sensor", func(t *testing.T) {
+	// 		grpcServer, port := setupTestGRPCServer(t)
+	// 		test.That(t, err, test.ShouldBeNil)
+	// 		attrCfg := &vcConfig.Config{
+	// 			Sensors:       []string{"lidar", "one-too-many"},
+	// 			ConfigParams:  map[string]string{"mode": "2d"},
+	// 			DataDirectory: dataDir,
+	// 			Port:          "localhost:" + strconv.Itoa(port),
+	// 			UseLiveData:   &_false,
+	// 		}
+
+	// 		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// 		test.That(t, err, test.ShouldBeError,
+	// 			errors.New("configuring lidar camera error: 'sensors' must contain only one "+
+	// 				"lidar camera, but is 'sensors: [lidar, one-too-many]'"))
+
+	// 		grpcServer.Stop()
+	// 	})
+
+	// 	t.Run("Failed creation of cartographer slam service with non-existing sensor", func(t *testing.T) {
+	// 		attrCfg := &vcConfig.Config{
+	// 			Sensors:       []string{"gibberish"},
+	// 			ConfigParams:  map[string]string{"mode": "2d"},
+	// 			DataDirectory: dataDir,
+	// 			DataRateMsec:  testDataRateMsec,
+	// 			UseLiveData:   &_true,
+	// 		}
+
+	// 		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// 		test.That(t, err, test.ShouldBeError,
+	// 			errors.New("configuring lidar camera error: error getting lidar camera "+
+	// 				"gibberish for slam service: \"rdk:component:camera/gibberish\" missing from dependencies"))
+	// 	})
+
+	// 	t.Run("Successful creation of cartographer slam service with good lidar", func(t *testing.T) {
+	// 		grpcServer, port := setupTestGRPCServer(t)
+	// 		attrCfg := &vcConfig.Config{
+	// 			Sensors:       []string{"good_lidar"},
+	// 			ConfigParams:  map[string]string{"mode": "2d"},
+	// 			DataDirectory: dataDir,
+	// 			DataRateMsec:  testDataRateMsec,
+	// 			Port:          "localhost:" + strconv.Itoa(port),
+	// 			UseLiveData:   &_true,
+	// 		}
+
+	// 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// 		test.That(t, err, test.ShouldBeNil)
+
+	// 		grpcServer.Stop()
+	// 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
+	// 	})
+
+	// 	t.Run("Failed creation of cartographer slam service with invalid sensor "+
+	// 		"that errors during call to NextPointCloud", func(t *testing.T) {
+	// 		attrCfg := &vcConfig.Config{
+	// 			Sensors:       []string{"invalid_sensor"},
+	// 			ConfigParams:  map[string]string{"mode": "2d"},
+	// 			DataDirectory: dataDir,
+	// 			DataRateMsec:  testDataRateMsec,
+	// 			UseLiveData:   &_true,
+	// 		}
+
+	// 		_, err = internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// 		test.That(t, err, test.ShouldBeError,
+	// 			errors.New("getting and saving data failed: error getting data from sensor: invalid sensor"))
+	// 	})
+
+	// 	internaltesthelper.ClearDirectory(t, dataDir)
+
+	// 	t.Run("Successful creation of cartographer slam service in localization mode", func(t *testing.T) {
+	// 		grpcServer, port := setupTestGRPCServer(t)
+
+	// 		attrCfg := &vcConfig.Config{
+	// 			Sensors:       []string{},
+	// 			ConfigParams:  map[string]string{"mode": "2d"},
+	// 			DataDirectory: dataDir,
+	// 			Port:          "localhost:" + strconv.Itoa(port),
+	// 			UseLiveData:   &_false,
+	// 			MapRateSec:    &_zeroInt,
+	// 		}
+
+	// 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// 		test.That(t, err, test.ShouldBeNil)
+
+	// 		timestamp1, err := svc.GetLatestMapInfo(context.Background())
+	// 		test.That(t, err, test.ShouldBeNil)
+	// 		_, err = svc.GetPointCloudMap(context.Background())
+	// 		test.That(t, err, test.ShouldBeNil)
+	// 		timestamp2, err := svc.GetLatestMapInfo(context.Background())
+	// 		test.That(t, err, test.ShouldBeNil)
+	// 		test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
+	// 		test.That(t, timestamp1, test.ShouldResemble, timestamp2)
+
+	// 		grpcServer.Stop()
+	// 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
+	// 	})
+
+	// 	t.Run("Successful creation of cartographer slam service in non localization mode", func(t *testing.T) {
+	// 		grpcServer, port := setupTestGRPCServer(t)
+	// 		attrCfg := &vcConfig.Config{
+	// 			Sensors:       []string{},
+	// 			ConfigParams:  map[string]string{"mode": "2d"},
+	// 			DataDirectory: dataDir,
+	// 			Port:          "localhost:" + strconv.Itoa(port),
+	// 			UseLiveData:   &_false,
+	// 			MapRateSec:    &testMapRateSec,
+	// 		}
+
+	// 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// 		test.That(t, err, test.ShouldBeNil)
+
+	// 		timestamp1, err := svc.GetLatestMapInfo(context.Background())
+	// 		test.That(t, err, test.ShouldBeNil)
+	// 		_, err = svc.GetPointCloudMap(context.Background())
+	// 		test.That(t, err, test.ShouldBeNil)
+	// 		timestamp2, err := svc.GetLatestMapInfo(context.Background())
+	// 		test.That(t, err, test.ShouldBeNil)
+
+	// 		test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
+	// 		test.That(t, timestamp2.After(timestamp1), test.ShouldBeTrue)
+
+	// 		grpcServer.Stop()
+	// 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
+	// 	})
+
+	// 	t.Run("Successful creation of cartographer slam service with no sensor when feature flag enabled", func(t *testing.T) {
+	// 		attrCfg := &vcConfig.Config{
+	// 			ModularizationV2Enabled: &_true,
+	// 			Sensors:                 []string{},
+	// 			ConfigParams:            map[string]string{"mode": "2d"},
+	// 			DataDirectory:           dataDir,
+	// 			UseLiveData:             &_false,
+	// 		}
+
+	// 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// 		test.That(t, err, test.ShouldBeNil)
+	// 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
+	// 	})
+
+	// 	t.Run("Failed creation of cartographer slam service with more than one sensor when feature flag enabled", func(t *testing.T) {
+	// 		attrCfg := &vcConfig.Config{
+	// 			ModularizationV2Enabled: &_true,
+	// 			Sensors:       []string{"lidar", "one-too-many"},
+	// 			ConfigParams:  map[string]string{"mode": "2d"},
+	// 			DataDirectory: dataDir,
+	// 			UseLiveData:   &_false,
+	// 		}
+
+	// 		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// 		test.That(t, err, test.ShouldBeError,
+	// 			errors.New("configuring lidar camera error: 'sensors' must contain only one "+
+	// 				"lidar camera, but is 'sensors: [lidar, one-too-many]'"))
+	// 	})
+
+	// 	t.Run("Failed creation of cartographer slam service with non-existing sensor when feature flag enabled", func(t *testing.T) {
+	// 		attrCfg := &vcConfig.Config{
+	// 			ModularizationV2Enabled: &_true,
+	// 			Sensors:       []string{"gibberish"},
+	// 			ConfigParams:  map[string]string{"mode": "2d"},
+	// 			DataDirectory: dataDir,
+	// 			DataRateMsec:  testDataRateMsec,
+	// 			UseLiveData:   &_true,
+	// 		}
+
+	// 		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// 		test.That(t, err, test.ShouldBeError,
+	// 			errors.New("configuring lidar camera error: error getting lidar camera "+
+	// 				"gibberish for slam service: \"rdk:component:camera/gibberish\" missing from dependencies"))
+	// 	})
+
+	// 	t.Run("Successful creation of cartographer slam service with good lidar when feature flag enabled", func(t *testing.T) {
+	// 		attrCfg := &vcConfig.Config{
+	// 			ModularizationV2Enabled: &_true,
+	// 			Sensors:       []string{"good_lidar"},
+	// 			ConfigParams:  map[string]string{"mode": "2d"},
+	// 			DataDirectory: dataDir,
+	// 			DataRateMsec:  testDataRateMsec,
+	// 			UseLiveData:   &_true,
+	// 		}
+
+	// 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// 		test.That(t, err, test.ShouldBeNil)
+
+	// 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
+	// 	})
+
+	// 	t.Run("Failed creation of cartographer slam service with invalid sensor "+
+	// 		"that errors during call to NextPointCloud when feature flag enabled", func(t *testing.T) {
+	// 		attrCfg := &vcConfig.Config{
+	// 			ModularizationV2Enabled: &_true,
+	// 			Sensors:       []string{"invalid_sensor"},
+	// 			ConfigParams:  map[string]string{"mode": "2d"},
+	// 			DataDirectory: dataDir,
+	// 			DataRateMsec:  testDataRateMsec,
+	// 			UseLiveData:   &_true,
+	// 		}
+
+	// 		_, err = internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// 		test.That(t, err, test.ShouldBeError,
+	// 			errors.New("failed to get data from lidar: ValidateGetData timeout: invalid sensor"))
+	// 	})
+
+	t.Run("Successful creation of cartographer slam service in localization mode when feature flag enabled", func(t *testing.T) {
+		err := viamcartographer.InitCartoLib(logger)
 		test.That(t, err, test.ShouldBeNil)
+		defer func() {
+			err = viamcartographer.TerminateCartoLib()
+			test.That(t, err, test.ShouldBeNil)
+		}()
 
-		grpcServer.Stop()
-		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
-	})
-
-	t.Run("Failed creation of cartographer slam service with more than one sensor", func(t *testing.T) {
-		grpcServer, port := setupTestGRPCServer(t)
+		dataDirectory, err := os.MkdirTemp("", "*")
 		test.That(t, err, test.ShouldBeNil)
-		attrCfg := &vcConfig.Config{
-			Sensors:       []string{"lidar", "one-too-many"},
-			ConfigParams:  map[string]string{"mode": "2d"},
-			DataDirectory: dataDir,
-			Port:          "localhost:" + strconv.Itoa(port),
-			UseLiveData:   &_false,
-		}
-
-		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-		test.That(t, err, test.ShouldBeError,
-			errors.New("configuring lidar camera error: 'sensors' must contain only one "+
-				"lidar camera, but is 'sensors: [lidar, one-too-many]'"))
-
-		grpcServer.Stop()
-	})
-
-	t.Run("Failed creation of cartographer slam service with non-existing sensor", func(t *testing.T) {
-		attrCfg := &vcConfig.Config{
-			Sensors:       []string{"gibberish"},
-			ConfigParams:  map[string]string{"mode": "2d"},
-			DataDirectory: dataDir,
-			DataRateMsec:  testDataRateMsec,
-			UseLiveData:   &_true,
-		}
-
-		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-		test.That(t, err, test.ShouldBeError,
-			errors.New("configuring lidar camera error: error getting lidar camera "+
-				"gibberish for slam service: \"rdk:component:camera/gibberish\" missing from dependencies"))
-	})
-
-	t.Run("Successful creation of cartographer slam service with good lidar", func(t *testing.T) {
-		grpcServer, port := setupTestGRPCServer(t)
-		attrCfg := &vcConfig.Config{
-			Sensors:       []string{"good_lidar"},
-			ConfigParams:  map[string]string{"mode": "2d"},
-			DataDirectory: dataDir,
-			DataRateMsec:  testDataRateMsec,
-			Port:          "localhost:" + strconv.Itoa(port),
-			UseLiveData:   &_true,
-		}
 
-		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		internalStateDir := filepath.Join(dataDirectory, "internal_state")
+		err = os.Mkdir(internalStateDir, os.ModePerm)
 		test.That(t, err, test.ShouldBeNil)
 
-		grpcServer.Stop()
-		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
-	})
-
-	t.Run("Failed creation of cartographer slam service with invalid sensor "+
-		"that errors during call to NextPointCloud", func(t *testing.T) {
-		attrCfg := &vcConfig.Config{
-			Sensors:       []string{"invalid_sensor"},
-			ConfigParams:  map[string]string{"mode": "2d"},
-			DataDirectory: dataDir,
-			DataRateMsec:  testDataRateMsec,
-			UseLiveData:   &_true,
-		}
-
-		_, err = internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-		test.That(t, err, test.ShouldBeError,
-			errors.New("getting and saving data failed: error getting data from sensor: invalid sensor"))
-	})
-
-	internaltesthelper.ClearDirectory(t, dataDir)
-
-	t.Run("Successful creation of cartographer slam service in localization mode", func(t *testing.T) {
-		grpcServer, port := setupTestGRPCServer(t)
-
-		attrCfg := &vcConfig.Config{
-			Sensors:       []string{},
-			ConfigParams:  map[string]string{"mode": "2d"},
-			DataDirectory: dataDir,
-			Port:          "localhost:" + strconv.Itoa(port),
-			UseLiveData:   &_false,
-			MapRateSec:    &_zeroInt,
-		}
-
-		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		internalState, err := os.ReadFile(artifact.MustPath("viam-cartographer/outputs/viam-office-02-22-3/internal_state/internal_state_0.pbstream"))
 		test.That(t, err, test.ShouldBeNil)
 
-		timestamp1, err := svc.GetLatestMapInfo(context.Background())
-		test.That(t, err, test.ShouldBeNil)
-		_, err = svc.GetPointCloudMap(context.Background())
-		test.That(t, err, test.ShouldBeNil)
-		timestamp2, err := svc.GetLatestMapInfo(context.Background())
+		sensor := "replay_sensor"
+		timestamp := time.Date(2006, 1, 2, 15, 4, 5, 999900000, time.UTC)
+		filename := dataprocess.CreateTimestampFilename(dataDirectory+"/internal_state", sensor, ".pbstream", timestamp)
+		err = os.WriteFile(filename, internalState, os.ModePerm)
 		test.That(t, err, test.ShouldBeNil)
-		test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
-		test.That(t, timestamp1, test.ShouldResemble, timestamp2)
 
-		grpcServer.Stop()
-		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
-	})
+		defer func() {
+			err := os.RemoveAll(dataDirectory)
+			test.That(t, err, test.ShouldBeNil)
+		}()
 
-	t.Run("Successful creation of cartographer slam service in non localization mode", func(t *testing.T) {
-		grpcServer, port := setupTestGRPCServer(t)
 		attrCfg := &vcConfig.Config{
-			Sensors:       []string{},
-			ConfigParams:  map[string]string{"mode": "2d"},
-			DataDirectory: dataDir,
-			Port:          "localhost:" + strconv.Itoa(port),
-			UseLiveData:   &_false,
-			MapRateSec:    &testMapRateSec,
+			ModularizationV2Enabled: &_true,
+			Sensors:                 []string{sensor},
+			ConfigParams:            map[string]string{"mode": "2d"},
+			DataDirectory:           dataDirectory,
+			UseLiveData:             &_true,
+			MapRateSec:              &_zeroInt,
 		}
 
 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
 		test.That(t, err, test.ShouldBeNil)
 
-		timestamp1, err := svc.GetLatestMapInfo(context.Background())
-		test.That(t, err, test.ShouldBeNil)
-		_, err = svc.GetPointCloudMap(context.Background())
-		test.That(t, err, test.ShouldBeNil)
-		timestamp2, err := svc.GetLatestMapInfo(context.Background())
-		test.That(t, err, test.ShouldBeNil)
-
-		test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
-		test.That(t, timestamp2.After(timestamp1), test.ShouldBeTrue)
-
-		grpcServer.Stop()
+		// TODO: Implement these
+		// timestamp1, err := svc.GetLatestMapInfo(context.Background())
+		// test.That(t, err, test.ShouldBeNil)
+		// _, err = svc.GetPointCloudMap(context.Background())
+		// test.That(t, err, test.ShouldBeNil)
+		// timestamp2, err := svc.GetLatestMapInfo(context.Background())
+		// test.That(t, err, test.ShouldBeNil)
+		// test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
+		// test.That(t, timestamp1, test.ShouldResemble, timestamp2)
+
+    logger.Warn("calling close")
 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
 	})
+
+	// t.Run("Successful creation of cartographer slam service in non localization mode when feature flag enabled", func(t *testing.T) {
+	// 	attrCfg := &vcConfig.Config{
+	// 		ModularizationV2Enabled: &_true,
+	// 		Sensors:       []string{},
+	// 		ConfigParams:  map[string]string{"mode": "2d"},
+	// 		DataDirectory: dataDir,
+	// 		UseLiveData:   &_false,
+	// 		MapRateSec:    &testMapRateSec,
+	// 	}
+
+	// 	svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+	// 	test.That(t, err, test.ShouldBeNil)
+
+	// // TODO: Implement these
+	// 	// timestamp1, err := svc.GetLatestMapInfo(context.Background())
+	// 	// test.That(t, err, test.ShouldBeNil)
+	// 	// _, err = svc.GetPointCloudMap(context.Background())
+	// 	// test.That(t, err, test.ShouldBeNil)
+	// 	// timestamp2, err := svc.GetLatestMapInfo(context.Background())
+	// 	// test.That(t, err, test.ShouldBeNil)
+
+	// 	// test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
+	// 	// test.That(t, timestamp2.After(timestamp1), test.ShouldBeTrue)
+
+	// 	test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
+	// })
 }
 
 func TestDataProcess(t *testing.T) {

From 2ccc1bc9853b84e763bf2d9edebf35a113c113c7 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 10:36:18 -0400
Subject: [PATCH 17/32] wip

---
 cartofacade/capi.go              |  8 +++
 cartofacade/carto_facade.go      | 20 +++----
 cartofacade/carto_facade_test.go | 50 +++++++++---------
 integration_test.go              |  6 +--
 viam-cartographer.go             | 89 +++++++++++++++++++++++---------
 viam-cartographer_test.go        | 66 +++++++++++++----------
 6 files changed, 148 insertions(+), 91 deletions(-)

diff --git a/cartofacade/capi.go b/cartofacade/capi.go
index e74742a2..2c3f12ae 100644
--- a/cartofacade/capi.go
+++ b/cartofacade/capi.go
@@ -29,6 +29,7 @@ import "C"
 
 import (
 	"errors"
+	"fmt"
 	"time"
 	"unsafe"
 )
@@ -161,6 +162,7 @@ func NewCarto(cfg CartoConfig, acfg CartoAlgoConfig, vcl CartoLibInterface) (Car
 
 // Start is a wrapper for viam_carto_start
 func (vc *Carto) start() error {
+  fmt.Printf("start: %p\n", vc)
 	status := C.viam_carto_start(vc.value)
 
 	if err := toError(status); err != nil {
@@ -172,6 +174,7 @@ func (vc *Carto) start() error {
 
 // Stop is a wrapper for viam_carto_stop
 func (vc *Carto) stop() error {
+  fmt.Printf("stop: %p\n", vc)
 	status := C.viam_carto_stop(vc.value)
 
 	if err := toError(status); err != nil {
@@ -183,6 +186,7 @@ func (vc *Carto) stop() error {
 
 // Terminate calls viam_carto_terminate to clean up memory for viam carto
 func (vc *Carto) terminate() error {
+  fmt.Printf("terminate: %p\n", vc)
 	status := C.viam_carto_terminate(&vc.value)
 
 	if err := toError(status); err != nil {
@@ -194,6 +198,7 @@ func (vc *Carto) terminate() error {
 
 // AddSensorReading is a wrapper for viam_carto_add_sensor_reading
 func (vc *Carto) addSensorReading(sensor string, readings []byte, timestamp time.Time) error {
+  fmt.Printf("addSensorReading: %p\n", vc)
 	value := toSensorReading(sensor, readings, timestamp)
 
 	status := C.viam_carto_add_sensor_reading(vc.value, &value)
@@ -212,6 +217,7 @@ func (vc *Carto) addSensorReading(sensor string, readings []byte, timestamp time
 
 // GetPosition is a wrapper for viam_carto_get_position
 func (vc *Carto) getPosition() (GetPosition, error) {
+  fmt.Printf("getPosition: %p\n", vc)
 	value := C.viam_carto_get_position_response{}
 
 	status := C.viam_carto_get_position(vc.value, &value)
@@ -232,6 +238,7 @@ func (vc *Carto) getPosition() (GetPosition, error) {
 
 // GetPointCloudMap is a wrapper for viam_carto_get_point_cloud_map
 func (vc *Carto) getPointCloudMap() ([]byte, error) {
+  fmt.Printf("getPointCloudMap: %p\n", vc)
 	// TODO: determine whether or not return needs to be a pointer for performance reasons
 	value := C.viam_carto_get_point_cloud_map_response{}
 
@@ -253,6 +260,7 @@ func (vc *Carto) getPointCloudMap() ([]byte, error) {
 
 // GetInternalState is a wrapper for viam_carto_get_internal_state
 func (vc *Carto) getInternalState() ([]byte, error) {
+  fmt.Printf("getInternalState: %p\n", vc)
 	value := C.viam_carto_get_internal_state_response{}
 
 	status := C.viam_carto_get_internal_state(vc.value, &value)
diff --git a/cartofacade/carto_facade.go b/cartofacade/carto_facade.go
index 3440bd21..d408ef91 100644
--- a/cartofacade/carto_facade.go
+++ b/cartofacade/carto_facade.go
@@ -29,7 +29,7 @@ func (cf *CartoFacade) Initialize(ctx context.Context, timeout time.Duration, ac
 		return errors.New("unable to cast response from cartofacade to a carto struct")
 	}
 
-	cf.carto = &carto
+	cf.Carto = &carto
 
 	return nil
 }
@@ -177,7 +177,7 @@ go runtime doesn't spawn multiple OS threads, which would harm performance.
 */
 type CartoFacade struct {
 	cartoLib        CartoLibInterface
-	carto           CartoInterface
+	Carto           CartoInterface
 	cartoConfig     CartoConfig
 	cartoAlgoConfig CartoAlgoConfig
 	requestChan     chan Request
@@ -250,7 +250,7 @@ type Request struct {
 // New instantiates the Cartofacade struct which limits calls into C.
 func New(cartoLib CartoLibInterface, cartoCfg CartoConfig, cartoAlgoCfg CartoAlgoConfig) CartoFacade {
 	return CartoFacade{
-		carto:           &Carto{},
+		Carto:           &Carto{},
 		cartoLib:        cartoLib,
 		cartoConfig:     cartoCfg,
 		cartoAlgoConfig: cartoAlgoCfg,
@@ -267,11 +267,11 @@ func (r *Request) doWork(
 	case initialize:
 		return NewCarto(cf.cartoConfig, cf.cartoAlgoConfig, cf.cartoLib)
 	case start:
-		return nil, cf.carto.start()
+		return nil, cf.Carto.start()
 	case stop:
-		return nil, cf.carto.stop()
+		return nil, cf.Carto.stop()
 	case terminate:
-		return nil, cf.carto.terminate()
+		return nil, cf.Carto.terminate()
 	case addSensorReading:
 		sensor, ok := r.requestParams[sensor].(string)
 		if !ok {
@@ -288,13 +288,13 @@ func (r *Request) doWork(
 			return nil, errors.New("could not cast inputted timestamp to times.Time")
 		}
 
-		return nil, cf.carto.addSensorReading(sensor, reading, timestamp)
+		return nil, cf.Carto.addSensorReading(sensor, reading, timestamp)
 	case position:
-		return cf.carto.getPosition()
+		return cf.Carto.getPosition()
 	case internalState:
-		return cf.carto.getInternalState()
+		return cf.Carto.getInternalState()
 	case pointCloudMap:
-		return cf.carto.getPointCloudMap()
+		return cf.Carto.getPointCloudMap()
 	}
 	return nil, fmt.Errorf("no worktype found for: %v", r.requestType)
 }
diff --git a/cartofacade/carto_facade_test.go b/cartofacade/carto_facade_test.go
index d4f44f1a..1750b367 100644
--- a/cartofacade/carto_facade_test.go
+++ b/cartofacade/carto_facade_test.go
@@ -39,7 +39,7 @@ func TestRequest(t *testing.T) {
 		}
 
 		cf := New(&cartoLib, config, algoConfig)
-		cf.carto = &carto
+		cf.Carto = &carto
 		cf.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 		res, err := cf.request(cancelCtx, position, map[RequestParamType]interface{}{}, 5*time.Second)
@@ -66,7 +66,7 @@ func TestRequest(t *testing.T) {
 		}
 
 		cf := New(&cartoLib, config, algoConfig)
-		cf.carto = &carto
+		cf.Carto = &carto
 		cf.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 		_, err = cf.request(cancelCtx, start, map[RequestParamType]interface{}{}, 5*time.Second)
@@ -92,7 +92,7 @@ func TestRequest(t *testing.T) {
 		}
 
 		cf := New(&cartoLib, config, algoConfig)
-		cf.carto = &carto
+		cf.Carto = &carto
 		cf.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 		cancelFunc()
 		activeBackgroundWorkers.Wait()
@@ -121,7 +121,7 @@ func TestRequest(t *testing.T) {
 		}
 
 		cf := New(&cartoLib, config, algoConfig)
-		cf.carto = &carto
+		cf.Carto = &carto
 		cf.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 		_, err = cf.request(cancelCtx, start, map[RequestParamType]interface{}{}, 10*time.Millisecond)
@@ -180,7 +180,7 @@ func TestStart(t *testing.T) {
 	carto.StartFunc = func() error {
 		return nil
 	}
-	cartoFacade.carto = &carto
+	cartoFacade.Carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing Start", func(t *testing.T) {
@@ -191,7 +191,7 @@ func TestStart(t *testing.T) {
 		carto.StartFunc = func() error {
 			return errors.New("test error 1")
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// returns error
 		err = cartoFacade.Start(cancelCtx, 5*time.Second)
@@ -202,7 +202,7 @@ func TestStart(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return nil
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// times out
 		err = cartoFacade.Start(cancelCtx, 1*time.Millisecond)
@@ -231,7 +231,7 @@ func TestStop(t *testing.T) {
 	carto.StopFunc = func() error {
 		return nil
 	}
-	cartoFacade.carto = &carto
+	cartoFacade.Carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing Stop", func(t *testing.T) {
@@ -242,7 +242,7 @@ func TestStop(t *testing.T) {
 		carto.StopFunc = func() error {
 			return errors.New("test error 2")
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// returns error
 		err = cartoFacade.Stop(cancelCtx, 5*time.Second)
@@ -253,7 +253,7 @@ func TestStop(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return nil
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// times out
 		err = cartoFacade.Stop(cancelCtx, 1*time.Millisecond)
@@ -282,7 +282,7 @@ func TestTerminate(t *testing.T) {
 	carto.TerminateFunc = func() error {
 		return nil
 	}
-	cartoFacade.carto = &carto
+	cartoFacade.Carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing Terminate", func(t *testing.T) {
@@ -293,7 +293,7 @@ func TestTerminate(t *testing.T) {
 		carto.TerminateFunc = func() error {
 			return errors.New("test error 3")
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// returns error
 		err = cartoFacade.Terminate(cancelCtx, 5*time.Second)
@@ -304,7 +304,7 @@ func TestTerminate(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return nil
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// times out
 		err = cartoFacade.Terminate(cancelCtx, 1*time.Millisecond)
@@ -333,7 +333,7 @@ func TestAddSensorReading(t *testing.T) {
 	carto.AddSensorReadingFunc = func(name string, reading []byte, time time.Time) error {
 		return nil
 	}
-	cartoFacade.carto = &carto
+	cartoFacade.Carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing AddSensorReading", func(t *testing.T) {
@@ -355,7 +355,7 @@ func TestAddSensorReading(t *testing.T) {
 		carto.AddSensorReadingFunc = func(name string, reading []byte, time time.Time) error {
 			return errors.New("test error 4")
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// returns error
 		err = cartoFacade.AddSensorReading(cancelCtx, 5*time.Second, "mysensor", buf.Bytes(), timestamp)
@@ -366,7 +366,7 @@ func TestAddSensorReading(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return nil
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// times out
 		err = cartoFacade.AddSensorReading(cancelCtx, 1*time.Millisecond, "mysensor", buf.Bytes(), timestamp)
@@ -396,7 +396,7 @@ func TestGetPosition(t *testing.T) {
 		pos := GetPosition{X: 1, Y: 2, Z: 3}
 		return pos, nil
 	}
-	cartoFacade.carto = &carto
+	cartoFacade.Carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing GetPosition", func(t *testing.T) {
@@ -410,7 +410,7 @@ func TestGetPosition(t *testing.T) {
 		carto.GetPositionFunc = func() (GetPosition, error) {
 			return GetPosition{}, errors.New("test error 5")
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// returns error
 		_, err = cartoFacade.GetPosition(cancelCtx, 5*time.Second)
@@ -421,7 +421,7 @@ func TestGetPosition(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return GetPosition{}, nil
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// times out
 		_, err = cartoFacade.GetPosition(cancelCtx, 1*time.Millisecond)
@@ -451,7 +451,7 @@ func TestGetInternalState(t *testing.T) {
 		internalState := []byte("hello!")
 		return internalState, nil
 	}
-	cartoFacade.carto = &carto
+	cartoFacade.Carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing GetInternalState", func(t *testing.T) {
@@ -463,7 +463,7 @@ func TestGetInternalState(t *testing.T) {
 		carto.GetInternalStateFunc = func() ([]byte, error) {
 			return []byte{}, errors.New("test error 6")
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// returns error
 		_, err = cartoFacade.GetInternalState(cancelCtx, 5*time.Second)
@@ -474,7 +474,7 @@ func TestGetInternalState(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return []byte{}, nil
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// times out
 		_, err = cartoFacade.GetInternalState(cancelCtx, 1*time.Millisecond)
@@ -504,7 +504,7 @@ func TestGetPointCloudMap(t *testing.T) {
 		internalState := []byte("hello!")
 		return internalState, nil
 	}
-	cartoFacade.carto = &carto
+	cartoFacade.Carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing GetPointCloudMap", func(t *testing.T) {
@@ -516,7 +516,7 @@ func TestGetPointCloudMap(t *testing.T) {
 		carto.GetPointCloudMapFunc = func() ([]byte, error) {
 			return []byte{}, errors.New("test error 7")
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// returns error
 		_, err = cartoFacade.GetPointCloudMap(cancelCtx, 5*time.Second)
@@ -527,7 +527,7 @@ func TestGetPointCloudMap(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return []byte{}, nil
 		}
-		cartoFacade.carto = &carto
+		cartoFacade.Carto = &carto
 
 		// times out
 		_, err = cartoFacade.GetPointCloudMap(cancelCtx, 1*time.Millisecond)
diff --git a/integration_test.go b/integration_test.go
index 9854778a..ee795dd2 100644
--- a/integration_test.go
+++ b/integration_test.go
@@ -415,6 +415,6 @@ func testCartographerDir(t *testing.T, path string, expectedMaps int) {
 	test.That(t, len(mapsInDir), test.ShouldBeGreaterThanOrEqualTo, expectedMaps)
 }
 
-func TestCartographerIntegration2D(t *testing.T) {
-	integrationtestHelperCartographer(t, viamcartographer.Dim2d)
-}
+// func TestCartographerIntegration2D(t *testing.T) {
+// 	integrationtestHelperCartographer(t, viamcartographer.Dim2d)
+// }
diff --git a/viam-cartographer.go b/viam-cartographer.go
index 418fb23c..8fbc28cc 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -131,9 +131,11 @@ func initSensorProcess(cancelCtx context.Context, cartoSvc *cartographerService)
 		Logger:           cartoSvc.logger,
 		TelemetryEnabled: cartoSvc.logger.Level() == zapcore.DebugLevel,
 	}
-	cartoSvc.activeBackgroundWorkers.Add(1)
+	cartoSvc.logger.Warnf("cartoSvc.cartofacade.Carto: %p", &cartoSvc.cartofacade.Carto)
+
+	cartoSvc.sensorProcessWorkers.Add(1)
 	go func() {
-		defer cartoSvc.activeBackgroundWorkers.Done()
+		defer cartoSvc.sensorProcessWorkers.Done()
 		sensorprocess.Start(cancelCtx, spConfig)
 	}()
 }
@@ -194,6 +196,9 @@ func New(
 
 	// Need to pass in a long-lived context because ctx is short-lived
 	cancelCtx, cancelFunc := context.WithCancel(context.Background())
+	// Need to be able to shut down the sensor process before the cartoFacade
+	cancelSensorProcessCtx, cancelSensorProcessFunc := context.WithCancel(context.Background())
+	cancelCartoFacadeCtx, cancelCartoFacadeFunc := context.WithCancel(context.Background())
 
 	// Cartographer SLAM Service Object
 	cartoSvc := &cartographerService{
@@ -212,6 +217,8 @@ func New(
 		dataRateMs:                    dataRateMsec,
 		mapRateSec:                    mapRateSec,
 		cancelFunc:                    cancelFunc,
+		cancelSensorProcessFunc:       cancelSensorProcessFunc,
+		cancelCartoFacadeFunc:         cancelCartoFacadeFunc,
 		logger:                        logger,
 		bufferSLAMProcessLogs:         bufferSLAMProcessLogs,
 		modularizationV2Enabled:       modularizationV2Enabled,
@@ -224,6 +231,7 @@ func New(
 	}
 	success := false
 	defer func() {
+		// TODO: Change this to use err != nil
 		if !success {
 			logger.Errorw("New() hit error, closing...", "error", err)
 			if err := cartoSvc.Close(ctx); err != nil {
@@ -234,7 +242,7 @@ func New(
 
 	if modularizationV2Enabled {
 		if err := dim2d.ValidateGetData(
-			cancelCtx,
+			cancelSensorProcessCtx,
 			cartoSvc.lidar,
 			time.Duration(sensorValidationMaxTimeoutSec)*time.Second,
 			time.Duration(cartoSvc.sensorValidationIntervalSec)*time.Second,
@@ -243,13 +251,17 @@ func New(
 			return nil, err
 		}
 
-		err = initCartoFacade(cancelCtx, cartoSvc)
+		err = initCartoFacade(cancelCartoFacadeCtx, cartoSvc)
 		if err != nil {
 			return nil, err
 		}
-		initSensorProcess(cancelCtx, cartoSvc)
+
+		cartoSvc.logger.Warnf("before sensor process: cartoSvc.cartofacade.Carto: %p", &cartoSvc.cartofacade.Carto)
+		initSensorProcess(cancelSensorProcessCtx, cartoSvc)
+		success = true
 		return cartoSvc, err
 	}
+
 	err = initCartoGrpcServer(ctx, cancelCtx, cartoSvc)
 	if err != nil {
 		return nil, err
@@ -352,7 +364,7 @@ func parseCartoAlgoConfig(configParams map[string]string, logger golog.Logger) (
 // 1. creates a new initCartoFacade
 // 2. initializes it and starts it
 // 3. terminates it if start fails.
-func initCartoFacade(cancelCtx context.Context, cartoSvc *cartographerService) error {
+func initCartoFacade(ctx context.Context, cartoSvc *cartographerService) error {
 	cartoAlgoConfig, err := parseCartoAlgoConfig(cartoSvc.configParams, cartoSvc.logger)
 	if err != nil {
 		return err
@@ -367,22 +379,25 @@ func initCartoFacade(cancelCtx context.Context, cartoSvc *cartographerService) e
 	}
 
 	cf := cartofacade.New(&cartoLib, cartoCfg, cartoAlgoConfig)
-	err = cf.Initialize(cancelCtx, cartoSvc.cartoFacadeTimeout, &cartoSvc.activeBackgroundWorkers)
+	err = cf.Initialize(ctx, cartoSvc.cartoFacadeTimeout, &cartoSvc.cartoFacadeWorkers)
 	if err != nil {
 		cartoSvc.logger.Errorw("cartofacade initialize failed", "error", err)
 		return err
 	}
-	err = cf.Start(cancelCtx, cartoSvc.cartoFacadeTimeout)
+	err = cf.Start(ctx, cartoSvc.cartoFacadeTimeout)
 	if err != nil {
 		cartoSvc.logger.Errorw("cartofacade start failed", "error", err)
-		termErr := cf.Terminate(cancelCtx, cartoSvc.cartoFacadeTimeout)
+		termErr := cf.Terminate(ctx, cartoSvc.cartoFacadeTimeout)
 		if termErr != nil {
 			cartoSvc.logger.Errorw("cartofacade terminate failed", "error", termErr)
 			return termErr
 		}
 		return err
 	}
+
+	cartoSvc.logger.Warnf("cf.Carto: %p", cf.Carto)
 	cartoSvc.cartofacade = &cf
+	cartoSvc.logger.Warnf("cartoSvc.cartofacade.Carto: %p", cartoSvc.cartofacade.Carto)
 
 	return nil
 }
@@ -440,38 +455,55 @@ type cartographerService struct {
 	lidar             lidar.Lidar
 	executableName    string
 	subAlgo           SubAlgo
-	slamProcess       pexec.ProcessManager
-	clientAlgo        pb.SLAMServiceClient
-	clientAlgoClose   func() error
-
-	configParams        map[string]string
-	dataDirectory       string
-	sensors             []string
+	// deprecated
+	slamProcess pexec.ProcessManager
+	// deprecated
+	clientAlgo pb.SLAMServiceClient
+	// deprecated
+	clientAlgoClose func() error
+
+	configParams  map[string]string
+	dataDirectory string
+	sensors       []string
+	// deprecated
 	deleteProcessedData bool
-	useLiveData         bool
+	// deprecated
+	useLiveData bool
 
 	modularizationV2Enabled bool
 	cartofacade             *cartofacade.CartoFacade
 	cartoFacadeTimeout      time.Duration
 
+	// deprecated
 	port       string
 	dataRateMs int
 	mapRateSec int
 
+	// deprecated
 	cancelFunc              func()
+	cancelSensorProcessFunc func()
+	cancelCartoFacadeFunc   func()
 	logger                  golog.Logger
+	// deprecated
 	activeBackgroundWorkers sync.WaitGroup
-
-	bufferSLAMProcessLogs        bool
-	slamProcessLogReader         io.ReadCloser
-	slamProcessLogWriter         io.WriteCloser
+	sensorProcessWorkers    sync.WaitGroup
+	cartoFacadeWorkers      sync.WaitGroup
+
+	// deprecated
+	bufferSLAMProcessLogs bool
+	// deprecated
+	slamProcessLogReader io.ReadCloser
+	// deprecated
+	slamProcessLogWriter io.WriteCloser
+	// deprecated
 	slamProcessBufferedLogReader bufio.Reader
 
 	localizationMode              bool
 	mapTimestamp                  time.Time
 	sensorValidationMaxTimeoutSec int
 	sensorValidationIntervalSec   int
-	dialMaxTimeoutSec             int
+	// deprecated
+	dialMaxTimeoutSec int
 }
 
 // GetPosition forwards the request for positional data to the slam library's gRPC service. Once a response is received,
@@ -611,14 +643,21 @@ func (cartoSvc *cartographerService) DoCommand(ctx context.Context, req map[stri
 
 // Close out of all slam related processes.
 func (cartoSvc *cartographerService) Close(ctx context.Context) error {
+	// TODO: Make this atomic & idempotent
 	if cartoSvc.modularizationV2Enabled {
-		// TODO: Make this atomic & idempotent
+		// stop sensor process workers
+		cartoSvc.cancelSensorProcessFunc()
+		cartoSvc.sensorProcessWorkers.Wait()
+
+		// termintae carto facade
 		err := terminateCartoFacade(ctx, cartoSvc)
 		if err != nil {
 			cartoSvc.logger.Errorw("close hit error", "error", err)
 		}
-		cartoSvc.cancelFunc()
-		cartoSvc.activeBackgroundWorkers.Wait()
+
+		// stop carto facade workers
+		cartoSvc.cancelCartoFacadeFunc()
+		cartoSvc.cartoFacadeWorkers.Wait()
 		return nil
 	}
 
diff --git a/viam-cartographer_test.go b/viam-cartographer_test.go
index 4ff33cfb..5014897c 100644
--- a/viam-cartographer_test.go
+++ b/viam-cartographer_test.go
@@ -43,6 +43,39 @@ var (
 	_zeroTime      = time.Time{}
 )
 
+func initTestCL(t *testing.T, logger golog.Logger) func() {
+	t.Helper()
+	err := viamcartographer.InitCartoLib(logger)
+	test.That(t, err, test.ShouldBeNil)
+	return func() {
+		err = viamcartographer.TerminateCartoLib()
+		test.That(t, err, test.ShouldBeNil)
+	}
+}
+
+func initInternalState(t *testing.T) (string, func()) {
+	dataDirectory, err := os.MkdirTemp("", "*")
+	test.That(t, err, test.ShouldBeNil)
+
+	internalStateDir := filepath.Join(dataDirectory, "internal_state")
+	err = os.Mkdir(internalStateDir, os.ModePerm)
+	test.That(t, err, test.ShouldBeNil)
+
+	internalState, err := os.ReadFile(artifact.MustPath("viam-cartographer/outputs/viam-office-02-22-3/internal_state/internal_state_0.pbstream"))
+	test.That(t, err, test.ShouldBeNil)
+
+	timestamp := time.Date(2006, 1, 2, 15, 4, 5, 999900000, time.UTC)
+	filename := dataprocess.CreateTimestampFilename(dataDirectory+"/internal_state", "internal_state", ".pbstream", timestamp)
+	err = os.WriteFile(filename, internalState, os.ModePerm)
+	test.That(t, err, test.ShouldBeNil)
+
+	return dataDirectory, func() {
+		err := os.RemoveAll(dataDirectory)
+		test.That(t, err, test.ShouldBeNil)
+	}
+
+}
+
 func TestNew(t *testing.T) {
 	logger := golog.NewTestLogger(t)
 	// dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger)
@@ -269,37 +302,15 @@ func TestNew(t *testing.T) {
 	// 	})
 
 	t.Run("Successful creation of cartographer slam service in localization mode when feature flag enabled", func(t *testing.T) {
-		err := viamcartographer.InitCartoLib(logger)
-		test.That(t, err, test.ShouldBeNil)
-		defer func() {
-			err = viamcartographer.TerminateCartoLib()
-			test.That(t, err, test.ShouldBeNil)
-		}()
-
-		dataDirectory, err := os.MkdirTemp("", "*")
-		test.That(t, err, test.ShouldBeNil)
-
-		internalStateDir := filepath.Join(dataDirectory, "internal_state")
-		err = os.Mkdir(internalStateDir, os.ModePerm)
-		test.That(t, err, test.ShouldBeNil)
-
-		internalState, err := os.ReadFile(artifact.MustPath("viam-cartographer/outputs/viam-office-02-22-3/internal_state/internal_state_0.pbstream"))
-		test.That(t, err, test.ShouldBeNil)
-
-		sensor := "replay_sensor"
-		timestamp := time.Date(2006, 1, 2, 15, 4, 5, 999900000, time.UTC)
-		filename := dataprocess.CreateTimestampFilename(dataDirectory+"/internal_state", sensor, ".pbstream", timestamp)
-		err = os.WriteFile(filename, internalState, os.ModePerm)
-		test.That(t, err, test.ShouldBeNil)
+		termFunc := initTestCL(t, logger)
+		defer termFunc()
 
-		defer func() {
-			err := os.RemoveAll(dataDirectory)
-			test.That(t, err, test.ShouldBeNil)
-		}()
+		dataDirectory, fsCleanupFunc := initInternalState(t)
+		defer fsCleanupFunc()
 
 		attrCfg := &vcConfig.Config{
 			ModularizationV2Enabled: &_true,
-			Sensors:                 []string{sensor},
+			Sensors:                 []string{"replay_sensor"},
 			ConfigParams:            map[string]string{"mode": "2d"},
 			DataDirectory:           dataDirectory,
 			UseLiveData:             &_true,
@@ -319,7 +330,6 @@ func TestNew(t *testing.T) {
 		// test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
 		// test.That(t, timestamp1, test.ShouldResemble, timestamp2)
 
-    logger.Warn("calling close")
 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
 	})
 

From a4012ab86adb90e7b29edb9a22b9ce024c678a9d Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 10:49:23 -0400
Subject: [PATCH 18/32] add tests

---
 viam-cartographer_test.go | 534 ++++++++++++++++++++------------------
 1 file changed, 285 insertions(+), 249 deletions(-)

diff --git a/viam-cartographer_test.go b/viam-cartographer_test.go
index 5014897c..add73a08 100644
--- a/viam-cartographer_test.go
+++ b/viam-cartographer_test.go
@@ -16,7 +16,7 @@ import (
 	"time"
 
 	"github.com/edaniels/golog"
-	// "github.com/pkg/errors"
+	"github.com/pkg/errors"
 	viamgrpc "go.viam.com/rdk/grpc"
 	"go.viam.com/test"
 	"google.golang.org/grpc"
@@ -78,228 +78,258 @@ func initInternalState(t *testing.T) (string, func()) {
 
 func TestNew(t *testing.T) {
 	logger := golog.NewTestLogger(t)
-	// dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger)
-	// test.That(t, err, test.ShouldBeNil)
-
-	// 	t.Run("Successful creation of cartographer slam service with no sensor", func(t *testing.T) {
-	// 		grpcServer, port := setupTestGRPCServer(t)
-	// 		test.That(t, err, test.ShouldBeNil)
-	// 		attrCfg := &vcConfig.Config{
-	// 			Sensors:       []string{},
-	// 			ConfigParams:  map[string]string{"mode": "2d"},
-	// 			DataDirectory: dataDir,
-	// 			Port:          "localhost:" + strconv.Itoa(port),
-	// 			UseLiveData:   &_false,
-	// 		}
-
-	// 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-	// 		test.That(t, err, test.ShouldBeNil)
-
-	// 		grpcServer.Stop()
-	// 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
-	// 	})
-
-	// 	t.Run("Failed creation of cartographer slam service with more than one sensor", func(t *testing.T) {
-	// 		grpcServer, port := setupTestGRPCServer(t)
-	// 		test.That(t, err, test.ShouldBeNil)
-	// 		attrCfg := &vcConfig.Config{
-	// 			Sensors:       []string{"lidar", "one-too-many"},
-	// 			ConfigParams:  map[string]string{"mode": "2d"},
-	// 			DataDirectory: dataDir,
-	// 			Port:          "localhost:" + strconv.Itoa(port),
-	// 			UseLiveData:   &_false,
-	// 		}
-
-	// 		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-	// 		test.That(t, err, test.ShouldBeError,
-	// 			errors.New("configuring lidar camera error: 'sensors' must contain only one "+
-	// 				"lidar camera, but is 'sensors: [lidar, one-too-many]'"))
-
-	// 		grpcServer.Stop()
-	// 	})
-
-	// 	t.Run("Failed creation of cartographer slam service with non-existing sensor", func(t *testing.T) {
-	// 		attrCfg := &vcConfig.Config{
-	// 			Sensors:       []string{"gibberish"},
-	// 			ConfigParams:  map[string]string{"mode": "2d"},
-	// 			DataDirectory: dataDir,
-	// 			DataRateMsec:  testDataRateMsec,
-	// 			UseLiveData:   &_true,
-	// 		}
-
-	// 		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-	// 		test.That(t, err, test.ShouldBeError,
-	// 			errors.New("configuring lidar camera error: error getting lidar camera "+
-	// 				"gibberish for slam service: \"rdk:component:camera/gibberish\" missing from dependencies"))
-	// 	})
-
-	// 	t.Run("Successful creation of cartographer slam service with good lidar", func(t *testing.T) {
-	// 		grpcServer, port := setupTestGRPCServer(t)
-	// 		attrCfg := &vcConfig.Config{
-	// 			Sensors:       []string{"good_lidar"},
-	// 			ConfigParams:  map[string]string{"mode": "2d"},
-	// 			DataDirectory: dataDir,
-	// 			DataRateMsec:  testDataRateMsec,
-	// 			Port:          "localhost:" + strconv.Itoa(port),
-	// 			UseLiveData:   &_true,
-	// 		}
-
-	// 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-	// 		test.That(t, err, test.ShouldBeNil)
-
-	// 		grpcServer.Stop()
-	// 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
-	// 	})
-
-	// 	t.Run("Failed creation of cartographer slam service with invalid sensor "+
-	// 		"that errors during call to NextPointCloud", func(t *testing.T) {
-	// 		attrCfg := &vcConfig.Config{
-	// 			Sensors:       []string{"invalid_sensor"},
-	// 			ConfigParams:  map[string]string{"mode": "2d"},
-	// 			DataDirectory: dataDir,
-	// 			DataRateMsec:  testDataRateMsec,
-	// 			UseLiveData:   &_true,
-	// 		}
-
-	// 		_, err = internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-	// 		test.That(t, err, test.ShouldBeError,
-	// 			errors.New("getting and saving data failed: error getting data from sensor: invalid sensor"))
-	// 	})
-
-	// 	internaltesthelper.ClearDirectory(t, dataDir)
-
-	// 	t.Run("Successful creation of cartographer slam service in localization mode", func(t *testing.T) {
-	// 		grpcServer, port := setupTestGRPCServer(t)
-
-	// 		attrCfg := &vcConfig.Config{
-	// 			Sensors:       []string{},
-	// 			ConfigParams:  map[string]string{"mode": "2d"},
-	// 			DataDirectory: dataDir,
-	// 			Port:          "localhost:" + strconv.Itoa(port),
-	// 			UseLiveData:   &_false,
-	// 			MapRateSec:    &_zeroInt,
-	// 		}
-
-	// 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-	// 		test.That(t, err, test.ShouldBeNil)
-
-	// 		timestamp1, err := svc.GetLatestMapInfo(context.Background())
-	// 		test.That(t, err, test.ShouldBeNil)
-	// 		_, err = svc.GetPointCloudMap(context.Background())
-	// 		test.That(t, err, test.ShouldBeNil)
-	// 		timestamp2, err := svc.GetLatestMapInfo(context.Background())
-	// 		test.That(t, err, test.ShouldBeNil)
-	// 		test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
-	// 		test.That(t, timestamp1, test.ShouldResemble, timestamp2)
-
-	// 		grpcServer.Stop()
-	// 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
-	// 	})
-
-	// 	t.Run("Successful creation of cartographer slam service in non localization mode", func(t *testing.T) {
-	// 		grpcServer, port := setupTestGRPCServer(t)
-	// 		attrCfg := &vcConfig.Config{
-	// 			Sensors:       []string{},
-	// 			ConfigParams:  map[string]string{"mode": "2d"},
-	// 			DataDirectory: dataDir,
-	// 			Port:          "localhost:" + strconv.Itoa(port),
-	// 			UseLiveData:   &_false,
-	// 			MapRateSec:    &testMapRateSec,
-	// 		}
-
-	// 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-	// 		test.That(t, err, test.ShouldBeNil)
-
-	// 		timestamp1, err := svc.GetLatestMapInfo(context.Background())
-	// 		test.That(t, err, test.ShouldBeNil)
-	// 		_, err = svc.GetPointCloudMap(context.Background())
-	// 		test.That(t, err, test.ShouldBeNil)
-	// 		timestamp2, err := svc.GetLatestMapInfo(context.Background())
-	// 		test.That(t, err, test.ShouldBeNil)
-
-	// 		test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
-	// 		test.That(t, timestamp2.After(timestamp1), test.ShouldBeTrue)
-
-	// 		grpcServer.Stop()
-	// 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
-	// 	})
-
-	// 	t.Run("Successful creation of cartographer slam service with no sensor when feature flag enabled", func(t *testing.T) {
-	// 		attrCfg := &vcConfig.Config{
-	// 			ModularizationV2Enabled: &_true,
-	// 			Sensors:                 []string{},
-	// 			ConfigParams:            map[string]string{"mode": "2d"},
-	// 			DataDirectory:           dataDir,
-	// 			UseLiveData:             &_false,
-	// 		}
-
-	// 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-	// 		test.That(t, err, test.ShouldBeNil)
-	// 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
-	// 	})
-
-	// 	t.Run("Failed creation of cartographer slam service with more than one sensor when feature flag enabled", func(t *testing.T) {
-	// 		attrCfg := &vcConfig.Config{
-	// 			ModularizationV2Enabled: &_true,
-	// 			Sensors:       []string{"lidar", "one-too-many"},
-	// 			ConfigParams:  map[string]string{"mode": "2d"},
-	// 			DataDirectory: dataDir,
-	// 			UseLiveData:   &_false,
-	// 		}
-
-	// 		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-	// 		test.That(t, err, test.ShouldBeError,
-	// 			errors.New("configuring lidar camera error: 'sensors' must contain only one "+
-	// 				"lidar camera, but is 'sensors: [lidar, one-too-many]'"))
-	// 	})
-
-	// 	t.Run("Failed creation of cartographer slam service with non-existing sensor when feature flag enabled", func(t *testing.T) {
-	// 		attrCfg := &vcConfig.Config{
-	// 			ModularizationV2Enabled: &_true,
-	// 			Sensors:       []string{"gibberish"},
-	// 			ConfigParams:  map[string]string{"mode": "2d"},
-	// 			DataDirectory: dataDir,
-	// 			DataRateMsec:  testDataRateMsec,
-	// 			UseLiveData:   &_true,
-	// 		}
-
-	// 		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-	// 		test.That(t, err, test.ShouldBeError,
-	// 			errors.New("configuring lidar camera error: error getting lidar camera "+
-	// 				"gibberish for slam service: \"rdk:component:camera/gibberish\" missing from dependencies"))
-	// 	})
-
-	// 	t.Run("Successful creation of cartographer slam service with good lidar when feature flag enabled", func(t *testing.T) {
-	// 		attrCfg := &vcConfig.Config{
-	// 			ModularizationV2Enabled: &_true,
-	// 			Sensors:       []string{"good_lidar"},
-	// 			ConfigParams:  map[string]string{"mode": "2d"},
-	// 			DataDirectory: dataDir,
-	// 			DataRateMsec:  testDataRateMsec,
-	// 			UseLiveData:   &_true,
-	// 		}
-
-	// 		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-	// 		test.That(t, err, test.ShouldBeNil)
-
-	// 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
-	// 	})
-
-	// 	t.Run("Failed creation of cartographer slam service with invalid sensor "+
-	// 		"that errors during call to NextPointCloud when feature flag enabled", func(t *testing.T) {
-	// 		attrCfg := &vcConfig.Config{
-	// 			ModularizationV2Enabled: &_true,
-	// 			Sensors:       []string{"invalid_sensor"},
-	// 			ConfigParams:  map[string]string{"mode": "2d"},
-	// 			DataDirectory: dataDir,
-	// 			DataRateMsec:  testDataRateMsec,
-	// 			UseLiveData:   &_true,
-	// 		}
-
-	// 		_, err = internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-	// 		test.That(t, err, test.ShouldBeError,
-	// 			errors.New("failed to get data from lidar: ValidateGetData timeout: invalid sensor"))
-	// 	})
+	dataDir, err := internaltesthelper.CreateTempFolderArchitecture(logger)
+	test.That(t, err, test.ShouldBeNil)
+
+	t.Run("Successful creation of cartographer slam service with no sensor", func(t *testing.T) {
+		grpcServer, port := setupTestGRPCServer(t)
+		test.That(t, err, test.ShouldBeNil)
+		attrCfg := &vcConfig.Config{
+			Sensors:       []string{},
+			ConfigParams:  map[string]string{"mode": "2d"},
+			DataDirectory: dataDir,
+			Port:          "localhost:" + strconv.Itoa(port),
+			UseLiveData:   &_false,
+		}
+
+		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		test.That(t, err, test.ShouldBeNil)
+
+		grpcServer.Stop()
+		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
+	})
+
+	t.Run("Failed creation of cartographer slam service with more than one sensor", func(t *testing.T) {
+		grpcServer, port := setupTestGRPCServer(t)
+		test.That(t, err, test.ShouldBeNil)
+		attrCfg := &vcConfig.Config{
+			Sensors:       []string{"lidar", "one-too-many"},
+			ConfigParams:  map[string]string{"mode": "2d"},
+			DataDirectory: dataDir,
+			Port:          "localhost:" + strconv.Itoa(port),
+			UseLiveData:   &_false,
+		}
+
+		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		test.That(t, err, test.ShouldBeError,
+			errors.New("configuring lidar camera error: 'sensors' must contain only one "+
+				"lidar camera, but is 'sensors: [lidar, one-too-many]'"))
+
+		grpcServer.Stop()
+	})
+
+	t.Run("Failed creation of cartographer slam service with non-existing sensor", func(t *testing.T) {
+		attrCfg := &vcConfig.Config{
+			Sensors:       []string{"gibberish"},
+			ConfigParams:  map[string]string{"mode": "2d"},
+			DataDirectory: dataDir,
+			DataRateMsec:  testDataRateMsec,
+			UseLiveData:   &_true,
+		}
+
+		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		test.That(t, err, test.ShouldBeError,
+			errors.New("configuring lidar camera error: error getting lidar camera "+
+				"gibberish for slam service: \"rdk:component:camera/gibberish\" missing from dependencies"))
+	})
+
+	t.Run("Successful creation of cartographer slam service with good lidar", func(t *testing.T) {
+		grpcServer, port := setupTestGRPCServer(t)
+		attrCfg := &vcConfig.Config{
+			Sensors:       []string{"good_lidar"},
+			ConfigParams:  map[string]string{"mode": "2d"},
+			DataDirectory: dataDir,
+			DataRateMsec:  testDataRateMsec,
+			Port:          "localhost:" + strconv.Itoa(port),
+			UseLiveData:   &_true,
+		}
+
+		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		test.That(t, err, test.ShouldBeNil)
+
+		grpcServer.Stop()
+		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
+	})
+
+	t.Run("Failed creation of cartographer slam service with invalid sensor "+
+		"that errors during call to NextPointCloud", func(t *testing.T) {
+		attrCfg := &vcConfig.Config{
+			Sensors:       []string{"invalid_sensor"},
+			ConfigParams:  map[string]string{"mode": "2d"},
+			DataDirectory: dataDir,
+			DataRateMsec:  testDataRateMsec,
+			UseLiveData:   &_true,
+		}
+
+		_, err = internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		test.That(t, err, test.ShouldBeError,
+			errors.New("getting and saving data failed: error getting data from sensor: invalid sensor"))
+	})
+
+	internaltesthelper.ClearDirectory(t, dataDir)
+
+	t.Run("Successful creation of cartographer slam service in localization mode", func(t *testing.T) {
+		grpcServer, port := setupTestGRPCServer(t)
+
+		attrCfg := &vcConfig.Config{
+			Sensors:       []string{},
+			ConfigParams:  map[string]string{"mode": "2d"},
+			DataDirectory: dataDir,
+			Port:          "localhost:" + strconv.Itoa(port),
+			UseLiveData:   &_false,
+			MapRateSec:    &_zeroInt,
+		}
+
+		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		test.That(t, err, test.ShouldBeNil)
+
+		timestamp1, err := svc.GetLatestMapInfo(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		_, err = svc.GetPointCloudMap(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		timestamp2, err := svc.GetLatestMapInfo(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
+		test.That(t, timestamp1, test.ShouldResemble, timestamp2)
+
+		grpcServer.Stop()
+		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
+	})
+
+	t.Run("Successful creation of cartographer slam service in non localization mode", func(t *testing.T) {
+		grpcServer, port := setupTestGRPCServer(t)
+		attrCfg := &vcConfig.Config{
+			Sensors:       []string{},
+			ConfigParams:  map[string]string{"mode": "2d"},
+			DataDirectory: dataDir,
+			Port:          "localhost:" + strconv.Itoa(port),
+			UseLiveData:   &_false,
+			MapRateSec:    &testMapRateSec,
+		}
+
+		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		test.That(t, err, test.ShouldBeNil)
+
+		timestamp1, err := svc.GetLatestMapInfo(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		_, err = svc.GetPointCloudMap(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		timestamp2, err := svc.GetLatestMapInfo(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+
+		test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
+		test.That(t, timestamp2.After(timestamp1), test.ShouldBeTrue)
+
+		grpcServer.Stop()
+		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
+	})
+
+	t.Run("Fails to create cartographer slam service with no sensor when feature flag enabled", func(t *testing.T) {
+		termFunc := initTestCL(t, logger)
+		defer termFunc()
+
+		dataDirectory, fsCleanupFunc := initInternalState(t)
+		defer fsCleanupFunc()
+
+		attrCfg := &vcConfig.Config{
+			ModularizationV2Enabled: &_true,
+			Sensors:                 []string{},
+			ConfigParams:            map[string]string{"mode": "2d"},
+			DataDirectory:           dataDirectory,
+			UseLiveData:             &_false,
+		}
+
+		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		test.That(t, err, test.ShouldBeError, errors.New("error validating \"path\": \"at least one sensor must be configured\" is required"))
+		test.That(t, svc, test.ShouldBeNil)
+	})
+
+	t.Run("Failed creation of cartographer slam service with more than one sensor when feature flag enabled", func(t *testing.T) {
+		termFunc := initTestCL(t, logger)
+		defer termFunc()
+
+		dataDirectory, fsCleanupFunc := initInternalState(t)
+		defer fsCleanupFunc()
+
+		attrCfg := &vcConfig.Config{
+			ModularizationV2Enabled: &_true,
+			Sensors:                 []string{"lidar", "one-too-many"},
+			ConfigParams:            map[string]string{"mode": "2d"},
+			DataDirectory:           dataDirectory,
+			UseLiveData:             &_false,
+		}
+
+		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		test.That(t, err, test.ShouldBeError,
+			errors.New("configuring lidar camera error: 'sensors' must contain only one "+
+				"lidar camera, but is 'sensors: [lidar, one-too-many]'"))
+	})
+
+	t.Run("Failed creation of cartographer slam service with non-existing sensor when feature flag enabled", func(t *testing.T) {
+		termFunc := initTestCL(t, logger)
+		defer termFunc()
+
+		dataDirectory, fsCleanupFunc := initInternalState(t)
+		defer fsCleanupFunc()
+
+		attrCfg := &vcConfig.Config{
+			ModularizationV2Enabled: &_true,
+			Sensors:                 []string{"gibberish"},
+			ConfigParams:            map[string]string{"mode": "2d"},
+			DataDirectory:           dataDirectory,
+			DataRateMsec:            testDataRateMsec,
+			UseLiveData:             &_true,
+		}
+
+		_, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		test.That(t, err, test.ShouldBeError,
+			errors.New("configuring lidar camera error: error getting lidar camera "+
+				"gibberish for slam service: \"rdk:component:camera/gibberish\" missing from dependencies"))
+	})
+
+	t.Run("Successful creation of cartographer slam service with good lidar when feature flag enabled", func(t *testing.T) {
+		termFunc := initTestCL(t, logger)
+		defer termFunc()
+
+		dataDirectory, fsCleanupFunc := initInternalState(t)
+		defer fsCleanupFunc()
+
+		attrCfg := &vcConfig.Config{
+			ModularizationV2Enabled: &_true,
+			Sensors:                 []string{"good_lidar"},
+			ConfigParams:            map[string]string{"mode": "2d"},
+			DataDirectory:           dataDirectory,
+			DataRateMsec:            testDataRateMsec,
+			UseLiveData:             &_true,
+		}
+
+		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		test.That(t, err, test.ShouldBeNil)
+
+		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
+	})
+
+	t.Run("Failed creation of cartographer slam service with invalid sensor "+
+		"that errors during call to NextPointCloud when feature flag enabled", func(t *testing.T) {
+		termFunc := initTestCL(t, logger)
+		defer termFunc()
+
+		dataDirectory, fsCleanupFunc := initInternalState(t)
+		defer fsCleanupFunc()
+
+		attrCfg := &vcConfig.Config{
+			ModularizationV2Enabled: &_true,
+			Sensors:                 []string{"invalid_sensor"},
+			ConfigParams:            map[string]string{"mode": "2d"},
+			DataDirectory:           dataDirectory,
+			DataRateMsec:            testDataRateMsec,
+			UseLiveData:             &_true,
+		}
+
+		_, err = internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		test.That(t, err, test.ShouldBeError,
+			errors.New("failed to get data from lidar: ValidateGetData timeout: invalid sensor"))
+	})
 
 	t.Run("Successful creation of cartographer slam service in localization mode when feature flag enabled", func(t *testing.T) {
 		termFunc := initTestCL(t, logger)
@@ -333,32 +363,38 @@ func TestNew(t *testing.T) {
 		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
 	})
 
-	// t.Run("Successful creation of cartographer slam service in non localization mode when feature flag enabled", func(t *testing.T) {
-	// 	attrCfg := &vcConfig.Config{
-	// 		ModularizationV2Enabled: &_true,
-	// 		Sensors:       []string{},
-	// 		ConfigParams:  map[string]string{"mode": "2d"},
-	// 		DataDirectory: dataDir,
-	// 		UseLiveData:   &_false,
-	// 		MapRateSec:    &testMapRateSec,
-	// 	}
-
-	// 	svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
-	// 	test.That(t, err, test.ShouldBeNil)
-
-	// // TODO: Implement these
-	// 	// timestamp1, err := svc.GetLatestMapInfo(context.Background())
-	// 	// test.That(t, err, test.ShouldBeNil)
-	// 	// _, err = svc.GetPointCloudMap(context.Background())
-	// 	// test.That(t, err, test.ShouldBeNil)
-	// 	// timestamp2, err := svc.GetLatestMapInfo(context.Background())
-	// 	// test.That(t, err, test.ShouldBeNil)
-
-	// 	// test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
-	// 	// test.That(t, timestamp2.After(timestamp1), test.ShouldBeTrue)
-
-	// 	test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
-	// })
+	t.Run("Successful creation of cartographer slam service in non localization mode when feature flag enabled", func(t *testing.T) {
+		termFunc := initTestCL(t, logger)
+		defer termFunc()
+
+		dataDirectory, err := os.MkdirTemp("", "*")
+		test.That(t, err, test.ShouldBeNil)
+
+		attrCfg := &vcConfig.Config{
+			ModularizationV2Enabled: &_true,
+			Sensors:                 []string{"replay_sensor"},
+			ConfigParams:            map[string]string{"mode": "2d"},
+			DataDirectory:           dataDirectory,
+			UseLiveData:             &_false,
+			MapRateSec:              &testMapRateSec,
+		}
+
+		svc, err := internaltesthelper.CreateSLAMService(t, attrCfg, logger, false, testExecutableName)
+		test.That(t, err, test.ShouldBeNil)
+
+		// TODO: Implement these
+		// timestamp1, err := svc.GetLatestMapInfo(context.Background())
+		// test.That(t, err, test.ShouldBeNil)
+		// _, err = svc.GetPointCloudMap(context.Background())
+		// test.That(t, err, test.ShouldBeNil)
+		// timestamp2, err := svc.GetLatestMapInfo(context.Background())
+		// test.That(t, err, test.ShouldBeNil)
+
+		// test.That(t, timestamp1.After(_zeroTime), test.ShouldBeTrue)
+		// test.That(t, timestamp2.After(timestamp1), test.ShouldBeTrue)
+
+		test.That(t, svc.Close(context.Background()), test.ShouldBeNil)
+	})
 }
 
 func TestDataProcess(t *testing.T) {

From bc1c1499c867e00d8ee8921f37b6e63bbc0fa58f Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 10:50:59 -0400
Subject: [PATCH 19/32] lint

---
 cartofacade/capi.go                | 8 --------
 viam-cartographer.go               | 1 -
 viam-cartographer_internal_test.go | 1 -
 viam-cartographer_test.go          | 3 +--
 4 files changed, 1 insertion(+), 12 deletions(-)

diff --git a/cartofacade/capi.go b/cartofacade/capi.go
index 2c3f12ae..e74742a2 100644
--- a/cartofacade/capi.go
+++ b/cartofacade/capi.go
@@ -29,7 +29,6 @@ import "C"
 
 import (
 	"errors"
-	"fmt"
 	"time"
 	"unsafe"
 )
@@ -162,7 +161,6 @@ func NewCarto(cfg CartoConfig, acfg CartoAlgoConfig, vcl CartoLibInterface) (Car
 
 // Start is a wrapper for viam_carto_start
 func (vc *Carto) start() error {
-  fmt.Printf("start: %p\n", vc)
 	status := C.viam_carto_start(vc.value)
 
 	if err := toError(status); err != nil {
@@ -174,7 +172,6 @@ func (vc *Carto) start() error {
 
 // Stop is a wrapper for viam_carto_stop
 func (vc *Carto) stop() error {
-  fmt.Printf("stop: %p\n", vc)
 	status := C.viam_carto_stop(vc.value)
 
 	if err := toError(status); err != nil {
@@ -186,7 +183,6 @@ func (vc *Carto) stop() error {
 
 // Terminate calls viam_carto_terminate to clean up memory for viam carto
 func (vc *Carto) terminate() error {
-  fmt.Printf("terminate: %p\n", vc)
 	status := C.viam_carto_terminate(&vc.value)
 
 	if err := toError(status); err != nil {
@@ -198,7 +194,6 @@ func (vc *Carto) terminate() error {
 
 // AddSensorReading is a wrapper for viam_carto_add_sensor_reading
 func (vc *Carto) addSensorReading(sensor string, readings []byte, timestamp time.Time) error {
-  fmt.Printf("addSensorReading: %p\n", vc)
 	value := toSensorReading(sensor, readings, timestamp)
 
 	status := C.viam_carto_add_sensor_reading(vc.value, &value)
@@ -217,7 +212,6 @@ func (vc *Carto) addSensorReading(sensor string, readings []byte, timestamp time
 
 // GetPosition is a wrapper for viam_carto_get_position
 func (vc *Carto) getPosition() (GetPosition, error) {
-  fmt.Printf("getPosition: %p\n", vc)
 	value := C.viam_carto_get_position_response{}
 
 	status := C.viam_carto_get_position(vc.value, &value)
@@ -238,7 +232,6 @@ func (vc *Carto) getPosition() (GetPosition, error) {
 
 // GetPointCloudMap is a wrapper for viam_carto_get_point_cloud_map
 func (vc *Carto) getPointCloudMap() ([]byte, error) {
-  fmt.Printf("getPointCloudMap: %p\n", vc)
 	// TODO: determine whether or not return needs to be a pointer for performance reasons
 	value := C.viam_carto_get_point_cloud_map_response{}
 
@@ -260,7 +253,6 @@ func (vc *Carto) getPointCloudMap() ([]byte, error) {
 
 // GetInternalState is a wrapper for viam_carto_get_internal_state
 func (vc *Carto) getInternalState() ([]byte, error) {
-  fmt.Printf("getInternalState: %p\n", vc)
 	value := C.viam_carto_get_internal_state_response{}
 
 	status := C.viam_carto_get_internal_state(vc.value, &value)
diff --git a/viam-cartographer.go b/viam-cartographer.go
index 8fbc28cc..adac5c8c 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -177,7 +177,6 @@ func New(
 		defaultMapRateSec,
 		logger,
 	)
-
 	if err != nil {
 		return nil, err
 	}
diff --git a/viam-cartographer_internal_test.go b/viam-cartographer_internal_test.go
index 489f06bc..f596549c 100644
--- a/viam-cartographer_internal_test.go
+++ b/viam-cartographer_internal_test.go
@@ -484,4 +484,3 @@ func TestParseCartoAlgoConfig(t *testing.T) {
 		test.That(t, cartoAlgoConfig, test.ShouldResemble, defaultCartoAlgoCfg)
 	})
 }
-
diff --git a/viam-cartographer_test.go b/viam-cartographer_test.go
index add73a08..d16191b6 100644
--- a/viam-cartographer_test.go
+++ b/viam-cartographer_test.go
@@ -19,6 +19,7 @@ import (
 	"github.com/pkg/errors"
 	viamgrpc "go.viam.com/rdk/grpc"
 	"go.viam.com/test"
+	"go.viam.com/utils/artifact"
 	"google.golang.org/grpc"
 
 	viamcartographer "github.com/viamrobotics/viam-cartographer"
@@ -27,7 +28,6 @@ import (
 	internaltesthelper "github.com/viamrobotics/viam-cartographer/internal/testhelper"
 	"github.com/viamrobotics/viam-cartographer/sensors/lidar"
 	"github.com/viamrobotics/viam-cartographer/testhelper"
-	"go.viam.com/utils/artifact"
 )
 
 const (
@@ -73,7 +73,6 @@ func initInternalState(t *testing.T) (string, func()) {
 		err := os.RemoveAll(dataDirectory)
 		test.That(t, err, test.ShouldBeNil)
 	}
-
 }
 
 func TestNew(t *testing.T) {

From ad23318d6e93f84bcac998dcc9ca3d1266b3f090 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 10:52:05 -0400
Subject: [PATCH 20/32] fix lint

---
 integration_test.go       | 6 +++---
 viam-cartographer_test.go | 3 ++-
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/integration_test.go b/integration_test.go
index ee795dd2..9854778a 100644
--- a/integration_test.go
+++ b/integration_test.go
@@ -415,6 +415,6 @@ func testCartographerDir(t *testing.T, path string, expectedMaps int) {
 	test.That(t, len(mapsInDir), test.ShouldBeGreaterThanOrEqualTo, expectedMaps)
 }
 
-// func TestCartographerIntegration2D(t *testing.T) {
-// 	integrationtestHelperCartographer(t, viamcartographer.Dim2d)
-// }
+func TestCartographerIntegration2D(t *testing.T) {
+	integrationtestHelperCartographer(t, viamcartographer.Dim2d)
+}
diff --git a/viam-cartographer_test.go b/viam-cartographer_test.go
index d16191b6..11339e47 100644
--- a/viam-cartographer_test.go
+++ b/viam-cartographer_test.go
@@ -61,7 +61,8 @@ func initInternalState(t *testing.T) (string, func()) {
 	err = os.Mkdir(internalStateDir, os.ModePerm)
 	test.That(t, err, test.ShouldBeNil)
 
-	internalState, err := os.ReadFile(artifact.MustPath("viam-cartographer/outputs/viam-office-02-22-3/internal_state/internal_state_0.pbstream"))
+	file := "viam-cartographer/outputs/viam-office-02-22-3/internal_state/internal_state_0.pbstream"
+	internalState, err := os.ReadFile(artifact.MustPath(file))
 	test.That(t, err, test.ShouldBeNil)
 
 	timestamp := time.Date(2006, 1, 2, 15, 4, 5, 999900000, time.UTC)

From 02df80fe22b69bf8dfad96c5e2e64d813b0400b8 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 10:56:35 -0400
Subject: [PATCH 21/32] remove debugging stubs

---
 cartofacade/carto_facade.go      | 20 ++++++-------
 cartofacade/carto_facade_test.go | 50 ++++++++++++++++----------------
 viam-cartographer.go             |  4 ---
 3 files changed, 35 insertions(+), 39 deletions(-)

diff --git a/cartofacade/carto_facade.go b/cartofacade/carto_facade.go
index d408ef91..3440bd21 100644
--- a/cartofacade/carto_facade.go
+++ b/cartofacade/carto_facade.go
@@ -29,7 +29,7 @@ func (cf *CartoFacade) Initialize(ctx context.Context, timeout time.Duration, ac
 		return errors.New("unable to cast response from cartofacade to a carto struct")
 	}
 
-	cf.Carto = &carto
+	cf.carto = &carto
 
 	return nil
 }
@@ -177,7 +177,7 @@ go runtime doesn't spawn multiple OS threads, which would harm performance.
 */
 type CartoFacade struct {
 	cartoLib        CartoLibInterface
-	Carto           CartoInterface
+	carto           CartoInterface
 	cartoConfig     CartoConfig
 	cartoAlgoConfig CartoAlgoConfig
 	requestChan     chan Request
@@ -250,7 +250,7 @@ type Request struct {
 // New instantiates the Cartofacade struct which limits calls into C.
 func New(cartoLib CartoLibInterface, cartoCfg CartoConfig, cartoAlgoCfg CartoAlgoConfig) CartoFacade {
 	return CartoFacade{
-		Carto:           &Carto{},
+		carto:           &Carto{},
 		cartoLib:        cartoLib,
 		cartoConfig:     cartoCfg,
 		cartoAlgoConfig: cartoAlgoCfg,
@@ -267,11 +267,11 @@ func (r *Request) doWork(
 	case initialize:
 		return NewCarto(cf.cartoConfig, cf.cartoAlgoConfig, cf.cartoLib)
 	case start:
-		return nil, cf.Carto.start()
+		return nil, cf.carto.start()
 	case stop:
-		return nil, cf.Carto.stop()
+		return nil, cf.carto.stop()
 	case terminate:
-		return nil, cf.Carto.terminate()
+		return nil, cf.carto.terminate()
 	case addSensorReading:
 		sensor, ok := r.requestParams[sensor].(string)
 		if !ok {
@@ -288,13 +288,13 @@ func (r *Request) doWork(
 			return nil, errors.New("could not cast inputted timestamp to times.Time")
 		}
 
-		return nil, cf.Carto.addSensorReading(sensor, reading, timestamp)
+		return nil, cf.carto.addSensorReading(sensor, reading, timestamp)
 	case position:
-		return cf.Carto.getPosition()
+		return cf.carto.getPosition()
 	case internalState:
-		return cf.Carto.getInternalState()
+		return cf.carto.getInternalState()
 	case pointCloudMap:
-		return cf.Carto.getPointCloudMap()
+		return cf.carto.getPointCloudMap()
 	}
 	return nil, fmt.Errorf("no worktype found for: %v", r.requestType)
 }
diff --git a/cartofacade/carto_facade_test.go b/cartofacade/carto_facade_test.go
index 1750b367..d4f44f1a 100644
--- a/cartofacade/carto_facade_test.go
+++ b/cartofacade/carto_facade_test.go
@@ -39,7 +39,7 @@ func TestRequest(t *testing.T) {
 		}
 
 		cf := New(&cartoLib, config, algoConfig)
-		cf.Carto = &carto
+		cf.carto = &carto
 		cf.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 		res, err := cf.request(cancelCtx, position, map[RequestParamType]interface{}{}, 5*time.Second)
@@ -66,7 +66,7 @@ func TestRequest(t *testing.T) {
 		}
 
 		cf := New(&cartoLib, config, algoConfig)
-		cf.Carto = &carto
+		cf.carto = &carto
 		cf.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 		_, err = cf.request(cancelCtx, start, map[RequestParamType]interface{}{}, 5*time.Second)
@@ -92,7 +92,7 @@ func TestRequest(t *testing.T) {
 		}
 
 		cf := New(&cartoLib, config, algoConfig)
-		cf.Carto = &carto
+		cf.carto = &carto
 		cf.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 		cancelFunc()
 		activeBackgroundWorkers.Wait()
@@ -121,7 +121,7 @@ func TestRequest(t *testing.T) {
 		}
 
 		cf := New(&cartoLib, config, algoConfig)
-		cf.Carto = &carto
+		cf.carto = &carto
 		cf.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 		_, err = cf.request(cancelCtx, start, map[RequestParamType]interface{}{}, 10*time.Millisecond)
@@ -180,7 +180,7 @@ func TestStart(t *testing.T) {
 	carto.StartFunc = func() error {
 		return nil
 	}
-	cartoFacade.Carto = &carto
+	cartoFacade.carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing Start", func(t *testing.T) {
@@ -191,7 +191,7 @@ func TestStart(t *testing.T) {
 		carto.StartFunc = func() error {
 			return errors.New("test error 1")
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// returns error
 		err = cartoFacade.Start(cancelCtx, 5*time.Second)
@@ -202,7 +202,7 @@ func TestStart(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return nil
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// times out
 		err = cartoFacade.Start(cancelCtx, 1*time.Millisecond)
@@ -231,7 +231,7 @@ func TestStop(t *testing.T) {
 	carto.StopFunc = func() error {
 		return nil
 	}
-	cartoFacade.Carto = &carto
+	cartoFacade.carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing Stop", func(t *testing.T) {
@@ -242,7 +242,7 @@ func TestStop(t *testing.T) {
 		carto.StopFunc = func() error {
 			return errors.New("test error 2")
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// returns error
 		err = cartoFacade.Stop(cancelCtx, 5*time.Second)
@@ -253,7 +253,7 @@ func TestStop(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return nil
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// times out
 		err = cartoFacade.Stop(cancelCtx, 1*time.Millisecond)
@@ -282,7 +282,7 @@ func TestTerminate(t *testing.T) {
 	carto.TerminateFunc = func() error {
 		return nil
 	}
-	cartoFacade.Carto = &carto
+	cartoFacade.carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing Terminate", func(t *testing.T) {
@@ -293,7 +293,7 @@ func TestTerminate(t *testing.T) {
 		carto.TerminateFunc = func() error {
 			return errors.New("test error 3")
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// returns error
 		err = cartoFacade.Terminate(cancelCtx, 5*time.Second)
@@ -304,7 +304,7 @@ func TestTerminate(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return nil
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// times out
 		err = cartoFacade.Terminate(cancelCtx, 1*time.Millisecond)
@@ -333,7 +333,7 @@ func TestAddSensorReading(t *testing.T) {
 	carto.AddSensorReadingFunc = func(name string, reading []byte, time time.Time) error {
 		return nil
 	}
-	cartoFacade.Carto = &carto
+	cartoFacade.carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing AddSensorReading", func(t *testing.T) {
@@ -355,7 +355,7 @@ func TestAddSensorReading(t *testing.T) {
 		carto.AddSensorReadingFunc = func(name string, reading []byte, time time.Time) error {
 			return errors.New("test error 4")
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// returns error
 		err = cartoFacade.AddSensorReading(cancelCtx, 5*time.Second, "mysensor", buf.Bytes(), timestamp)
@@ -366,7 +366,7 @@ func TestAddSensorReading(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return nil
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// times out
 		err = cartoFacade.AddSensorReading(cancelCtx, 1*time.Millisecond, "mysensor", buf.Bytes(), timestamp)
@@ -396,7 +396,7 @@ func TestGetPosition(t *testing.T) {
 		pos := GetPosition{X: 1, Y: 2, Z: 3}
 		return pos, nil
 	}
-	cartoFacade.Carto = &carto
+	cartoFacade.carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing GetPosition", func(t *testing.T) {
@@ -410,7 +410,7 @@ func TestGetPosition(t *testing.T) {
 		carto.GetPositionFunc = func() (GetPosition, error) {
 			return GetPosition{}, errors.New("test error 5")
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// returns error
 		_, err = cartoFacade.GetPosition(cancelCtx, 5*time.Second)
@@ -421,7 +421,7 @@ func TestGetPosition(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return GetPosition{}, nil
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// times out
 		_, err = cartoFacade.GetPosition(cancelCtx, 1*time.Millisecond)
@@ -451,7 +451,7 @@ func TestGetInternalState(t *testing.T) {
 		internalState := []byte("hello!")
 		return internalState, nil
 	}
-	cartoFacade.Carto = &carto
+	cartoFacade.carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing GetInternalState", func(t *testing.T) {
@@ -463,7 +463,7 @@ func TestGetInternalState(t *testing.T) {
 		carto.GetInternalStateFunc = func() ([]byte, error) {
 			return []byte{}, errors.New("test error 6")
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// returns error
 		_, err = cartoFacade.GetInternalState(cancelCtx, 5*time.Second)
@@ -474,7 +474,7 @@ func TestGetInternalState(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return []byte{}, nil
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// times out
 		_, err = cartoFacade.GetInternalState(cancelCtx, 1*time.Millisecond)
@@ -504,7 +504,7 @@ func TestGetPointCloudMap(t *testing.T) {
 		internalState := []byte("hello!")
 		return internalState, nil
 	}
-	cartoFacade.Carto = &carto
+	cartoFacade.carto = &carto
 	cartoFacade.startCGoroutine(cancelCtx, &activeBackgroundWorkers)
 
 	t.Run("testing GetPointCloudMap", func(t *testing.T) {
@@ -516,7 +516,7 @@ func TestGetPointCloudMap(t *testing.T) {
 		carto.GetPointCloudMapFunc = func() ([]byte, error) {
 			return []byte{}, errors.New("test error 7")
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// returns error
 		_, err = cartoFacade.GetPointCloudMap(cancelCtx, 5*time.Second)
@@ -527,7 +527,7 @@ func TestGetPointCloudMap(t *testing.T) {
 			time.Sleep(50 * time.Millisecond)
 			return []byte{}, nil
 		}
-		cartoFacade.Carto = &carto
+		cartoFacade.carto = &carto
 
 		// times out
 		_, err = cartoFacade.GetPointCloudMap(cancelCtx, 1*time.Millisecond)
diff --git a/viam-cartographer.go b/viam-cartographer.go
index adac5c8c..b2f8eb05 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -131,7 +131,6 @@ func initSensorProcess(cancelCtx context.Context, cartoSvc *cartographerService)
 		Logger:           cartoSvc.logger,
 		TelemetryEnabled: cartoSvc.logger.Level() == zapcore.DebugLevel,
 	}
-	cartoSvc.logger.Warnf("cartoSvc.cartofacade.Carto: %p", &cartoSvc.cartofacade.Carto)
 
 	cartoSvc.sensorProcessWorkers.Add(1)
 	go func() {
@@ -255,7 +254,6 @@ func New(
 			return nil, err
 		}
 
-		cartoSvc.logger.Warnf("before sensor process: cartoSvc.cartofacade.Carto: %p", &cartoSvc.cartofacade.Carto)
 		initSensorProcess(cancelSensorProcessCtx, cartoSvc)
 		success = true
 		return cartoSvc, err
@@ -394,9 +392,7 @@ func initCartoFacade(ctx context.Context, cartoSvc *cartographerService) error {
 		return err
 	}
 
-	cartoSvc.logger.Warnf("cf.Carto: %p", cf.Carto)
 	cartoSvc.cartofacade = &cf
-	cartoSvc.logger.Warnf("cartoSvc.cartofacade.Carto: %p", cartoSvc.cartofacade.Carto)
 
 	return nil
 }

From 62223bc22f213e1ac9ef9840665e0384a554735b Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 11:13:28 -0400
Subject: [PATCH 22/32] wip

---
 viam-cartographer.go | 15 ++++-----------
 1 file changed, 4 insertions(+), 11 deletions(-)

diff --git a/viam-cartographer.go b/viam-cartographer.go
index b2f8eb05..217ae7a1 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -166,9 +166,6 @@ func New(
 			c.Model.Name, svcConfig.ConfigParams["mode"])
 	}
 
-	// Set up the data directories
-	// TODO: Figure out how to not do this if v2 is enabled
-
 	port, dataRateMsec, mapRateSec, useLiveData, deleteProcessedData, modularizationV2Enabled, err := vcConfig.GetOptionalParameters(
 		svcConfig,
 		localhost0,
@@ -227,10 +224,8 @@ func New(
 		localizationMode:              mapRateSec == 0,
 		mapTimestamp:                  time.Now().UTC(),
 	}
-	success := false
 	defer func() {
-		// TODO: Change this to use err != nil
-		if !success {
+		if err != nil {
 			logger.Errorw("New() hit error, closing...", "error", err)
 			if err := cartoSvc.Close(ctx); err != nil {
 				logger.Errorw("error closing out after error", "error", err)
@@ -239,13 +234,13 @@ func New(
 	}()
 
 	if modularizationV2Enabled {
-		if err := dim2d.ValidateGetData(
+		if err = dim2d.ValidateGetData(
 			cancelSensorProcessCtx,
 			cartoSvc.lidar,
 			time.Duration(sensorValidationMaxTimeoutSec)*time.Second,
 			time.Duration(cartoSvc.sensorValidationIntervalSec)*time.Second,
 			cartoSvc.logger); err != nil {
-			err := errors.Wrap(err, "failed to get data from lidar")
+			err = errors.Wrap(err, "failed to get data from lidar")
 			return nil, err
 		}
 
@@ -255,15 +250,13 @@ func New(
 		}
 
 		initSensorProcess(cancelSensorProcessCtx, cartoSvc)
-		success = true
-		return cartoSvc, err
+		return cartoSvc, nil
 	}
 
 	err = initCartoGrpcServer(ctx, cancelCtx, cartoSvc)
 	if err != nil {
 		return nil, err
 	}
-	success = true
 	return cartoSvc, nil
 }
 

From 8ba1d3cef61f56cdeb14d38af9410bb0db3a824e Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 11:18:51 -0400
Subject: [PATCH 23/32] wip

---
 viam-cartographer.go | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/viam-cartographer.go b/viam-cartographer.go
index 217ae7a1..d75967d3 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -260,7 +260,6 @@ func New(
 	return cartoSvc, nil
 }
 
-// TODO: write a test for this.
 func parseCartoAlgoConfig(configParams map[string]string, logger golog.Logger) (cartofacade.CartoAlgoConfig, error) {
 	cartoAlgoCfg := defaultCartoAlgoCfg
 	for k, val := range configParams {
@@ -390,9 +389,6 @@ func initCartoFacade(ctx context.Context, cartoSvc *cartographerService) error {
 	return nil
 }
 
-// terminateCartoFacade
-// 1. stops & terminates carto facade
-// TODO: How does this work if we are closing down the context which manages the carto facade?
 func terminateCartoFacade(ctx context.Context, cartoSvc *cartographerService) error {
 	if cartoSvc.cartofacade == nil {
 		cartoSvc.logger.Debug("terminateCartoFacade called when cartoSvc.cartofacade is nil")

From 98dc6ee32204b975485f88cd61045b2f8eeef2ec Mon Sep 17 00:00:00 2001
From: nicksanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 12:16:54 -0400
Subject: [PATCH 24/32] Update internal/testhelper/testhelper.go

Co-authored-by: kim-mishra <121991867+kim-mishra@users.noreply.github.com>
---
 internal/testhelper/testhelper.go | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/internal/testhelper/testhelper.go b/internal/testhelper/testhelper.go
index 31c0413a..4a9721b1 100644
--- a/internal/testhelper/testhelper.go
+++ b/internal/testhelper/testhelper.go
@@ -24,7 +24,7 @@ import (
 )
 
 const (
-	// SensorValidationMaxTimeoutSecForTest is used in the ValidateGetAndSaveDa
+	// SensorValidationMaxTimeoutSecForTest is used in the ValidateGetAndSaveData
 	// function to ensure that the sensor in the GetAndSaveData function
 	// returns data within an acceptable time.
 	SensorValidationMaxTimeoutSecForTest = 1

From c57dce6fd16fc93e405c2788502cf3b292341d59 Mon Sep 17 00:00:00 2001
From: nicksanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 12:17:09 -0400
Subject: [PATCH 25/32] Update sensors/lidar/dim-2d/dim-2d.go

Co-authored-by: kim-mishra <121991867+kim-mishra@users.noreply.github.com>
---
 sensors/lidar/dim-2d/dim-2d.go | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sensors/lidar/dim-2d/dim-2d.go b/sensors/lidar/dim-2d/dim-2d.go
index 96456db2..00ff3a11 100644
--- a/sensors/lidar/dim-2d/dim-2d.go
+++ b/sensors/lidar/dim-2d/dim-2d.go
@@ -56,7 +56,7 @@ func NewLidar(
 }
 
 // GetTimedData returns a 2d lidar reading, the timestamp from when it wask taken
-// (eitehr in live mode or offline mode)
+// (either in live mode or offline mode)
 // and an error if an error occurred getting the lidar data or parsing the offline
 // timestamp.
 func GetTimedData(ctx context.Context, lidar lidar.Lidar) (time.Time, pointcloud.PointCloud, error) {

From a35cd37c01739679fc532e8bc1b1fb2b2d64cdbf Mon Sep 17 00:00:00 2001
From: nicksanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 12:17:19 -0400
Subject: [PATCH 26/32] Update testhelper/testhelper.go

Co-authored-by: kim-mishra <121991867+kim-mishra@users.noreply.github.com>
---
 testhelper/testhelper.go | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/testhelper/testhelper.go b/testhelper/testhelper.go
index 7e790d99..f1c9a8f9 100644
--- a/testhelper/testhelper.go
+++ b/testhelper/testhelper.go
@@ -63,7 +63,7 @@ func getWarmingUpLidar() *inject.Camera {
 	cam := &inject.Camera{}
 	couter := 0
 	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
-		couter++
+		counter++
 		if couter == 1 {
 			return nil, errors.Errorf("warming up %d", couter)
 		}

From 63619eca4a64898171c4a65bb6040e81245a4df9 Mon Sep 17 00:00:00 2001
From: nicksanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 12:17:27 -0400
Subject: [PATCH 27/32] Update testhelper/testhelper.go

Co-authored-by: kim-mishra <121991867+kim-mishra@users.noreply.github.com>
---
 testhelper/testhelper.go | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/testhelper/testhelper.go b/testhelper/testhelper.go
index f1c9a8f9..ffb8be68 100644
--- a/testhelper/testhelper.go
+++ b/testhelper/testhelper.go
@@ -64,7 +64,7 @@ func getWarmingUpLidar() *inject.Camera {
 	couter := 0
 	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
 		counter++
-		if couter == 1 {
+		if counter == 1 {
 			return nil, errors.Errorf("warming up %d", couter)
 		}
 		return pointcloud.New(), nil

From 4f2677c89cbd00f78d4883f07c83d2f8d0e57b83 Mon Sep 17 00:00:00 2001
From: nicksanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 12:17:35 -0400
Subject: [PATCH 28/32] Update testhelper/testhelper.go

Co-authored-by: kim-mishra <121991867+kim-mishra@users.noreply.github.com>
---
 testhelper/testhelper.go | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/testhelper/testhelper.go b/testhelper/testhelper.go
index ffb8be68..41fd70f9 100644
--- a/testhelper/testhelper.go
+++ b/testhelper/testhelper.go
@@ -65,7 +65,7 @@ func getWarmingUpLidar() *inject.Camera {
 	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
 		counter++
 		if counter == 1 {
-			return nil, errors.Errorf("warming up %d", couter)
+			return nil, errors.Errorf("warming up %d", counter)
 		}
 		return pointcloud.New(), nil
 	}

From e37e9d97a7e56fc19d08333e11d23266291d9b72 Mon Sep 17 00:00:00 2001
From: nicksanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 12:17:41 -0400
Subject: [PATCH 29/32] Update testhelper/testhelper.go

Co-authored-by: kim-mishra <121991867+kim-mishra@users.noreply.github.com>
---
 testhelper/testhelper.go | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/testhelper/testhelper.go b/testhelper/testhelper.go
index 41fd70f9..679922e6 100644
--- a/testhelper/testhelper.go
+++ b/testhelper/testhelper.go
@@ -61,7 +61,7 @@ func SetupDeps(sensors []string) resource.Dependencies {
 
 func getWarmingUpLidar() *inject.Camera {
 	cam := &inject.Camera{}
-	couter := 0
+	counter := 0
 	cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
 		counter++
 		if counter == 1 {

From 23f7ea162108bb8081264e90bd28e5693c2ad8ce Mon Sep 17 00:00:00 2001
From: nicksanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 12:19:11 -0400
Subject: [PATCH 30/32] Update viam-cartographer.go

Co-authored-by: kim-mishra <121991867+kim-mishra@users.noreply.github.com>
---
 viam-cartographer.go | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/viam-cartographer.go b/viam-cartographer.go
index d75967d3..f9ef59ad 100644
--- a/viam-cartographer.go
+++ b/viam-cartographer.go
@@ -633,7 +633,7 @@ func (cartoSvc *cartographerService) Close(ctx context.Context) error {
 		cartoSvc.cancelSensorProcessFunc()
 		cartoSvc.sensorProcessWorkers.Wait()
 
-		// termintae carto facade
+		// terminate carto facade
 		err := terminateCartoFacade(ctx, cartoSvc)
 		if err != nil {
 			cartoSvc.logger.Errorw("close hit error", "error", err)

From edfb2d7b587257bd4b0301a50438fea00707c1dc Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 12:29:46 -0400
Subject: [PATCH 31/32] fix build & respond to feedback

---
 Makefile       | 10 ++++++----
 module/main.go |  6 +++++-
 2 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/Makefile b/Makefile
index cab20f79..7744714f 100644
--- a/Makefile
+++ b/Makefile
@@ -95,9 +95,14 @@ else
 	$(error "Unsupported system. Only apt and brew currently supported.")
 endif
 
-build: ensure-submodule-initialized grpc/buf build-module
+build: bin/cartographer-module
+
+viam-cartographer/build/carto_grpc_server: ensure-submodule-initialized grpc/buf
 	cd viam-cartographer && cmake -Bbuild -G Ninja ${EXTRA_CMAKE_FLAGS} && cmake --build build
 
+bin/cartographer-module: viam-cartographer/build/carto_grpc_server
+	mkdir -p bin && go build $(GO_BUILD_LDFLAGS) -o bin/cartographer-module module/main.go
+
 # Ideally build-asan would be added to build-debug, but can't yet 
 # as these options they fail on arm64 linux. This is b/c that 
 # platform currently uses gcc as opposed to clang & gcc doesn't
@@ -111,9 +116,6 @@ build-asan: build-debug
 build-debug: EXTRA_CMAKE_FLAGS += -DCMAKE_BUILD_TYPE=Debug -DFORCE_DEBUG_BUILD=True
 build-debug: build
 
-build-module:
-	mkdir -p bin && go build $(GO_BUILD_LDFLAGS) -o bin/cartographer-module module/main.go
-
 test-cpp:
 	viam-cartographer/build/unit_tests -p -l all
 
diff --git a/module/main.go b/module/main.go
index ec066cad..29aed2c2 100644
--- a/module/main.go
+++ b/module/main.go
@@ -44,7 +44,11 @@ func mainWithArgs(ctx context.Context, args []string, logger golog.Logger) error
 	if err := viamcartographer.InitCartoLib(logger); err != nil {
 		return err
 	}
-	defer utils.UncheckedErrorFunc(viamcartographer.TerminateCartoLib)
+
+	defer func() {
+		err := viamcartographer.TerminateCartoLib()
+		logger.Errorw("failed to terminate carto lib", "error", err)
+	}()
 
 	// Instantiate the module
 	cartoModule, err := module.NewModuleFromArgs(ctx, logger)

From 3b7930b295654ddd7fd1522a2bfe554efeb29e41 Mon Sep 17 00:00:00 2001
From: Nick Sanford <nick.sanford@viam.com>
Date: Tue, 11 Jul 2023 13:21:30 -0400
Subject: [PATCH 32/32] lint

---
 module/main.go | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/module/main.go b/module/main.go
index 29aed2c2..f35b84bd 100644
--- a/module/main.go
+++ b/module/main.go
@@ -46,8 +46,9 @@ func mainWithArgs(ctx context.Context, args []string, logger golog.Logger) error
 	}
 
 	defer func() {
-		err := viamcartographer.TerminateCartoLib()
-		logger.Errorw("failed to terminate carto lib", "error", err)
+		if err := viamcartographer.TerminateCartoLib(); err != nil {
+			logger.Errorw("failed to terminate carto lib", "error", err)
+		}
 	}()
 
 	// Instantiate the module