Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Check yaw rates instead of positions when yoyoing (retargeted #124 to main) #133

Merged
merged 6 commits into from
Jan 6, 2022
Merged

Check yaw rates instead of positions when yoyoing (retargeted #124 to main) #133

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
90d204b
bump depth expectations
arjo129 Dec 21, 2021
6cba3f0
Check yaw rates instead of positions when yoyoing
arjo129 Dec 21, 2021
11e2c56
remove unused variable
arjo129 Dec 21, 2021
36be8a0
Fix tolerances. :man-facepalming:
arjo129 Dec 21, 2021
a020b31
increase ramp up time
arjo129 Dec 21, 2021
39c508f
Fix bad merge and increase tolerance
chapulina Jan 5, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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;

Expand Down
29 changes: 18 additions & 11 deletions lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}
}