Check yaw rates instead of positions when yoyoing (retargeted #124 t…

…o main) (#133)

* bump depth expectations

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

* 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 <arjo@openrobotics.org>

* remove unused variable

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

* Fix tolerances. :man-facepalming:

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

* increase ramp up time

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

* Fix bad merge and increase tolerance

Signed-off-by: Louise Poubel <louise@openrobotics.org>

Co-authored-by: Arjo Chakravarty <arjo@openrobotics.org>

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<ignition::math::Vector3d> tethysLinearVel;
 
+  /// \brief All tethys angular velocities in order
+  public: std::vector<ignition::math::Vector3d> tethysAngularVel;
+
   /// \brief All state messages in order
   public: std::vector<lrauv_ignition_plugins::msgs::LRAUVState> stateMsgs;
 

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;
+    }
   }
 }