From 248f0868967c6e2f90fea79b5b3e5d5f8f82a2c5 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 30 Nov 2021 14:45:29 +0800 Subject: [PATCH 1/9] Add mission_depth_vbs test Signed-off-by: Arjo Chakravarty --- .../test/test_mission_depth_vbs.cc | 35 +++++++++++-------- 1 file changed, 21 insertions(+), 14 deletions(-) diff --git a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc index 11d09f7f..1ed1fdaa 100644 --- a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc +++ b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc @@ -85,19 +85,26 @@ TEST_F(LrauvTestFixture, DepthVBS) // << std::endl; // } - this->CheckRange(2000, {0.01, -0.00, -0.78, -0.00, 0.04, 0.00}, {15.0, 3.14}); - this->CheckRange(4000, {0.47, -0.00, -2.80, -0.00, 0.08, 0.00}, {15.0, 3.14}); - this->CheckRange(6000, {2.77, -0.00, -5.73, 0.00, 0.12, 0.00}, {15.0, 3.14}); - this->CheckRange(8000, {8.78, -0.04, -9.62, 0.00, 0.12, 0.03}, {15.0, 3.14}); - this->CheckRange(10000, {15.66, 2.84, -13.07, 0.00, 0.09, 1.31}, {15.0, 3.14}); - this->CheckRange(12000, {14.82, 8.71, -15.58, 0.00, 0.04, 2.55}, {15.0, 3.14}); - this->CheckRange(14000, {10.69, 10.22, -16.95, -0.00, -0.03, -2.80}, {15.0, 3.14}); - this->CheckRange(16000, {7.85, 8.85, -16.07, -0.00, -0.01, -2.13}, {15.0, 3.14}); - this->CheckRange(18000, {6.48, 6.29, -13.84, 0.00, -0.07, -1.59}, {15.0, 3.14}); - this->CheckRange(20000, {6.99, 2.26, -10.73, 0.00, -0.17, -0.93}, {15.0, 3.14}); - this->CheckRange(22000, {11.22, -0.97, -7.71, -0.00, -0.10, 0.06}, {15.0, 3.14}); - this->CheckRange(24000, {15.47, 0.32, -5.62, -0.00, 0.04, 0.98}, {15.0, 3.14}); - this->CheckRange(26000, {16.95, 3.13, -5.16, 0.00, 0.00, 1.66}, {15.0, 3.14}); - this->CheckRange(28000, {16.65, 5.57, -6.79, -0.00, 0.02, 2.17}, {15.0, 3.14}); + for (int i = 1; i <= this->tethysPoses.size(); i ++) + { + // Vehicle should descend + EXPECT_LE(tethysPoses[i-1].Pos().Z(), tethysPoses[i].Pos().Z()); + + // Vehicle roll should be constant + EXPECT_NEAR(tethysPoses[i].Rot().Euler().X(), 0, 1e-2); + + // Vehicle should pitch backward slightly + EXPECT_GE(tethysPoses[i].Rot().Euler().Y(), 0); + + // Vehicle should not go vertical + EXPECT_LT(tethysPoses[i].Rot().Euler().Y(), IGN_DTOR(40)); + + // Vehicle yaw should not change + EXPECT_NEAR(tethysPoses[i].Rot().Euler().X(), 0, 1e-2); + + // Vehicle should not translate in Y + EXPECT_NEAR(tethysPoses[i].Pos().X(), 0, 1e-2); + } + } From 025133ebcd9fbd0eae3a451c2d41f55105bff0a9 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 10 Dec 2021 16:13:41 +0800 Subject: [PATCH 2/9] Add ability to log linear velocities of the craft Signed-off-by: Arjo Chakravarty --- .../test/helper/LrauvTestFixture.hh | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh index 76dd0f30..2b07aa34 100644 --- a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh +++ b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh @@ -85,6 +85,16 @@ class LrauvTestFixture : public ::testing::Test this->tethysPoses.push_back( ignition::gazebo::worldPose(modelEntity, _ecm)); + + ignition::gazebo::Model model(modelEntity); + auto linkEntity = model.LinkByName(_ecm, "base_link"); + EXPECT_NE(ignition::gazebo::kNullEntity, linkEntity); + + ignition::gazebo::Link link(linkEntity); + auto link = link.WorldLinearVelocity(_ecm); + EXPECT_TRUE(link.has_value()); + tethysLinearVel.push_back(link.value()); + this->iterations++; }); fixture->Finalize(); @@ -252,6 +262,9 @@ class LrauvTestFixture : public ::testing::Test /// \brief All tethys world poses in order public: std::vector tethysPoses; + /// \brief All tethys linear velocities in order + public: std::vector tethysLinearVel; + /// \brief All state messages in order public: std::vector stateMsgs; From 98b0b2165c586c93e858e584dc7c3913aeba6e99 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 10 Dec 2021 17:01:08 +0800 Subject: [PATCH 3/9] update expectations Signed-off-by: Arjo Chakravarty --- .../test/test_mission_depth_vbs.cc | 31 +++++++++++++------ 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc index 1ed1fdaa..61d11f04 100644 --- a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc +++ b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc @@ -85,26 +85,37 @@ TEST_F(LrauvTestFixture, DepthVBS) // << std::endl; // } + ignition::math::Vector3d maxVel(0, 0, 0); for (int i = 1; i <= this->tethysPoses.size(); i ++) { - // Vehicle should descend - EXPECT_LE(tethysPoses[i-1].Pos().Z(), tethysPoses[i].Pos().Z()); - // Vehicle roll should be constant EXPECT_NEAR(tethysPoses[i].Rot().Euler().X(), 0, 1e-2); - // Vehicle should pitch backward slightly - EXPECT_GE(tethysPoses[i].Rot().Euler().Y(), 0); - // Vehicle should not go vertical EXPECT_LT(tethysPoses[i].Rot().Euler().Y(), IGN_DTOR(40)); - // Vehicle yaw should not change - EXPECT_NEAR(tethysPoses[i].Rot().Euler().X(), 0, 1e-2); + // Vehicle should not exceed 20m depth + EXPECT_GT(tethysPoses[i].Pos().Z(), -20); - // Vehicle should not translate in Y - EXPECT_NEAR(tethysPoses[i].Pos().X(), 0, 1e-2); + if (tethysLinearVel[i].Length() > maxVel.Length()) + { + maxVel = tethysLinearVel[i]; + } } + // Vehicle's final pose should be near the 10m mark + EXPECT_NEAR(tethysPoses.back().Pos().Z(), -10, 1e-2); + + // Vehicle's final pose should be near the 10m mark + EXPECT_NEAR(tethysPoses.back().Pos().Z(), -10, 1e-2); + + // Vehicle should have almost zero Z velocity at the end. + EXPECT_NEAR(tethysLinearVel.back().Z(), 0, 1e-1); + + // Expect velocity to approach zero + EXPECT_LT(tethysLinearVel.back().Length(), maxVel.Length()); + + // Vehicle should pitch backward slightly + EXPECT_GE(tethysPoses.back().Rot().Euler().Y(), 0); } From 950f4f079cac206bbebd4976d8c0941a0065c45f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 10 Dec 2021 17:02:03 +0800 Subject: [PATCH 4/9] Add test fixture velocity component Signed-off-by: Arjo Chakravarty --- .../test/helper/LrauvTestFixture.hh | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh index 2b07aa34..0974bf79 100644 --- a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh +++ b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh @@ -25,6 +25,8 @@ #include +#include +#include #include #include #include @@ -91,9 +93,9 @@ class LrauvTestFixture : public ::testing::Test EXPECT_NE(ignition::gazebo::kNullEntity, linkEntity); ignition::gazebo::Link link(linkEntity); - auto link = link.WorldLinearVelocity(_ecm); - EXPECT_TRUE(link.has_value()); - tethysLinearVel.push_back(link.value()); + auto linkVel = link.WorldLinearVelocity(_ecm); + EXPECT_TRUE(linkVel.has_value()); + tethysLinearVel.push_back(linkVel.value()); this->iterations++; }); @@ -213,10 +215,10 @@ class LrauvTestFixture : public ::testing::Test return; } - char buffer[128]; + char buffer[256]; while (!feof(pipe)) { - if (fgets(buffer, 128, pipe) != nullptr) + if (fgets(buffer, 256, pipe) != nullptr) { igndbg << "CMD OUTPUT: " << buffer << std::endl; From ec1c2fde7bfb312c74230b0eaecfd711c1abd8e3 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 13 Dec 2021 09:18:33 +0800 Subject: [PATCH 5/9] address PR feedback Signed-off-by: Arjo Chakravarty --- .../test/test_mission_depth_vbs.cc | 28 ++++++------------- 1 file changed, 9 insertions(+), 19 deletions(-) diff --git a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc index 61d11f04..2f410b67 100644 --- a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc +++ b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc @@ -70,21 +70,6 @@ TEST_F(LrauvTestFixture, DepthVBS) int maxIterations{28000}; ASSERT_LT(maxIterations, this->tethysPoses.size()); - // Uncomment to get new expectations - // for (int i = 2000; i <= maxIterations; i += 2000) - // { - // auto pose = this->tethysPoses[i]; - // std::cout << "this->CheckRange(" << i << ", {" - // << std::setprecision(2) << std::fixed - // << pose.Pos().X() << ", " - // << pose.Pos().Y() << ", " - // << pose.Pos().Z() << ", " - // << pose.Rot().Roll() << ", " - // << pose.Rot().Pitch() << ", " - // << pose.Rot().Yaw() << "}, {12.0, 3.14});" - // << std::endl; - // } - ignition::math::Vector3d maxVel(0, 0, 0); for (int i = 1; i <= this->tethysPoses.size(); i ++) { @@ -94,7 +79,13 @@ TEST_F(LrauvTestFixture, DepthVBS) // Vehicle should not go vertical EXPECT_LT(tethysPoses[i].Rot().Euler().Y(), IGN_DTOR(40)); + // Vehicle should not go vertical + EXPECT_GT(tethysPoses[i].Rot().Euler().Y(), IGN_DTOR(-40)); + // Vehicle should not exceed 20m depth + // Note: Although the mission has a target depth of 10m, the vehicle has + // a tendency to overshoot first. Eventually the vehicle will reach steady + // state. EXPECT_GT(tethysPoses[i].Pos().Z(), -20); if (tethysLinearVel[i].Length() > maxVel.Length()) @@ -106,13 +97,12 @@ TEST_F(LrauvTestFixture, DepthVBS) // Vehicle's final pose should be near the 10m mark EXPECT_NEAR(tethysPoses.back().Pos().Z(), -10, 1e-2); - // Vehicle's final pose should be near the 10m mark - EXPECT_NEAR(tethysPoses.back().Pos().Z(), -10, 1e-2); - // Vehicle should have almost zero Z velocity at the end. EXPECT_NEAR(tethysLinearVel.back().Z(), 0, 1e-1); - // Expect velocity to approach zero + // Expect velocity to approach zero. At the end of the test, the vehicle may + // not have actually reached zero as it may still be translating or yawing, + // but its velocity should be less than the maximum velocity/ EXPECT_LT(tethysLinearVel.back().Length(), maxVel.Length()); // Vehicle should pitch backward slightly From e9d5bc4f1df34cec947a6f3bb3de46c3dc74c8c8 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 13 Dec 2021 09:50:11 +0800 Subject: [PATCH 6/9] Add check Signed-off-by: Arjo Chakravarty --- lrauv_ignition_plugins/test/test_mission_depth_vbs.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc index 2f410b67..03deee13 100644 --- a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc +++ b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc @@ -107,5 +107,6 @@ TEST_F(LrauvTestFixture, DepthVBS) // Vehicle should pitch backward slightly EXPECT_GE(tethysPoses.back().Rot().Euler().Y(), 0); + EXPECT_LE(tethysPoses.back().Rot().Euler().Y(), IGN_DTOR(25)); } From 673ac6fe3c99df82bbf27bc8eef7d779b719c01b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 20 Dec 2021 08:38:24 +0800 Subject: [PATCH 7/9] update tolerance and time Signed-off-by: Arjo Chakravarty --- lrauv_ignition_plugins/test/test_mission_depth_vbs.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc index 29fe93b0..52f30771 100644 --- a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc +++ b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc @@ -55,7 +55,7 @@ TEST_F(LrauvTestFixture, DepthVBS) // Run enough iterations (chosen empirically) to reach steady state, then kill // the controller - int targetIterations{28000}; + int targetIterations{50000}; int maxSleep{100}; int sleep{0}; for (; sleep < maxSleep && lrauvRunning && this->iterations < targetIterations; ++sleep) @@ -99,7 +99,7 @@ TEST_F(LrauvTestFixture, DepthVBS) } // Vehicle's final pose should be near the 10m mark - EXPECT_NEAR(tethysPoses.back().Pos().Z(), -10, 1e-2); + EXPECT_NEAR(tethysPoses.back().Pos().Z(), -10, 1e-1); // Vehicle should have almost zero Z velocity at the end. EXPECT_NEAR(tethysLinearVel.back().Z(), 0, 1e-1); From 099a58c9806fc7e0831e35520f40ec8172eb10c9 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 20 Dec 2021 09:24:40 +0800 Subject: [PATCH 8/9] remove pitch expectation after #96 is merged Signed-off-by: Arjo Chakravarty --- lrauv_ignition_plugins/test/test_mission_depth_vbs.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc index 52f30771..c5cd05e7 100644 --- a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc +++ b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc @@ -110,7 +110,8 @@ TEST_F(LrauvTestFixture, DepthVBS) EXPECT_LT(tethysLinearVel.back().Length(), maxVel.Length()); // Vehicle should pitch backward slightly - EXPECT_GE(tethysPoses.back().Rot().Euler().Y(), 0); - EXPECT_LE(tethysPoses.back().Rot().Euler().Y(), IGN_DTOR(25)); + // TODO(arjo): enable pitch check after #89 is merged + // EXPECT_GE(tethysPoses.back().Rot().Euler().Y(), 0); + // EXPECT_LE(tethysPoses.back().Rot().Euler().Y(), IGN_DTOR(25)); } From 0694f3a743b7fab1214d04fd4546143933a9ad98 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 21 Dec 2021 13:44:47 +0800 Subject: [PATCH 9/9] updated comments Signed-off-by: Arjo Chakravarty --- lrauv_ignition_plugins/test/test_mission_depth_vbs.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc index c5cd05e7..29370211 100644 --- a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc +++ b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc @@ -89,7 +89,7 @@ TEST_F(LrauvTestFixture, DepthVBS) // Vehicle should not exceed 20m depth // Note: Although the mission has a target depth of 10m, the vehicle has // a tendency to overshoot first. Eventually the vehicle will reach steady - // state. + // state, however at this point we are just checking for excess overshoot. EXPECT_GT(tethysPoses[i].Pos().Z(), -20); if (tethysLinearVel[i].Length() > maxVel.Length()) @@ -106,7 +106,7 @@ TEST_F(LrauvTestFixture, DepthVBS) // Expect velocity to approach zero. At the end of the test, the vehicle may // not have actually reached zero as it may still be translating or yawing, - // but its velocity should be less than the maximum velocity/ + // but its velocity should be less than the maximum velocity. EXPECT_LT(tethysLinearVel.back().Length(), maxVel.Length()); // Vehicle should pitch backward slightly