config changes

config/config.go

@@ -53,6 +53,7 @@ type Config struct {
 	DataDirectory           string            `json:"data_dir"`
 	UseLiveData             *bool             `json:"use_live_data"`
 	DataRateMsec            int               `json:"data_rate_msec"`
+	IMUDataRateMsec         int               `json:"imu_data_rate_msec"`
 	MapRateSec              *int              `json:"map_rate_sec"`
 	Port                    string            `json:"port"`
 	DeleteProcessedData     *bool             `json:"delete_processed_data"`
@@ -86,6 +87,10 @@ func (config *Config) Validate(path string) ([]string, error) {
 		return nil, errors.New("cannot specify data_rate_msec less than zero")
 	}
 
+	if config.IMUDataRateMsec < 0 {
+		return nil, errors.New("cannot specify imu_data_rate_msec less than zero")
+	}
+
 	if config.MapRateSec != nil && *config.MapRateSec < 0 {
 		return nil, errors.New("cannot specify map_rate_sec less than zero")
 	}
@@ -98,8 +103,8 @@ func (config *Config) Validate(path string) ([]string, error) {
 // GetOptionalParameters sets any unset optional config parameters to the values passed to this function,
 // and returns them.
 func GetOptionalParameters(config *Config, defaultPort string,
-	defaultDataRateMsec, defaultMapRateSec int, logger golog.Logger,
-) (string, int, int, bool, bool, bool, error) {
+	defaultDataRateMsec, defaultIMUDataRateMsec, defaultMapRateSec int, logger golog.Logger,
+) (string, int, int, int, bool, bool, bool, error) {
 	modularizationV2Enabled := false
 	if config.ModularizationV2Enabled != nil {
 		modularizationV2Enabled = *config.ModularizationV2Enabled
@@ -120,6 +125,12 @@ func GetOptionalParameters(config *Config, defaultPort string,
 		logger.Debugf("no data_rate_msec given, setting to default value of %d", defaultDataRateMsec)
 	}
 
+	imuDataRateMsec := config.IMUDataRateMsec
+	if config.IMUDataRateMsec == 0 {
+		imuDataRateMsec = defaultIMUDataRateMsec
+		logger.Debugf("no imu_data_rate_msec given, setting to default value of %d", defaultIMUDataRateMsec)
+	}
+
 	mapRateSec := 0
 	if config.MapRateSec == nil {
 		logger.Debugf("no map_rate_sec given, setting to default value of %d", defaultMapRateSec)
@@ -136,7 +147,7 @@ func GetOptionalParameters(config *Config, defaultPort string,
 	if !modularizationV2Enabled {
 		useLiveData, err = DetermineUseLiveData(logger, config.UseLiveData, config.Sensors)
 		if err != nil {
-			return "", 0, 0, false, false, false, err
+			return "", 0, 0, 0, false, false, false, err
 		}
 	}
 
@@ -146,5 +157,5 @@ func GetOptionalParameters(config *Config, defaultPort string,
 		deleteProcessedData = DetermineDeleteProcessedData(logger, config.DeleteProcessedData, useLiveData)
 	}
 
-	return port, dataRateMsec, mapRateSec, useLiveData, deleteProcessedData, modularizationV2Enabled, nil
+	return port, dataRateMsec, imuDataRateMsec, mapRateSec, deleteProcessedData, useLiveData, modularizationV2Enabled, nil
 }

sensors/imu/imu.go

@@ -0,0 +1,50 @@
+// Package imu implements the IMU sensor.
+package imu
+
+import (
+	"context"
+
+	"github.com/golang/geo/r3"
+	"github.com/pkg/errors"
+	"go.viam.com/rdk/components/movementsensor"
+	"go.viam.com/rdk/resource"
+	"go.viam.com/rdk/spatialmath"
+
+	"github.com/viamrobotics/viam-cartographer/sensors/utils"
+)
+
+// Lidar represents a LIDAR sensor.
+type IMU struct {
+	Name string
+	imu  movementsensor.MovementSensor
+}
+
+// New creates a new IMU sensor based on the sensor definition and the service config.
+func New(deps resource.Dependencies, sensors []string, sensorIndex int) (IMU, error) {
+	name, err := utils.GetName(sensors, sensorIndex)
+	if err != nil {
+		return IMU{}, err
+	}
+
+	newIMU, err := movementsensor.FromDependencies(deps, name)
+	if err != nil {
+		return IMU{}, errors.Wrapf(err, "error getting IMU sensor %v for slam service", name)
+	}
+
+	return IMU{
+		Name: name,
+		imu:  newIMU,
+	}, nil
+}
+
+// GetAngularVelocity returns angular velocity from the IMU sensor.
+func (imu IMU) GetAngularVelocity(ctx context.Context) (spatialmath.AngularVelocity, error) {
+	return imu.imu.AngularVelocity(ctx, make(map[string]interface{}))
+
+}
+
+// GetLinearAcceleration returns linear acceleration from the IMU sensor.
+func (imu IMU) GetLinearAcceleration(ctx context.Context) (r3.Vector, error) {
+	return imu.imu.LinearAcceleration(ctx, make(map[string]interface{}))
+
+}

viam-cartographer.go

@@ -37,6 +37,7 @@ const (
 	// DefaultExecutableName is what this program expects to call to start the cartographer grpc server.
 	DefaultExecutableName                = "carto_grpc_server"
 	defaultDataRateMsec                  = 200
+	defaultIMUDataRateMsec               = 20
 	defaultMapRateSec                    = 60
 	defaultDialMaxTimeoutSec             = 30
 	defaultSensorValidationMaxTimeoutSec = 30
@@ -105,10 +106,11 @@ func New(
 		return nil, err
 	}
 
-	port, dataRateMsec, mapRateSec, useLiveData, deleteProcessedData, _, err := vcConfig.GetOptionalParameters(
+	port, dataRateMsec, imuDataRateMsec, mapRateSec, useLiveData, deleteProcessedData, _, err := vcConfig.GetOptionalParameters(
 		svcConfig,
 		localhost0,
 		defaultDataRateMsec,
+		defaultIMUDataRateMsec,
 		defaultMapRateSec,
 		logger,
 	)
@@ -138,6 +140,7 @@ func New(
 		deleteProcessedData:   deleteProcessedData,
 		port:                  port,
 		dataRateMs:            dataRateMsec,
+		imuDataRateMs:         imuDataRateMsec,
 		mapRateSec:            mapRateSec,
 		cancelFunc:            cancelFunc,
 		logger:                logger,
@@ -195,9 +198,10 @@ type cartographerService struct {
 	deleteProcessedData bool
 	useLiveData         bool
 
-	port       string
-	dataRateMs int
-	mapRateSec int
+	port          string
+	dataRateMs    int
+	imuDataRateMs int
+	mapRateSec    int
 
 	cancelFunc              func()
 	logger                  golog.Logger

viam-cartographer/src/slam_service/config.cc

@@ -15,6 +15,7 @@ using namespace boost::filesystem;
 namespace viam {
 namespace config {
 
+int defaultIMUDataRateMS = 20;
 int defaultDataRateMS = 200;
 int defaultMapRateSec = 60;
 
@@ -25,6 +26,8 @@ DEFINE_string(port, "", "GRPC port");
 DEFINE_string(sensors, "", "Array of sensors.");
 DEFINE_int64(data_rate_ms, defaultDataRateMS,
              "Frequency at which we grab/save data.");
+DEFINE_int64(imu_data_rate_ms, defaultIMUDataRateMS,
+             "Frequency at which we grab/save IMU data.");
 DEFINE_int64(
     map_rate_sec, defaultMapRateSec,
     "Frequency at which we want to print map pictures while cartographer "
@@ -65,6 +68,7 @@ void ParseAndValidateConfigParams(int argc, char** argv,
     LOG(INFO) << "port: " << FLAGS_port;
     LOG(INFO) << "sensors: " << FLAGS_sensors;
     LOG(INFO) << "data_rate_ms: " << FLAGS_data_rate_ms;
+    LOG(INFO) << "imu_data_rate_ms: " << FLAGS_imu_data_rate_ms;
     LOG(INFO) << "map_rate_sec: " << FLAGS_map_rate_sec;
     LOG(INFO) << "delete_processed_data: " << FLAGS_delete_processed_data;
     LOG(INFO) << "use_live_data: " << FLAGS_use_live_data;
@@ -99,6 +103,7 @@ void ParseAndValidateConfigParams(int argc, char** argv,
     slamService.port = FLAGS_port;
     slamService.camera_name = FLAGS_sensors;
     slamService.data_rate_ms = std::chrono::milliseconds(FLAGS_data_rate_ms);
+    slamService.imu_data_rate_ms = std::chrono::milliseconds(FLAGS_imu_data_rate_ms);
     slamService.map_rate_sec = std::chrono::seconds(FLAGS_map_rate_sec);
 
     slamService.delete_processed_data = FLAGS_delete_processed_data;
@@ -223,6 +228,7 @@ void ResetFlagsForTesting() {
     FLAGS_port = "";
     FLAGS_sensors = "";
     FLAGS_data_rate_ms = defaultDataRateMS;
+    FLAGS_imu_data_rate_ms = defaultIMUDataRateMS;
     FLAGS_map_rate_sec = defaultMapRateSec;
     FLAGS_delete_processed_data = false;
     FLAGS_use_live_data = false;