From 303549ae84af68765739ddee05264f1d3ce13c87 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 5 Jan 2022 17:52:34 -0800 Subject: [PATCH] Check yaw rates instead of positions when yoyoing (retargeted #124 to main) (#133) * bump depth expectations Signed-off-by: Arjo Chakravarty * 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 * remove unused variable Signed-off-by: Arjo Chakravarty * Fix tolerances. :man-facepalming: Signed-off-by: Arjo Chakravarty * increase ramp up time Signed-off-by: Arjo Chakravarty * Fix bad merge and increase tolerance Signed-off-by: Louise Poubel Co-authored-by: Arjo Chakravarty --- .../test/helper/LrauvTestFixture.hh | 7 +++++ .../test/test_mission_yoyo_circle.cc | 29 ++++++++++++------- 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh index 79a941dd..0ce6fbea 100644 --- a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh +++ b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh @@ -97,6 +97,10 @@ class LrauvTestFixture : public ::testing::Test EXPECT_TRUE(linkVel.has_value()); tethysLinearVel.push_back(linkVel.value()); + auto linkAngVel = link.WorldAngularVelocity(_ecm); + EXPECT_TRUE(linkAngVel.has_value()); + tethysAngularVel.push_back(linkAngVel.value()); + this->iterations++; }); fixture->Finalize(); @@ -283,6 +287,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 89884286..6619173e 100644 --- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc +++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc @@ -103,24 +103,31 @@ 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.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()}; - - ignition::math::Vector2d center{-4.0, -23.0}; - planarPos -= center; - - double meanRadius{20.0}; - EXPECT_NEAR(20.0, planarPos.Length(), 4.0) << i; + 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 + // vehicle should keep spinning in a circle. + // 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 + EXPECT_NEAR(tethysAngularVel[i].Y(), 0, 1e-1) << i; + + // And the linear velocity should be near 1m/s + EXPECT_NEAR(tethysLinearVel[i].Length(), 1.0, 2e-1) << i; + } } }