From 49cdd9b65d21cc2ab13f0eb1119c96e869e5c790 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 23 Aug 2019 16:27:21 +0200
Subject: [PATCH] New tests for linear advancement and pure rotation
---
test/gtest_processor_diff_drive.cpp | 58 +++++++++++++++++++++++++++--
1 file changed, 55 insertions(+), 3 deletions(-)
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 46f3a9e32..3c15a924d 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);
--
GitLab