Use actual depth vbs expectations

lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh

@@ -25,6 +25,8 @@
 
 #include <gtest/gtest.h>
 
+#include <ignition/gazebo/Link.hh>
+#include <ignition/gazebo/Model.hh>
 #include <ignition/gazebo/TestFixture.hh>
 #include <ignition/gazebo/Util.hh>
 #include <ignition/gazebo/World.hh>
@@ -85,6 +87,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 linkVel = link.WorldLinearVelocity(_ecm);
+        EXPECT_TRUE(linkVel.has_value());
+        tethysLinearVel.push_back(linkVel.value());
+
         this->iterations++;
       });
     fixture->Finalize();
@@ -268,6 +280,9 @@ class LrauvTestFixture : public ::testing::Test
   /// \brief All tethys world poses in order
   public: std::vector<ignition::math::Pose3d> tethysPoses;
 
+  /// \brief All tethys linear velocities in order
+  public: std::vector<ignition::math::Vector3d> tethysLinearVel;
+
   /// \brief All state messages in order
   public: std::vector<lrauv_ignition_plugins::msgs::LRAUVState> stateMsgs;
 

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)
@@ -71,34 +71,47 @@ TEST_F(LrauvTestFixture, DepthVBS)
 
   ignmsg << "Logged [" << this->tethysPoses.size() << "] poses" << std::endl;
 
-  // Uncomment to get new expectations
-  // for (int i = 2000; i <= targetIterations; 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;
-  // }
-
-  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});
+  int maxIterations{28000};
+  ASSERT_LT(maxIterations, this->tethysPoses.size());
+
+  ignition::math::Vector3d maxVel(0, 0, 0);
+  for (int i = 1; i <= this->tethysPoses.size(); i ++)
+  {
+    // Vehicle roll should be constant
+    EXPECT_NEAR(tethysPoses[i].Rot().Euler().X(), 0, 1e-2);
+
+    // 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, however at this point we are just checking for excess overshoot.
+    EXPECT_GT(tethysPoses[i].Pos().Z(), -20);
+
+    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-1);
+
+  // Vehicle should have almost zero Z velocity at the end.
+  EXPECT_NEAR(tethysLinearVel.back().Z(), 0, 1e-1);
+
+  // 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
+  // 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));
 }