match linux & mac orientations

viam-modules · Jul 21, 2023 · cb8ada7 · cb8ada7
1 parent 9821961
commit cb8ada7
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 10 deletions.
diff --git 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
@@ -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
 		}