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

…o main) (#133) * bump depth expectations Signed-off-by: Arjo Chakravarty <[email protected]> * 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 <[email protected]> * remove unused variable Signed-off-by: Arjo Chakravarty <[email protected]> * Fix tolerances. :man-facepalming: Signed-off-by: Arjo Chakravarty <[email protected]> * increase ramp up time Signed-off-by: Arjo Chakravarty <[email protected]> * Fix bad merge and increase tolerance Signed-off-by: Louise Poubel <[email protected]> Co-authored-by: Arjo Chakravarty <[email protected]>
osrf · Jan 6, 2022 · 303549a · 303549a
1 parent 0d1ef82
commit 303549a
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 11 deletions.
diff --git 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<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;
 

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