diff --git a/lrauv_description/models/tethys/model.sdf b/lrauv_description/models/tethys/model.sdf index fd854274..2c5ecf13 100644 --- a/lrauv_description/models/tethys/model.sdf +++ b/lrauv_description/models/tethys/model.sdf @@ -344,7 +344,7 @@ - 0 0 0 0 0 0 + 0 0 0 0 180 0 base_link propeller diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf index 1bc54366..f0081a8e 100644 --- a/lrauv_description/models/tethys_equipped/model.sdf +++ b/lrauv_description/models/tethys_equipped/model.sdf @@ -93,7 +93,8 @@ name="ignition::gazebo::systems::Thruster"> tethys propeller_joint - 0.004422 + + 0.00004422 1000 0.2 true diff --git a/lrauv_ignition_plugins/example/example_thruster.cc b/lrauv_ignition_plugins/example/example_thruster.cc index 53ef62c1..84bdbad4 100644 --- a/lrauv_ignition_plugins/example/example_thruster.cc +++ b/lrauv_ignition_plugins/example/example_thruster.cc @@ -21,7 +21,10 @@ */ /** - * Send velocity commands to the thruster. + * Send angular velocity commands to the propeller. + * + * Positive values rotate the propeller clockwise when looking + * from the back, and propel the vehicle forward. * * Usage: * $ LRAUV_example_thruster diff --git a/lrauv_ignition_plugins/proto/lrauv_command.proto b/lrauv_ignition_plugins/proto/lrauv_command.proto index ba2281ac..9607fa9b 100644 --- a/lrauv_ignition_plugins/proto/lrauv_command.proto +++ b/lrauv_ignition_plugins/proto/lrauv_command.proto @@ -37,7 +37,12 @@ message LRAUVCommand /// \brief Optional header data ignition.msgs.Header header = 1; - float propOmega_ = 2; + float propOmega_ = 2; // Angular velocity that the controller + // believes the propeller is currently at. + // Unit: rad / s. Positive values rotate + // the propeller clockwise when looking + // from the back, and propel the vehicle + // forward. float rudderAngle_ = 3; // Angle that the controller believes the // rudder is currently at. Unit: radians. // Higher values have the vertical fins @@ -60,7 +65,11 @@ message LRAUVCommand bool dropWeightState_ = 7; // Indicator "dropweight OK" // 1 = in place, 0 = dropped - float propOmegaAction_ = 8; + float propOmegaAction_ = 8; // Target angular velocity for the + // propeller. Unit: rad / s. Positive + // values rotate the propeller clockwise + // when looking from the back, and propel + // the vehicle forward. float rudderAngleAction_ = 9; // Target angle for rudder joint. Unit: // radians. Higher values rotate the // vertical fins clockwise when looking diff --git a/lrauv_ignition_plugins/proto/lrauv_state.proto b/lrauv_ignition_plugins/proto/lrauv_state.proto index 164b21b4..b70aaa12 100644 --- a/lrauv_ignition_plugins/proto/lrauv_state.proto +++ b/lrauv_ignition_plugins/proto/lrauv_state.proto @@ -43,9 +43,13 @@ message LRAUVState int32 errorPad_ = 2; int32 utmZone_ = 3; int32 northernHemi_ = 4; - float propOmega_ = 5; - float propThrust_ = 6; - float propTorque_ = 7; + float propOmega_ = 5; // Current angular velocity of the + // propeller. Unit: rad / s. Positive + // values rotate the propeller + // clockwise when looking from the back, + // and propel the vehicle forward. + float propThrust_ = 6; // Not populated + float propTorque_ = 7; // Not populated float rudderAngle_ = 8; // Angle that the rudder joint is // currently at. Unit: radians. // Higher values have the vertical fins @@ -64,7 +68,8 @@ message LRAUVState ignition.msgs.Vector3d rph_ = 13; // roll_, pitch_, heading_ in SimResultStruct (rad) - float speed_ = 14; + float speed_ = 14; // Magnitude of linear velocity in the + // world frame. Unit: m / s double latitudeDeg_ = 15; double longitudeDeg_ = 16; float netBuoy_ = 17; // Net buoyancy forces applied to the diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.cc b/lrauv_ignition_plugins/src/TethysCommPlugin.cc index 40f36900..46abb956 100644 --- a/lrauv_ignition_plugins/src/TethysCommPlugin.cc +++ b/lrauv_ignition_plugins/src/TethysCommPlugin.cc @@ -414,10 +414,15 @@ void TethysCommPlugin::CommandCallback( // Thruster ignition::msgs::Double thrusterMsg; // TODO(arjo): - // Conversion from rpm-> force b/c thruster plugin takes force + // Conversion from angular velocity to force b/c thruster plugin takes force // Maybe we should change that? + // https://github.com/osrf/lrauv/issues/75 auto angVel = _msg.propomegaaction_(); - auto force = -0.004422 * 1000 * 0.0016 * angVel * angVel; + + // force = thrust_coefficient * fluid_density * omega ^ 2 * + // propeller_diameter ^ 4 + // These values are defined in the model's Thruster plugin's SDF + auto force = 0.00004422 * 1000 * 0.2 * angVel * angVel; if (angVel < 0) { force *=-1; @@ -479,8 +484,8 @@ void TethysCommPlugin::PostUpdate( const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) { - ignition::gazebo::Link baseLink(modelLink); - auto modelPose = ignition::gazebo::worldPose(modelLink, _ecm); + ignition::gazebo::Link baseLink(this->modelLink); + auto modelPose = ignition::gazebo::worldPose(this->modelLink, _ecm); // Publish state lrauv_ignition_plugins::msgs::LRAUVState stateMsg; @@ -500,7 +505,7 @@ void TethysCommPlugin::PostUpdate( ignerr << "Propeller joint has wrong size\n"; return; } - stateMsg.set_propomega_(-propAngVelComp->Data()[0]); + stateMsg.set_propomega_(propAngVelComp->Data()[0]); // Rudder joint position auto rudderPosComp = @@ -553,7 +558,7 @@ void TethysCommPlugin::PostUpdate( // Speed auto linearVelocity = _ecm.Component( - modelLink); + this->modelLink); stateMsg.set_speed_(linearVelocity->Data().Length()); // Lat long diff --git a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh index e6a10998..02012949 100644 --- a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh +++ b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh @@ -71,6 +71,8 @@ class LrauvTestFixture : public ::testing::Test [&](const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) { + this->dt = _info.dt; + auto worldEntity = ignition::gazebo::worldEntity(_ecm); ignition::gazebo::World world(worldEntity); @@ -233,6 +235,9 @@ class LrauvTestFixture : public ::testing::Test /// \brief How many times has OnPostUpdate been run public: unsigned int iterations{0u}; + /// \brief Latest simulation time step. + public: std::chrono::steady_clock::duration dt{0}; + /// \brief All tethys world poses in order public: std::vector tethysPoses; diff --git a/lrauv_ignition_plugins/test/test_controller.cc b/lrauv_ignition_plugins/test/test_controller.cc index 330d1234..7ccef9af 100644 --- a/lrauv_ignition_plugins/test/test_controller.cc +++ b/lrauv_ignition_plugins/test/test_controller.cc @@ -37,23 +37,52 @@ TEST_F(LrauvTestFixture, Command) EXPECT_EQ(100, this->tethysPoses.size()); EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().X(), 1e-6); - // Propel vehicle + // Propel vehicle forward by giving the propeller a positive angular velocity + // Vehicle is supposed to move at around 1 m/s with 300 RPM. + // 300 RPM = 300 * 2 pi / 60 = 10 pi rad/s lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg; - cmdMsg.set_propomegaaction_(30); + cmdMsg.set_propomegaaction_(10 * IGN_PI); // Neutral buoyancy + cmdMsg.set_dropweightstate_(true); cmdMsg.set_buoyancyaction_(0.0005); // Run server until the command is processed and the model reaches a certain // point ahead (the robot moves towards -X) - double targetX{-1.5}; + double targetX{-100.0}; this->PublishCommandWhile(cmdMsg, [&]() { return this->tethysPoses.back().Pos().X() > targetX; }); - EXPECT_LT(100, this->iterations); - EXPECT_LT(100, this->tethysPoses.size()); + int minIterations{5100}; + EXPECT_LT(minIterations, this->iterations); + EXPECT_LT(minIterations, this->tethysPoses.size()); + + // Check final position EXPECT_GT(targetX, this->tethysPoses.back().Pos().X()); + EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().Y(), 1e-3); + EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().Z(), 0.05); + EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Roll(), 1e-2); + EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Pitch(), 1e-3); + EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Yaw(), 1e-3); + + // Check approximate instantaneous speed after initial acceleration + double dtSec = std::chrono::duration(this->dt).count(); + ASSERT_LT(0.0, dtSec); + double time100it = 100 * dtSec; + for (unsigned int i = 1000; i < this->tethysPoses.size(); i += 100) + { + auto prevPose = this->tethysPoses[i - 100]; + auto pose = this->tethysPoses[i]; + + auto dist = (pose.Pos() - prevPose.Pos()).Length(); + + auto linVel = dist / time100it; + EXPECT_LT(0.0, linVel); + + // TODO(chapulina) Decrease tolerance + EXPECT_NEAR(1.0, linVel, 0.19) << i; + } } diff --git a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc index fba34c5a..84783479 100644 --- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc +++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc @@ -75,137 +75,46 @@ TEST_F(LrauvTestFixture, YoYoCircle) ignmsg << "Logged [" << this->tethysPoses.size() << "] poses" << std::endl; - int maxIterations{28000}; - ASSERT_LT(maxIterations, this->tethysPoses.size()); - - // Uncomment to get new expectations - // for (int i = 500; i <= maxIterations; i += 500) - // { - // auto pose = this->tethysPoses[i]; - // std::cout << "this->CheckRange(" << i << ", {" - // << std::setprecision(2) << std::fixed - // << pose.Pos().X() << ", " - // << pose.Pos().Y() << ", " - // << pose.Pos().Z() << ", " - // << pose.Rot().Roll() << ", " - // << pose.Rot().Pitch() << ", " - // << pose.Rot().Yaw() << "}, {12.0, 1.57});" - // << std::endl; - // } - - // TODO(louise) Make expectations tighter as various inputs are updated - - // Y - // ^ - // | - // | View from the top - // | - // | - // | _ - // | / | - // ,--------' '_ - // ( _+ -----------> X - // `--------, , - // \_| - // - // * (X/Y): With a positive rudder angle, the robot rotates counter-clockwise. - // So it stays on the -Y side and goes: - // -X-Y -> +X-Y -> +X+Y -> -X+Y -> repeat - // * (Z): starts at zero, but once it goes below -2 m, it never goes deeper than - // -20 m, and never higher than -2 m. - // * (Pitch): always between -20 deg and 20 deg - - // With 0.02 step size, the vehicle does a lap every 7000 iterations or so - - // -X: decrease X - // -Y: decrease Y - this->CheckRange(500, {-0.16, 0.00, -0.04, 0.01, -0.00, 0.00}, {12.0, 1.57}); - this->CheckRange(1000, {-3.61, 0.09, 0.09, 0.01, -0.04, 0.15}, {12.0, 1.57}); - this->CheckRange(1500, {-10.30, -1.30, 0.04, -0.00, -0.11, 0.47}, {12.0, 1.57}); - this->CheckRange(2000, {-17.51, -5.89, -0.58, -0.01, -0.17, 0.88}, {12.0, 1.57}); - this->CheckRange(2500, {-22.54, -13.64, -1.73, -0.01, -0.21, 1.33}, {12.0, 1.57}); - this->CheckRange(3000, {-23.68, -23.05, -3.24, -0.01, -0.24, 1.79}, {12.0, 1.57}); - - // +X: increase X - this->CheckRange(3500, {-20.43, -32.02, -5.03, -0.01, -0.26, 2.27}, {12.0, 1.57}); - this->CheckRange(4000, {-13.43, -38.51, -6.97, -0.00, -0.27, 2.74}, {12.0, 1.57}); - this->CheckRange(4500, {-4.24, -41.06, -9.00, 0.00, -0.29, -3.06}, {12.0, 1.57}); - this->CheckRange(5000, {5.05, -39.10, -11.18, 0.01, -0.29, -2.58}, {12.0, 1.57}); - - // +Y: increase Y - this->CheckRange(5500, {12.38, -33.08, -13.39, 0.02, -0.30, -2.10}, {12.0, 1.57}); - this->CheckRange(6000, {16.10, -24.36, -15.63, 0.02, -0.31, -1.62}, {12.0, 1.57}); - - // -X: decrease X - this->CheckRange(6500, {15.37, -14.96, -17.99, 0.02, -0.32, -1.14}, {12.0, 1.57}); - this->CheckRange(7000, {10.39, -7.01, -20.55, 0.02, -0.16, -0.66}, {12.0, 1.57}); - this->CheckRange(7500, {2.00, -2.06, -21.82, 0.00, -0.04, -0.19}, {12.0, 1.57}); - this->CheckRange(8000, {-7.86, -1.46, -22.27, -0.01, 0.01, 0.28}, {12.0, 1.57}); - - // -Y: decrease Y - this->CheckRange(8500, {-16.95, -5.44, -22.31, -0.02, 0.06, 0.76}, {12.0, 1.57}); - this->CheckRange(9000, {-23.22, -13.11, -21.99, -0.02, 0.10, 1.23}, {12.0, 1.57}); - this->CheckRange(9500, {-25.29, -22.76, -21.37, -0.01, 0.13, 1.70}, {12.0, 1.57}); - - // +X: increase X - this->CheckRange(10000, {-22.75, -32.26, -20.48, 0.01, 0.16, 2.18}, {12.0, 1.57}); - this->CheckRange(10500, {-16.18, -39.51, -19.34, 0.02, 0.18, 2.65}, {12.0, 1.57}); - this->CheckRange(11000, {-7.07, -42.95, -17.99, 0.02, 0.21, 3.13}, {12.0, 1.57}); - - // +Y: increase Y - this->CheckRange(11500, {2.56, -41.84, -16.46, 0.02, 0.23, -2.68}, {12.0, 1.57}); - this->CheckRange(12000, {10.57, -36.46, -14.75, 0.01, 0.24, -2.20}, {12.0, 1.57}); - this->CheckRange(12500, {15.21, -28.04, -12.94, -0.00, 0.26, -1.72}, {12.0, 1.57}); - this->CheckRange(13000, {15.46, -18.47, -10.98, -0.01, 0.27, -1.25}, {12.0, 1.57}); - - // -X: decrease X - this->CheckRange(13500, {11.27, -9.88, -8.96, -0.02, 0.28, -0.77}, {12.0, 1.57}); - this->CheckRange(14000, {3.64, -4.21, -6.82, -0.02, 0.30, -0.29}, {12.0, 1.57}); - this->CheckRange(14500, {-5.72, -2.71, -4.59, -0.03, 0.30, 0.19}, {12.0, 1.57}); - - // -Y: decrease Y - this->CheckRange(15000, {-14.67, -5.69, -2.15, -0.02, 0.22, 0.68}, {12.0, 1.57}); - this->CheckRange(15500, {-21.37, -12.64, -0.51, -0.01, 0.06, 1.15}, {12.0, 1.57}); - this->CheckRange(16000, {-24.23, -22.08, 0.09, 0.01, -0.00, 1.62}, {12.0, 1.57}); - - // +X: increase X - this->CheckRange(16500, {-22.48, -31.83, 0.20, 0.02, -0.05, 2.09}, {12.0, 1.57}); - this->CheckRange(17000, {-16.47, -39.71, -0.05, 0.02, -0.09, 2.57}, {12.0, 1.57}); - this->CheckRange(17500, {-7.55, -43.97, -0.61, 0.01, -0.12, 3.04}, {12.0, 1.57}); - - // +Y: increase Y - this->CheckRange(18000, {2.29, -43.69, -1.45, -0.01, -0.15, -2.77}, {12.0, 1.57}); - this->CheckRange(18500, {10.87, -38.98, -2.53, -0.02, -0.18, -2.29}, {12.0, 1.57}); - this->CheckRange(19000, {16.33, -30.90, -3.83, -0.03, -0.20, -1.82}, {12.0, 1.57}); - this->CheckRange(19500, {17.48, -21.26, -5.34, -0.02, -0.22, -1.34}, {12.0, 1.57}); - - // -X: decrease X - this->CheckRange(20000, {14.10, -12.22, -7.00, -0.01, -0.24, -0.86}, {12.0, 1.57}); - this->CheckRange(20500, {6.97, -5.76, -8.81, 0.00, -0.25, -0.39}, {12.0, 1.57}); - this->CheckRange(21000, {-2.29, -3.31, -10.74, 0.02, -0.27, 0.09}, {12.0, 1.57}); - - // -Y: decrease Y - this->CheckRange(21500, {-11.60, -5.40, -12.79, 0.03, -0.28, 0.57}, {12.0, 1.57}); - this->CheckRange(22000, {-18.89, -11.54, -14.88, 0.04, -0.29, 1.05}, {12.0, 1.57}); - this->CheckRange(22500, {-22.50, -20.31, -17.10, 0.04, -0.31, 1.53}, {12.0, 1.57}); - - // +X: increase X - this->CheckRange(23000, {-21.65, -29.69, -19.53, 0.03, -0.24, 2.02}, {12.0, 1.57}); - this->CheckRange(23500, {-16.42, -37.76, -21.33, 0.01, -0.07, 2.49}, {12.0, 1.57}); - this->CheckRange(24000, {-7.88, -42.67, -21.96, -0.02, -0.00, 2.96}, {12.0, 1.57}); - this->CheckRange(24500, {2.01, -43.16, -22.10, -0.04, 0.05, -2.85}, {12.0, 1.57}); - - // +Y: increase Y - this->CheckRange(25000, {11.04, -39.07, -21.87, -0.03, 0.09, -2.37}, {12.0, 1.57}); - this->CheckRange(25500, {17.19, -31.34, -21.31, -0.01, 0.13, -1.90}, {12.0, 1.57}); - this->CheckRange(26000, {19.12, -21.68, -20.48, 0.01, 0.15, -1.42}, {12.0, 1.57}); - - // -X: decrease X - this->CheckRange(26500, {16.43, -12.26, -19.39, 0.03, 0.18, -0.95}, {12.0, 1.57}); - this->CheckRange(27000, {9.76, -5.16, -18.07, 0.04, 0.20, -0.47}, {12.0, 1.57}); - this->CheckRange(27500, {0.62, -1.91, -16.59, 0.03, 0.22, 0.01}, {12.0, 1.57}); - - // -Y: decrease Y - this->CheckRange(28000, {-8.96, -3.21, -14.93, 0.02, 0.23, 0.49}, {12.0, 1.57}); + int minIterations{28000}; + ASSERT_LT(minIterations, this->tethysPoses.size()); + + // Check bounds + double dtSec = std::chrono::duration(this->dt).count(); + ASSERT_LT(0.0, dtSec); + double time100it = 100 * dtSec; + for (unsigned int i = 100; i < this->tethysPoses.size(); i += 100) + { + auto prevPose = this->tethysPoses[i - 100]; + auto pose = this->tethysPoses[i]; + + // Speed is about 1 m / s + auto dist = (pose.Pos() - prevPose.Pos()).Length(); + + auto linVel = dist / time100it; + EXPECT_LT(0.0, linVel); + + EXPECT_NEAR(1.0, linVel, 1.0) << i; + + // Depth is above 20m, and below 2m after initial descent, with some + // tolerance + EXPECT_LT(-22.1, pose.Pos().Z()) << i; + if (i > 2000) + { + EXPECT_GT(0.0, 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; + } } diff --git a/lrauv_ignition_plugins/test/test_rudder.cc b/lrauv_ignition_plugins/test/test_rudder.cc index a2afb399..a148177d 100644 --- a/lrauv_ignition_plugins/test/test_rudder.cc +++ b/lrauv_ignition_plugins/test/test_rudder.cc @@ -63,6 +63,6 @@ TEST_F(LrauvTestFixture, Rudder) // Vehicle makes a clockwise arch looking from the top EXPECT_GT(targetY, this->tethysPoses.back().Pos().Y()); - EXPECT_GT(-1.5, this->tethysPoses.back().Pos().X()); + EXPECT_GT(-1.0, this->tethysPoses.back().Pos().X()); }