RSDK-4110 IMU Data Integration (#255)

cartofacade/capi.go

@@ -17,6 +17,9 @@ import (
 	"errors"
 	"time"
 	"unsafe"
+
+	"github.com/golang/geo/r3"
+	"go.viam.com/rdk/spatialmath"
 )
 
 // CartoLib holds the c type viam_carto_lib
@@ -55,6 +58,7 @@ type CartoInterface interface {
 	stop() error
 	terminate() error
 	addLidarReading(string, []byte, time.Time) error
+	addIMUReading(string, IMUReading, time.Time) error
 	getPosition() (GetPosition, error)
 	getPointCloudMap() ([]byte, error)
 	getInternalState() ([]byte, error)
@@ -75,6 +79,12 @@ type GetPosition struct {
 	ComponentReference string
 }
 
+// IMUReading holds values for linear acceleration and angular velocity to be converted into c
+type IMUReading struct {
+	LinearAcceleration r3.Vector
+	AngularVelocity    spatialmath.AngularVelocity
+}
+
 // LidarConfig represents the lidar configuration
 type LidarConfig int64
 
@@ -107,6 +117,7 @@ type CartoAlgoConfig struct {
 	MissingDataRayLength float32
 	MaxRange             float32
 	MinRange             float32
+	UseIMUData           bool
 	MaxSubmapsToKeep     int
 	FreshSubmapsCount    int
 	MinCoveredArea       float64
@@ -208,7 +219,7 @@ func (vc *Carto) terminate() error {
 	return nil
 }
 
-// AddLidarReading is a wrapper for viam_carto_add_lidar_reading
+// addLidarReading is a wrapper for viam_carto_add_lidar_reading
 func (vc *Carto) addLidarReading(lidar string, readings []byte, timestamp time.Time) error {
 	value := toLidarReading(lidar, readings, timestamp)
 
@@ -226,6 +237,24 @@ func (vc *Carto) addLidarReading(lidar string, readings []byte, timestamp time.T
 	return nil
 }
 
+// addIMUReading is a wrapper for viam_carto_add_imu_reading
+func (vc *Carto) addIMUReading(imu string, readings IMUReading, timestamp time.Time) error {
+	value := toIMUReading(imu, readings, timestamp)
+
+	status := C.viam_carto_add_imu_reading(vc.value, &value)
+
+	if err := toError(status); err != nil {
+		return err
+	}
+
+	status = C.viam_carto_add_imu_reading_destroy(&value)
+	if err := toError(status); err != nil {
+		return err
+	}
+
+	return nil
+}
+
 // GetPosition is a wrapper for viam_carto_get_position
 func (vc *Carto) getPosition() (GetPosition, error) {
 	value := C.viam_carto_get_position_response{}
@@ -366,6 +395,7 @@ func toAlgoConfig(acfg CartoAlgoConfig) C.viam_carto_algo_config {
 	vcac.missing_data_ray_length = C.float(acfg.MissingDataRayLength)
 	vcac.max_range = C.float(acfg.MaxRange)
 	vcac.min_range = C.float(acfg.MinRange)
+	vcac.use_imu_data = C.bool(acfg.UseIMUData)
 	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)
@@ -403,6 +433,23 @@ func toLidarReading(lidar string, readings []byte, timestamp time.Time) C.viam_c
 	return sr
 }
 
+func toIMUReading(imu string, readings IMUReading, timestamp time.Time) C.viam_carto_imu_reading {
+	sr := C.viam_carto_imu_reading{}
+	sensorCStr := C.CString(imu)
+	defer C.free(unsafe.Pointer(sensorCStr))
+	sr.imu = C.blk2bstr(unsafe.Pointer(sensorCStr), C.int(len(imu)))
+
+	sr.lin_acc_x = C.double(readings.LinearAcceleration.X)
+	sr.lin_acc_y = C.double(readings.LinearAcceleration.Y)
+	sr.lin_acc_z = C.double(readings.LinearAcceleration.Z)
+	sr.ang_vel_x = C.double(readings.AngularVelocity.X)
+	sr.ang_vel_y = C.double(readings.AngularVelocity.Y)
+	sr.ang_vel_z = C.double(readings.AngularVelocity.Z)
+
+	sr.imu_reading_time_unix_milli = C.int64_t(timestamp.UnixMilli())
+	return sr
+}
+
 func bstringToByteSlice(bstr C.bstring) []byte {
 	return C.GoBytes(unsafe.Pointer(bstr.data), bstr.slen)
 }
@@ -445,8 +492,8 @@ func toError(status C.int) error {
 		return errors.New("VIAM_CARTO_DATA_DIR_FILE_SYSTEM_ERROR")
 	case C.VIAM_CARTO_MAP_CREATION_ERROR:
 		return errors.New("VIAM_CARTO_MAP_CREATION_ERROR")
-	case C.VIAM_CARTO_SENSOR_NOT_IN_SENSOR_LIST:
-		return errors.New("VIAM_CARTO_SENSOR_NOT_IN_SENSOR_LIST")
+	case C.VIAM_CARTO_UNKNOWN_SENSOR_NAME:
+		return errors.New("VIAM_CARTO_UNKNOWN_SENSOR_NAME")
 	case C.VIAM_CARTO_LIDAR_READING_EMPTY:
 		return errors.New("VIAM_CARTO_LIDAR_READING_EMPTY")
 	case C.VIAM_CARTO_LIDAR_READING_INVALID:
@@ -455,12 +502,12 @@ func toError(status C.int) error {
 		return errors.New("VIAM_CARTO_GET_POSITION_RESPONSE_INVALID")
 	case C.VIAM_CARTO_POINTCLOUD_MAP_EMPTY:
 		return errors.New("VIAM_CARTO_POINTCLOUD_MAP_EMPTY")
-	case C.VIAM_CARTO_GET_POINT_CLOUD_MAP_RESPONSE_INVLALID:
-		return errors.New("VIAM_CARTO_GET_POINT_CLOUD_MAP_RESPONSE_INVLALID")
+	case C.VIAM_CARTO_GET_POINT_CLOUD_MAP_RESPONSE_INVALID:
+		return errors.New("VIAM_CARTO_GET_POINT_CLOUD_MAP_RESPONSE_INVALID")
 	case C.VIAM_CARTO_LIB_ALREADY_INITIALIZED:
 		return errors.New("VIAM_CARTO_LIB_ALREADY_INITIALIZED")
-	case C.VIAM_CARTO_GET_INTERNAL_STATE_RESPONSE_INVLALID:
-		return errors.New("VIAM_CARTO_GET_INTERNAL_STATE_RESPONSE_INVLALID")
+	case C.VIAM_CARTO_GET_INTERNAL_STATE_RESPONSE_INVALID:
+		return errors.New("VIAM_CARTO_GET_INTERNAL_STATE_RESPONSE_INVALID")
 	case C.VIAM_CARTO_GET_INTERNAL_STATE_FILE_WRITE_IO_ERROR:
 		return errors.New("VIAM_CARTO_GET_INTERNAL_STATE_FILE_WRITE_IO_ERROR")
 	case C.VIAM_CARTO_GET_INTERNAL_STATE_FILE_READ_IO_ERROR:
@@ -473,6 +520,10 @@ func toError(status C.int) error {
 		return errors.New("VIAM_CARTO_NOT_IN_STARTED_STATE")
 	case C.VIAM_CARTO_NOT_IN_TERMINATABLE_STATE:
 		return errors.New("VIAM_CARTO_NOT_IN_TERMINATABLE_STATE")
+	case C.VIAM_CARTO_IMU_PROVIDED_AND_IMU_ENABLED_MISMATCH:
+		return errors.New("VIAM_CARTO_IMU_PROVIDED_AND_IMU_ENABLED_MISMATCH")
+	case C.VIAM_CARTO_IMU_READING_INVALID:
+		return errors.New("VIAM_CARTO_IMU_READING_INVALID")
 	default:
 		return errors.New("status code unclassified")
 	}

cartofacade/capi_mock.go

@@ -26,6 +26,7 @@ type CartoMock struct {
 	StopFunc                 func() error
 	TerminateFunc            func() error
 	AddLidarReadingFunc      func(string, []byte, time.Time) error
+	AddIMUReadingFunc        func(string, IMUReading, time.Time) error
 	GetPositionFunc          func() (GetPosition, error)
 	GetPointCloudMapFunc     func() ([]byte, error)
 	GetInternalStateFunc     func() ([]byte, error)
@@ -56,14 +57,22 @@ func (cf *CartoMock) terminate() error {
 	return cf.TerminateFunc()
 }
 
-// AddLidarReading calls the injected AddLidarReadingFunc or the real version.
+// addLidarReading calls the injected AddLidarReadingFunc or the real version.
 func (cf *CartoMock) addLidarReading(lidar string, readings []byte, time time.Time) error {
 	if cf.AddLidarReadingFunc == nil {
 		return cf.Carto.addLidarReading(lidar, readings, time)
 	}
 	return cf.AddLidarReadingFunc(lidar, readings, time)
 }
 
+// addIMUReading calls the injected AddIMUReadingFunc or the real version.
+func (cf *CartoMock) addIMUReading(imu string, readings IMUReading, time time.Time) error {
+	if cf.AddIMUReadingFunc == nil {
+		return cf.Carto.addIMUReading(imu, readings, time)
+	}
+	return cf.AddIMUReadingFunc(imu, readings, time)
+}
+
 // GetPosition calls the injected GetPositionFunc or the real version.
 func (cf *CartoMock) getPosition() (GetPosition, error) {
 	if cf.GetPositionFunc == nil {