From 90d204bb380e7fdb8d3accd9fe1cfa88b6ea829f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 21 Dec 2021 10:52:30 +0800 Subject: [PATCH 1/6] bump depth expectations Signed-off-by: Arjo Chakravarty --- .../test/test_mission_yoyo_circle.cc | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc index 89884286..8d3880dc 100644 --- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc +++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc @@ -103,12 +103,13 @@ TEST_F(LrauvTestFixture, YoYoCircle) // Depth is above 20m, and below 2m after initial descent, with some // tolerance - EXPECT_LT(-22.5, pose.Pos().Z()) << i; - if (i > 2000) + EXPECT_LT(-22.7, pose.Pos().Z()) << i; + if (i > 4000) { - EXPECT_GT(0.3, pose.Pos().Z()) << i; + EXPECT_GT(0.4, pose.Pos().Z()) << i; } + // Pitch is between -20 and 20 deg EXPECT_LT(IGN_DTOR(-20), pose.Rot().Pitch()) << i; EXPECT_GT(IGN_DTOR(20), pose.Rot().Pitch()) << i; @@ -116,11 +117,11 @@ TEST_F(LrauvTestFixture, YoYoCircle) // Trajectory projected onto the horizontal plane is roughly a circle ignition::math::Vector2d planarPos{pose.Pos().X(), pose.Pos().Y()}; - ignition::math::Vector2d center{-4.0, -23.0}; - planarPos -= center; + //ignition::math::Vector2d center{-4.0, -23.0}; + //planarPos -= center; - double meanRadius{20.0}; - EXPECT_NEAR(20.0, planarPos.Length(), 4.0) << i; + //double meanRadius{20.0}; + //EXPECT_NEAR(20.0, planarPos.Length(), 4.0) << i << ", " << planarPos; } } From 6cba3f0e94665005f81cdde7e5deb825aa83e65c Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 21 Dec 2021 12:37:01 +0800 Subject: [PATCH 2/6] Check yaw rates instead of positions when yoyoing This PR depends on #89, #96. Recommended review order is #89 > #96 > this PR. If we want I can cherry pick this branch as the changes are independent of the underlying code. The merge order should be this PR > #96 > #89. There is a slight change in the center of rotation of the yoyo mission with #96. This leads to test expectations failing. Rather than use position to check whether the vehicle is yoyoing, I think we should use @tfoote's suggestion and use yaw rate which makes the test independent of the center of rotation and hence more robust while at the same time ensuring that the vehicle actually moves in a circle. Signed-off-by: Arjo Chakravarty --- .../test/helper/LrauvTestFixture.hh | 16 ++++++++++++++++ .../test/test_mission_yoyo_circle.cc | 19 ++++++++++++++----- 2 files changed, 30 insertions(+), 5 deletions(-) diff --git a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh index 79a941dd..b2ed96fe 100644 --- a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh +++ b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh @@ -98,6 +98,19 @@ class LrauvTestFixture : public ::testing::Test tethysLinearVel.push_back(linkVel.value()); this->iterations++; + + ignition::gazebo::Model model(modelEntity); + auto linkEntity = model.LinkByName(_ecm, "base_link"); + EXPECT_NE(ignition::gazebo::kNullEntity, linkEntity); + + ignition::gazebo::Link link(linkEntity); + auto linkAngVel = link.WorldAngularVelocity(_ecm); + EXPECT_TRUE(linkAngVel.has_value()); + tethysAngularVel.push_back(linkAngVel.value()); + + auto linkVel = link.WorldLinearVelocity(_ecm); + EXPECT_TRUE(linkVel.has_value()); + tethysLinearVel.push_back(linkVel.value()); }); fixture->Finalize(); } @@ -283,6 +296,9 @@ class LrauvTestFixture : public ::testing::Test /// \brief All tethys linear velocities in order public: std::vector tethysLinearVel; + /// \brief All tethys angular velocities in order + public: std::vector tethysAngularVel; + /// \brief All state messages in order public: std::vector stateMsgs; diff --git a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc index 8d3880dc..92ae52cb 100644 --- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc +++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc @@ -106,7 +106,7 @@ TEST_F(LrauvTestFixture, YoYoCircle) EXPECT_LT(-22.7, pose.Pos().Z()) << i; if (i > 4000) { - EXPECT_GT(0.4, pose.Pos().Z()) << i; + EXPECT_GT(0.5, pose.Pos().Z()) << i; } @@ -117,11 +117,20 @@ TEST_F(LrauvTestFixture, YoYoCircle) // Trajectory projected onto the horizontal plane is roughly a circle ignition::math::Vector2d planarPos{pose.Pos().X(), pose.Pos().Y()}; - //ignition::math::Vector2d center{-4.0, -23.0}; - //planarPos -= center; + if (i > 6000) + { + // Once the vehicle achieves its full velocity the vehicle should have a + // nominal yaw rate of around 0.37-0.38rad/s. This means that the vehicle + // should keep spinning in a circle. + EXPECT_NEAR(tethysAngularVel[i].Z(), 0.037, 2e-2) + << i << " yaw rate: " << tethysAngularVel[i].Z(); + + // At the same time the roll rate should be near zero + EXPECT_NEAR(tethysAngularVel[i].Y(), 0, 1e-1) << i; - //double meanRadius{20.0}; - //EXPECT_NEAR(20.0, planarPos.Length(), 4.0) << i << ", " << planarPos; + // And the linear velocity should be near 1m/s + EXPECT_NEAR(tethysLinearVel[i].Length(), 1.0, 2e-1) << i; + } } } From 11e2c56394bba1a1611b5031e4ed993c4d34583f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 21 Dec 2021 12:44:03 +0800 Subject: [PATCH 3/6] remove unused variable Signed-off-by: Arjo Chakravarty --- lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc | 4 ---- 1 file changed, 4 deletions(-) diff --git a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc index 92ae52cb..788442e9 100644 --- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc +++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc @@ -109,14 +109,10 @@ TEST_F(LrauvTestFixture, YoYoCircle) EXPECT_GT(0.5, pose.Pos().Z()) << i; } - // Pitch is between -20 and 20 deg EXPECT_LT(IGN_DTOR(-20), pose.Rot().Pitch()) << i; EXPECT_GT(IGN_DTOR(20), pose.Rot().Pitch()) << i; - // Trajectory projected onto the horizontal plane is roughly a circle - ignition::math::Vector2d planarPos{pose.Pos().X(), pose.Pos().Y()}; - if (i > 6000) { // Once the vehicle achieves its full velocity the vehicle should have a From 36be8a024b720d92c9230136189603567f281b3d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 21 Dec 2021 13:12:27 +0800 Subject: [PATCH 4/6] Fix tolerances. :man-facepalming: Signed-off-by: Arjo Chakravarty --- lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc index 788442e9..cb518a6d 100644 --- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc +++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc @@ -116,9 +116,9 @@ TEST_F(LrauvTestFixture, YoYoCircle) if (i > 6000) { // Once the vehicle achieves its full velocity the vehicle should have a - // nominal yaw rate of around 0.37-0.38rad/s. This means that the vehicle - // should keep spinning in a circle. - EXPECT_NEAR(tethysAngularVel[i].Z(), 0.037, 2e-2) + // nominal yaw rate of around 0.037-0.038rad/s. This means that the + // vehicle should keep spinning in a circle. + EXPECT_NEAR(tethysAngularVel[i].Z(), 0.037, 2e-3) << i << " yaw rate: " << tethysAngularVel[i].Z(); // At the same time the roll rate should be near zero From a020b310d2468a56f3150e543d00cb7cc0d6c02e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 21 Dec 2021 13:27:59 +0800 Subject: [PATCH 5/6] increase ramp up time Signed-off-by: Arjo Chakravarty --- lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc index cb518a6d..5e6cf8c1 100644 --- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc +++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc @@ -113,7 +113,7 @@ TEST_F(LrauvTestFixture, YoYoCircle) EXPECT_LT(IGN_DTOR(-20), pose.Rot().Pitch()) << i; EXPECT_GT(IGN_DTOR(20), pose.Rot().Pitch()) << i; - if (i > 6000) + if (i > 7000) { // Once the vehicle achieves its full velocity the vehicle should have a // nominal yaw rate of around 0.037-0.038rad/s. This means that the From 39c508f28d1bc8ea8e2875d3c12eb0d1bb560cac Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 5 Jan 2022 14:25:38 -0800 Subject: [PATCH 6/6] Fix bad merge and increase tolerance Signed-off-by: Louise Poubel --- .../test/helper/LrauvTestFixture.hh | 11 +---------- .../test/test_mission_yoyo_circle.cc | 3 ++- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh index b2ed96fe..0ce6fbea 100644 --- a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh +++ b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh @@ -97,20 +97,11 @@ class LrauvTestFixture : public ::testing::Test EXPECT_TRUE(linkVel.has_value()); tethysLinearVel.push_back(linkVel.value()); - this->iterations++; - - ignition::gazebo::Model model(modelEntity); - auto linkEntity = model.LinkByName(_ecm, "base_link"); - EXPECT_NE(ignition::gazebo::kNullEntity, linkEntity); - - ignition::gazebo::Link link(linkEntity); auto linkAngVel = link.WorldAngularVelocity(_ecm); EXPECT_TRUE(linkAngVel.has_value()); tethysAngularVel.push_back(linkAngVel.value()); - auto linkVel = link.WorldLinearVelocity(_ecm); - EXPECT_TRUE(linkVel.has_value()); - tethysLinearVel.push_back(linkVel.value()); + this->iterations++; }); fixture->Finalize(); } diff --git a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc index 5e6cf8c1..6619173e 100644 --- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc +++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc @@ -118,7 +118,8 @@ TEST_F(LrauvTestFixture, YoYoCircle) // Once the vehicle achieves its full velocity the vehicle should have a // nominal yaw rate of around 0.037-0.038rad/s. This means that the // vehicle should keep spinning in a circle. - EXPECT_NEAR(tethysAngularVel[i].Z(), 0.037, 2e-3) + // TODO(arjo) Reduce tolerance when hydrodynamics is fixed + EXPECT_NEAR(tethysAngularVel[i].Z(), 0.037, 2e-2) << i << " yaw rate: " << tethysAngularVel[i].Z(); // At the same time the roll rate should be near zero