From cb8ada7d490e02c4449dff2b844d525b60a7fb48 Mon Sep 17 00:00:00 2001 From: Kim Mishra Date: Fri, 21 Jul 2023 14:04:46 -0400 Subject: [PATCH] match linux & mac orientations --- integration_test.go | 25 +++++++++++++++++++------ testhelper/testhelper.go | 8 ++++---- 2 files changed, 23 insertions(+), 10 deletions(-) diff --git a/integration_test.go b/integration_test.go index 1d7fba2c..495f40f1 100644 --- a/integration_test.go +++ b/integration_test.go @@ -53,9 +53,11 @@ func testCartographerMap(t *testing.T, svc slam.Service, localizationMode bool) func testCartographerPosition(t *testing.T, svc slam.Service, expectedComponentRef string) { expectedPosOSX := r3.Vector{X: 155.7488316264227, Y: -90.25868252233964, Z: 0} - expectedPosLinux := r3.Vector{X: -3.0874594959315647, Y: 4.502970388777862, Z: 0} + expectedPosLinux := r3.Vector{X: 158.79903385710674, Y: -77.01514065531592, Z: 0} tolerancePos := 0.001 - expectedOri := &spatialmath.R4AA{Theta: 1.5465081272043815, RX: 0, RY: 0, RZ: 1} + + expectedOriOSX := &spatialmath.R4AA{Theta: 1.5465081272043815, RX: 0, RY: 0, RZ: 1} + expectedOriLinux := &spatialmath.R4AA{Theta: 0.3331667853231311, RX: 0, RY: 0, RZ: 1} toleranceOri := 0.001 position, componentRef, err := svc.GetPosition(context.Background()) @@ -79,10 +81,21 @@ func testCartographerPosition(t *testing.T, svc slam.Service, expectedComponentR actualOri := position.Orientation().AxisAngles() t.Logf("Position orientation: RX: %v, RY: %v, RZ: %v, Theta: %v", actualOri.RX, actualOri.RY, actualOri.RZ, actualOri.Theta) - test.That(t, actualOri.RX, test.ShouldBeBetween, expectedOri.RX-toleranceOri, expectedOri.RX+toleranceOri) - test.That(t, actualOri.RY, test.ShouldBeBetween, expectedOri.RY-toleranceOri, expectedOri.RY+toleranceOri) - test.That(t, actualOri.RZ, test.ShouldBeBetween, expectedOri.RZ-toleranceOri, expectedOri.RZ+toleranceOri) - test.That(t, actualOri.Theta, test.ShouldBeBetween, expectedOri.Theta-toleranceOri, expectedOri.Theta+toleranceOri) + t.Logf("Expected orientation: RX: %v, RY: %v, RZ: %v, Theta: %v", expectedOriOSX.RX, expectedOriOSX.RY, expectedOriOSX.RZ, expectedOriOSX.Theta) + + if actualOri.Theta > expectedOriOSX.Theta-toleranceOri && actualOri.Theta < expectedOriOSX.Theta+toleranceOri { + test.That(t, actualOri.RX, test.ShouldBeBetween, expectedOriOSX.RX-toleranceOri, expectedOriOSX.RX+toleranceOri) + test.That(t, actualOri.RY, test.ShouldBeBetween, expectedOriOSX.RY-toleranceOri, expectedOriOSX.RY+toleranceOri) + test.That(t, actualOri.RZ, test.ShouldBeBetween, expectedOriOSX.RZ-toleranceOri, expectedOriOSX.RZ+toleranceOri) + test.That(t, actualOri.Theta, test.ShouldBeBetween, expectedOriOSX.Theta-toleranceOri, expectedOriOSX.Theta+toleranceOri) + } else if actualOri.Theta > expectedOriLinux.Theta-toleranceOri && actualOri.Theta < expectedOriLinux.Theta+toleranceOri { + test.That(t, actualOri.RX, test.ShouldBeBetween, expectedOriLinux.RX-toleranceOri, expectedOriLinux.RX+toleranceOri) + test.That(t, actualOri.RY, test.ShouldBeBetween, expectedOriLinux.RY-toleranceOri, expectedOriLinux.RY+toleranceOri) + test.That(t, actualOri.RZ, test.ShouldBeBetween, expectedOriLinux.RZ-toleranceOri, expectedOriLinux.RZ+toleranceOri) + test.That(t, actualOri.Theta, test.ShouldBeBetween, expectedOriLinux.Theta-toleranceOri, expectedOriLinux.Theta+toleranceOri) + } else { + t.Error("Orientation is outside of expected platform range") + } } func saveInternalState(t *testing.T, internalState []byte, dataDir string) { diff --git a/testhelper/testhelper.go b/testhelper/testhelper.go index 6958323c..f7b1065c 100644 --- a/testhelper/testhelper.go +++ b/testhelper/testhelper.go @@ -145,7 +145,7 @@ func IntegrationLidarTimedSensor( ts.TimedSensorReadingFunc = func(ctx context.Context) (s.TimedSensorReadingResponse, error) { readingTime = readingTime.Add(sensorReadingInterval) - t.Logf("TimedSenorReading Mock i: %d, closed: %v, readingTime: %s\n", i, closed, readingTime.String()) + t.Logf("TimedSensorReading Mock i: %d, closed: %v, readingTime: %s\n", i, closed, readingTime.String()) if i >= NumPointClouds { // communicate to the test that all lidar readings have been written if !closed { @@ -157,19 +157,19 @@ func IntegrationLidarTimedSensor( file, err := os.Open(artifact.MustPath("viam-cartographer/mock_lidar/" + strconv.FormatUint(i, 10) + ".pcd")) if err != nil { - t.Error("TimedSenorReading Mock failed to open pcd file") + t.Error("TimedSensorReading Mock failed to open pcd file") return s.TimedSensorReadingResponse{}, err } readingPc, err := pointcloud.ReadPCD(file) if err != nil { - t.Error("TimedSenorReading Mock failed to read pcd") + t.Error("TimedSensorReading Mock failed to read pcd") return s.TimedSensorReadingResponse{}, err } buf := new(bytes.Buffer) err = pointcloud.ToPCD(readingPc, buf, pointcloud.PCDBinary) if err != nil { - t.Error("TimedSenorReading Mock failed to parse pcd") + t.Error("TimedSensorReading Mock failed to parse pcd") return s.TimedSensorReadingResponse{}, err }