From 9fb64508a6978bc2c76396bfc263e8c1cbe1d99d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 21 Dec 2021 12:37:01 +0800 Subject: [PATCH] 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 | 21 +++++++++++++++++++ .../test/test_mission_yoyo_circle.cc | 19 ++++++++++++----- 2 files changed, 35 insertions(+), 5 deletions(-) diff --git a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh index a1cf9498..db4ca60e 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 @@ -86,6 +88,19 @@ class LrauvTestFixture : public ::testing::Test this->tethysPoses.push_back( ignition::gazebo::worldPose(modelEntity, _ecm)); 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(); } @@ -268,6 +283,12 @@ 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 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; + } } }