diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp index 46f3a9e3299a8074747a4d07d6e9add8fc49e249..3c15a924d782d331db4b6ef268a2ea123b27db7d 100644 --- a/test/gtest_processor_diff_drive.cpp +++ b/test/gtest_processor_diff_drive.cpp @@ -224,7 +224,6 @@ TEST_F(ProcessorDiffDriveTest, computeCurrentDelta) TEST_F(ProcessorDiffDriveTest, deltaPlusDelta) { - Vector2s data; Matrix2s data_cov; data_cov . setIdentity(); Vector3s calib(1,1,1); @@ -265,7 +264,6 @@ TEST_F(ProcessorDiffDriveTest, deltaPlusDelta) TEST_F(ProcessorDiffDriveTest, statePlusDelta) { - Vector2s data; Matrix2s data_cov; data_cov . setIdentity(); Vector3s calib(1,1,1); @@ -306,7 +304,6 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta) TEST_F(ProcessorDiffDriveTest, process) { - Vector2s data; Matrix2s data_cov; data_cov . setIdentity(); TimeStamp t = 0.0; @@ -336,6 +333,61 @@ TEST_F(ProcessorDiffDriveTest, process) problem->print(4,1,1,1); } +TEST_F(ProcessorDiffDriveTest, linear) +{ + Vector2s data; + Matrix2s data_cov; data_cov . setIdentity(); + TimeStamp t = 0.0; + Vector3s x(0,0,0); + Matrix3s P; P.setIdentity(); + + auto F0 = problem->setPrior(x, P, t, 0.1); + + // Straight one turn of the wheels, in one go + data(0) = 100.0 ; // one turn of the wheels + data(1) = 100.0 ; + + auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0); + + C->process(); + WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose()); + + // radius is 1.0m, 100 ticks per revolution, so advanced distance is + Scalar distance = 2 * M_PI * 1.0; + + ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3s(distance,0,0), 1e-6) +} + +TEST_F(ProcessorDiffDriveTest, angular) +{ + Vector2s data; + Matrix2s data_cov; data_cov . setIdentity(); + TimeStamp t = 0.0; + Vector3s x(0,0,0); + Matrix3s P; P.setIdentity(); + + auto F0 = problem->setPrior(x, P, t, 0.1); + + // Straight one turn of the wheels, in one go + data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse + data(1) = 20.0 ; // one fifth of a turn of the right wheel, forward --> we'll turn left --> positive angle + + auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0); + + C->process(); + WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose()); + + // this is a turn in place, so distance = 0; + Scalar distance = 0.0; + + // radius is 1.0m, 100 ticks per revolution, and wheel separation is 1m, so turn angle is + Scalar angle = pi2pi(2 * M_PI * 1.0 / 0.5 / 5); + + ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3s(distance,0,angle), 1e-6) +} + + + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv);