Modifies the dropweight test to test for breaching the surface. (#162)

This PR validates the behaviour of the dropweight with the surface of the water. It modifies the drop weight test such that the vehicle is started at a 10m depth and we monitor its rise. The test makes sure that we do not exceed a height of 0.5m (this tolerance is required to account for the oscillations introduces by the transition).

Signed-off-by: Arjo Chakravarty arjo@openrobotics.org

lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh

@@ -315,7 +315,8 @@ class LrauvTestFixture : public LrauvTestFixtureBase
   }
 };
 
-/// \brief Loads the default "buoyant_tethys_at_depth.sdf" world.
+/// \brief Loads the default "buyant_tethys_At_depth.sdf" world.
+/// This world has the robot start at a certain depth.
 class LrauvTestFixtureAtDepth : public LrauvTestFixtureBase
 {
   /// Documentation inherited

lrauv_ignition_plugins/test/test_drop_weight.cc

@@ -29,13 +29,23 @@
 #include <fstream>
 
 //////////////////////////////////////////////////
-TEST_F(LrauvTestFixture, DropWeightRelease)
+TEST_F(LrauvTestFixtureAtDepth, DropWeightRelease)
 {
+  // Max iterations
+  const int maxIterations{3800};
+
+  // Surface is at 0m
+  const double targetZ{0};
+  // Tolerance, the vehicle will breach the surface
+  const double tol{0.5};
+  // Starting point of the vehicle
+  const double startZ{-10};
+
   // Get initial Z
   this->fixture->Server()->Run(true, 100, false);
   EXPECT_EQ(100, this->iterations);
   EXPECT_EQ(100, this->tethysPoses.size());
-  EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().Z(), 0.05);
+  EXPECT_NEAR(startZ, this->tethysPoses.back().Pos().Z(), 0.05);
 
   // Tell the vehicle to release the weight
   lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg;
@@ -44,16 +54,26 @@ TEST_F(LrauvTestFixture, DropWeightRelease)
   // Neutral buoyancy
   cmdMsg.set_buoyancyaction_(0.0005);
 
-  // Run server until the command is processed and the model floats to a
-  // certain height
-  double targetZ{0.005};
+  // Run the server for fixed iterations
   this->PublishCommandWhile(cmdMsg, [&]()
   {
-    return this->tethysPoses.back().Pos().Z() < targetZ;
+    return this->iterations <= maxIterations;
   });
 
+  for (auto pose : this->tethysPoses)
+  {
+    // Make sure that the vehicle just breaches the surface of the the water
+    EXPECT_GT(targetZ + tol, pose.Pos().Z());
+
+    // MAke sure vehicle goes up.
+    EXPECT_LT(startZ - tol, pose.Pos().Z());
+  }
+
+  // Actually ran the server.
   EXPECT_LT(100, this->iterations);
   EXPECT_LT(100, this->tethysPoses.size());
-  EXPECT_LT(targetZ, this->tethysPoses.back().Pos().Z());
+
+  // Make sure we do get near the surface
+  EXPECT_NEAR(targetZ, this->tethysPoses.back().Pos().Z(), tol);
 }