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]>
osrf · Dec 21, 2021 · 9fb6450 · 9fb6450
1 parent 913ffae
commit 9fb6450
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 5 deletions.
diff --git a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh b/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>
@@ -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<ignition::math::Pose3d> tethysPoses;
 
+  /// \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
@@ -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;
+    }
   }
 }