RSDK-4861 - C++/cgo support for Odometer

cartofacade/capi.go

@@ -17,6 +17,9 @@ import (
 	"errors"
 	"unsafe"
 
+	geo "github.com/kellydunn/golang-geo"
+	"go.viam.com/rdk/spatialmath"
+
 	s "github.com/viamrobotics/viam-cartographer/sensors"
 )
 
@@ -57,6 +60,7 @@ type CartoInterface interface {
 	terminate() error
 	addLidarReading(string, s.TimedLidarReadingResponse) error
 	addIMUReading(string, s.TimedIMUReadingResponse) error
+	addOdometerReading(string, s.TimedOdometerReadingResponse) error
 	position() (Position, error)
 	pointCloudMap() ([]byte, error)
 	internalState() ([]byte, error)
@@ -248,6 +252,24 @@ func (vc *Carto) addIMUReading(imu string, reading s.TimedIMUReadingResponse) er
 	return nil
 }
 
+// addOdometerReading is a wrapper for viam_carto_add_odometer_reading
+func (vc *Carto) addOdometerReading(odometer string, reading s.TimedOdometerReadingResponse) error {
+	value := toOdometerReading(odometer, reading)
+
+	status := C.viam_carto_add_odometer_reading(vc.value, &value)
+
+	if err := toError(status); err != nil {
+		return err
+	}
+
+	status = C.viam_carto_add_odometer_reading_destroy(&value)
+	if err := toError(status); err != nil {
+		return err
+	}
+
+	return nil
+}
+
 // position is a wrapper for viam_carto_get_position
 func (vc *Carto) position() (Position, error) {
 	value := C.viam_carto_get_position_response{}
@@ -435,11 +457,11 @@ func toLidarReading(lidar string, reading s.TimedLidarReadingResponse) C.viam_ca
 	return sr
 }
 
-func toIMUReading(imu string, reading s.TimedIMUReadingResponse) C.viam_carto_imu_reading {
+func toIMUReading(movementSensor string, reading s.TimedIMUReadingResponse) C.viam_carto_imu_reading {
 	sr := C.viam_carto_imu_reading{}
-	sensorCStr := C.CString(imu)
+	sensorCStr := C.CString(movementSensor)
 	defer C.free(unsafe.Pointer(sensorCStr))
-	sr.imu = C.blk2bstr(unsafe.Pointer(sensorCStr), C.int(len(imu)))
+	sr.imu = C.blk2bstr(unsafe.Pointer(sensorCStr), C.int(len(movementSensor)))
 
 	sr.lin_acc_x = C.double(reading.LinearAcceleration.X)
 	sr.lin_acc_y = C.double(reading.LinearAcceleration.Y)
@@ -452,6 +474,27 @@ func toIMUReading(imu string, reading s.TimedIMUReadingResponse) C.viam_carto_im
 	return sr
 }
 
+func toOdometerReading(movementSensor string, reading s.TimedOdometerReadingResponse) C.viam_carto_odometer_reading {
+	sr := C.viam_carto_odometer_reading{}
+	sensorCStr := C.CString(movementSensor)
+	defer C.free(unsafe.Pointer(sensorCStr))
+	sr.odometer = C.blk2bstr(unsafe.Pointer(sensorCStr), C.int(len(movementSensor)))
+
+	translation := spatialmath.GeoPointToPose(reading.Position, geo.NewPoint(0, 0)).Point()
+	rotation := reading.Orientation.Quaternion()
+
+	sr.translation_x = C.double(translation.X)
+	sr.translation_y = C.double(translation.Y)
+	sr.translation_z = C.double(translation.Z)
+	sr.rotation_x = C.double(rotation.Imag)
+	sr.rotation_y = C.double(rotation.Jmag)
+	sr.rotation_z = C.double(rotation.Kmag)
+	sr.rotation_w = C.double(rotation.Real)
+
+	sr.odometer_reading_time_unix_milli = C.int64_t(reading.ReadingTime.UnixMilli())
+	return sr
+}
+
 func bstringToByteSlice(bstr C.bstring) []byte {
 	return C.GoBytes(unsafe.Pointer(bstr.data), bstr.slen)
 }
@@ -526,6 +569,8 @@ func toError(status C.int) error {
 		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")
+	case C.VIAM_CARTO_ODOMETER_READING_INVALID:
+		return errors.New("VIAM_CARTO_ODOMETER_READING_INVALID")
 	default:
 		return errors.New("status code unclassified")
 	}

cartofacade/capi_mock.go

@@ -27,6 +27,7 @@ type CartoMock struct {
 	TerminateFunc            func() error
 	AddLidarReadingFunc      func(string, s.TimedLidarReadingResponse) error
 	AddIMUReadingFunc        func(string, s.TimedIMUReadingResponse) error
+	AddOdometerReadingFunc   func(string, s.TimedOdometerReadingResponse) error
 	PositionFunc             func() (Position, error)
 	PointCloudMapFunc        func() ([]byte, error)
 	InternalStateFunc        func() ([]byte, error)
@@ -66,11 +67,19 @@ func (cf *CartoMock) addLidarReading(lidar string, reading s.TimedLidarReadingRe
 }
 
 // addIMUReading calls the injected AddIMUReadingFunc or the real version.
-func (cf *CartoMock) addIMUReading(imu string, reading s.TimedIMUReadingResponse) error {
+func (cf *CartoMock) addIMUReading(movementSensor string, reading s.TimedIMUReadingResponse) error {
 	if cf.AddIMUReadingFunc == nil {
-		return cf.Carto.addIMUReading(imu, reading)
+		return cf.Carto.addIMUReading(movementSensor, reading)
 	}
-	return cf.AddIMUReadingFunc(imu, reading)
+	return cf.AddIMUReadingFunc(movementSensor, reading)
+}
+
+// addOdometerReading calls the injected AddOdometerReadingFunc or the real version.
+func (cf *CartoMock) addOdometerReading(movementSensor string, reading s.TimedOdometerReadingResponse) error {
+	if cf.AddOdometerReadingFunc == nil {
+		return cf.Carto.addOdometerReading(movementSensor, reading)
+	}
+	return cf.AddOdometerReadingFunc(movementSensor, reading)
 }
 
 // position calls the injected PositionFunc or the real version.