From 0f8c4be440c4938625d54df45a04e6bf710a4597 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 16 Nov 2021 12:07:27 -0800 Subject: [PATCH] Documenting and cleaning up last message fields Signed-off-by: Louise Poubel --- .../models/axes/model.config | 18 ++ lrauv_ignition_plugins/models/axes/model.sdf | 48 +++++ .../proto/lrauv_command.proto | 6 +- .../proto/lrauv_state.proto | 104 ++++++++--- .../src/TethysCommPlugin.cc | 126 +++++++------ .../src/TethysCommPlugin.hh | 8 +- lrauv_ignition_plugins/test/test_state_msg.cc | 168 +++++++++++++++++- .../worlds/buoyant_tethys.sdf | 27 +++ 8 files changed, 416 insertions(+), 89 deletions(-) create mode 100644 lrauv_ignition_plugins/models/axes/model.config create mode 100644 lrauv_ignition_plugins/models/axes/model.sdf diff --git a/lrauv_ignition_plugins/models/axes/model.config b/lrauv_ignition_plugins/models/axes/model.config new file mode 100644 index 00000000..7c4dc021 --- /dev/null +++ b/lrauv_ignition_plugins/models/axes/model.config @@ -0,0 +1,18 @@ + + + + axes + 1.0 + model.sdf + + + Louise Poubel + louise@openrobotics.org + + + + + diff --git a/lrauv_ignition_plugins/models/axes/model.sdf b/lrauv_ignition_plugins/models/axes/model.sdf new file mode 100644 index 00000000..04d7229d --- /dev/null +++ b/lrauv_ignition_plugins/models/axes/model.sdf @@ -0,0 +1,48 @@ + + + + + + 0 0 0 0 180 -90 + + 0.5 0 0 0 90 0 + + + 1 + 0.1 + + + + 1 0 0 + + + + 0 0.5 0 90 0 0 + + + 1 + 0.1 + + + + 0 1 0 + + + + 0 0 0.5 0 0 0 + + + 1 + 0.1 + + + + 0 0 1 + + + + + diff --git a/lrauv_ignition_plugins/proto/lrauv_command.proto b/lrauv_ignition_plugins/proto/lrauv_command.proto index 9607fa9b..d0e9b4be 100644 --- a/lrauv_ignition_plugins/proto/lrauv_command.proto +++ b/lrauv_ignition_plugins/proto/lrauv_command.proto @@ -88,7 +88,7 @@ message LRAUVCommand // Volumes higher than the neutral volume // make the vehicle float. - float density_ = 13; - float dt_ = 14; - double time_ = 15; + float density_ = 13; // Not used + float dt_ = 14; // Not used + double time_ = 15; // Not used } diff --git a/lrauv_ignition_plugins/proto/lrauv_state.proto b/lrauv_ignition_plugins/proto/lrauv_state.proto index b70aaa12..4fe37489 100644 --- a/lrauv_ignition_plugins/proto/lrauv_state.proto +++ b/lrauv_ignition_plugins/proto/lrauv_state.proto @@ -34,15 +34,34 @@ import "ignition/msgs/vector3d.proto"; // Mirrors SimResultStruct // Note coordinate frame convention for all pose fields: -// FSK (x fore or forward, y starboard or right, z keel or down) +// +// Reference frames: +// +// * Vehicle frame: FSK +// * X: Fore or forward +// * Y: Starboard or right +// * Z: Keel or down +// * Roll: Clockwise rotation when looking from the back. +// * Pitch: Positive angles bring the nose up. +// * Yaw: Positive angles bring the nose to the right / starboard. +// +// * World frame: NED +// * X: North +// * Y: East +// * Z: Down +// * Roll: Right-hand rotation about North +// * Pitch: Right-hand rotation about East +// * Yaw: Right-hand rotation about Down +// message LRAUVState { /// \brief Optional header data + /// Stamped with simulation time. ignition.msgs.Header header = 1; - int32 errorPad_ = 2; - int32 utmZone_ = 3; - int32 northernHemi_ = 4; + int32 errorPad_ = 2; // Not populated. + int32 utmZone_ = 3; // Not populated. + int32 northernHemi_ = 4; // Not populated. float propOmega_ = 5; // Current angular velocity of the // propeller. Unit: rad / s. Positive // values rotate the propeller @@ -64,35 +83,64 @@ message LRAUVState // meters. Volumes higher than the // neutral volume push the vehicle // upwards. - float depth_ = 12; + float depth_ = 12; // Vertical position of the vehicle + // with respect to sea level. Higher + // values are deeper, negative values + // are above water. Unit: meters. - ignition.msgs.Vector3d rph_ = 13; // roll_, pitch_, heading_ in SimResultStruct (rad) + ignition.msgs.Vector3d rph_ = 13; // Duplicate of posRPH_ - float speed_ = 14; // Magnitude of linear velocity in the - // world frame. Unit: m / s - double latitudeDeg_ = 15; - double longitudeDeg_ = 16; + float speed_ = 14; // Magnitude of vehicle's linear velocity + // in the world frame. It's always positive. + // Unit: m/s + double latitudeDeg_ = 15; // Latitude in degrees + double longitudeDeg_ = 16; // Longitude in degrees float netBuoy_ = 17; // Net buoyancy forces applied to the // vehicle. Unit: Newtons. Currently // not populated. ignition.msgs.Vector3d force_ = 18; // forceX_, forceY_, forceZ_ in SimResultStruct - ignition.msgs.Vector3d pos_ = 19; // posX_, posY_, posZ_ in SimResultStruct - ignition.msgs.Vector3d posRPH_ = 20; // posRoll_, posPitch_, posHeading_ in SimResultStruct - ignition.msgs.Vector3d posDot_ = 21; // posXDot_, posYDot_, posZDot_ in SimResultStruct - // Velocity wrt ground - ignition.msgs.Vector3d rateUVW_ = 22; // rateU_, rateV_, rateW_ in SimResultStruct - // Water velocity - ignition.msgs.Vector3d ratePQR_ = 23; // rateP_, rateQ_, rateR_ in SimResultStruct - // for roll, pitch, yaw rates, respectively - - float northCurrent_ = 24; // +Y velocity in m / s - float eastCurrent_ = 25; // +X velocity in m / s - float vertCurrent_ = 26; // +Z velocity in m / s - float magneticVariation_ = 27; - float soundSpeed_ = 28; - float temperature_ = 29; // Celsius - float salinity_ = 30; // PSU - float density_ = 31; - repeated float values_ = 32; // Size 4 (0: chlorophyll in ug / L, 1: pressure in Pa) + + ignition.msgs.Vector3d pos_ = 19; // Vehicle's offset position in "world frame" + // (see above) with respect to its initial + // position. The vehicle starts facing West. + // Unit: meters + + ignition.msgs.Vector3d posRPH_ = 20; // Vehicle's offset orientation in "world frame" + // (see above) with respect to its initial + // orientation. The vehicle starts facing West. + // Unit: rad + + ignition.msgs.Vector3d posDot_ = 21; // Vehicle's instantaneous linear velocity in + // "world frame" (see above). Units: m / s + + ignition.msgs.Vector3d rateUVW_ = 22; // Vehicle's instantaneous linear velocity in + // "vehicle frame" (see above). Units: m / s + + ignition.msgs.Vector3d ratePQR_ = 23; // Vehicle's instantaneous angular velocity in + // "vehicle frame" (see above). Units: rad / s + + float northCurrent_ = 24; // Northward sea water velocity collected + // from current sensor. Unit: m/s. + + float eastCurrent_ = 25; // Eastward sea water velocity collected + // from current sensor. Unit: m/s. + + float vertCurrent_ = 26; // Not populated. + float magneticVariation_ = 27; // Not populated. + float soundSpeed_ = 28; // Not populated. + + float temperature_ = 29; // Data collected from temperature sensor. + // Unit: Celsius + + float salinity_ = 30; // Data collected from salinity sensor. + // Unit: PSU + + float density_ = 31; // Not populated + + repeated float values_ = 32; // Size 4 + // 0: Data collected from Chlorophyll + // sensor. Unit: ug / L + // 1: Pressure calculated from current + // depth and latitude. Unit: Pa } diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.cc b/lrauv_ignition_plugins/src/TethysCommPlugin.cc index 0d8c4d59..72996b29 100644 --- a/lrauv_ignition_plugins/src/TethysCommPlugin.cc +++ b/lrauv_ignition_plugins/src/TethysCommPlugin.cc @@ -41,6 +41,22 @@ #include "TethysCommPlugin.hh" +// East-North-Up to North-East-Down +#define ENU_TO_NED ignition::math::Pose3d(0, 0, 0, IGN_PI, 0, IGN_PI * 0.5) + +// Transform model frame to FSK +// Tethys model is currently setup as: +// X: Back +// Y: Starboard (right) +// Z: Up +// The controller expects FSK: +// X: Forward +// Y: Starboard (right) +// Z: Keel (down) +// We should make our lives easier by aligning the model with FSK, see +// https://github.com/osrf/lrauv/issues/80 +#define MODEL_TO_FSK ignition::math::Pose3d(0, 0, 0, 0, IGN_PI, 0) + using namespace tethys; /// \brief Calculates water pressure based on depth and latitude. @@ -164,15 +180,6 @@ void AddWorldLinearVelocity( } } -// Convert ROS coordinate frame convention (x forward, y left, z up) to -// FSK (x fore, y starboard right, z keel down). A 180 degree rotation wrt -// x (roll axis), i.e. flip signs on y (pitch axis) and z (heading axis). -void ROSToFSK(ignition::math::Vector3d &_vec) -{ - _vec.Y(-_vec.Y()); - _vec.Z(-_vec.Z()); -} - void TethysCommPlugin::Configure( const ignition::gazebo::Entity &_entity, const std::shared_ptr &_sdf, @@ -390,15 +397,10 @@ void TethysCommPlugin::SetupEntities( void TethysCommPlugin::CommandCallback( const lrauv_ignition_plugins::msgs::LRAUVCommand &_msg) { - // Lazy timestamp conversion just for printing - //if (std::chrono::seconds(int(floor(_msg.time_()))) - this->prevSubPrintTime - // > std::chrono::milliseconds(1000)) if (this->debugPrintout) { igndbg << "[" << this->ns << "] Received command: " << std::endl << _msg.DebugString() << std::endl; - - this->prevSubPrintTime = std::chrono::seconds(int(floor(_msg.time_()))); } // Rudder @@ -484,18 +486,22 @@ void TethysCommPlugin::PostUpdate( const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) { - ignition::gazebo::Link baseLink(this->modelLink); - auto modelPose = ignition::gazebo::worldPose(this->modelLink, _ecm); + if (_info.paused) + return; - // Publish state lrauv_ignition_plugins::msgs::LRAUVState stateMsg; + /////////////////////////////////// + // Header stateMsg.mutable_header()->mutable_stamp()->set_sec( std::chrono::duration_cast(_info.simTime).count()); stateMsg.mutable_header()->mutable_stamp()->set_nsec( int(std::chrono::duration_cast( _info.simTime).count()) - stateMsg.header().stamp().sec() * 1000000000); + /////////////////////////////////// + // Actuators + // TODO(anyone) Maybe angular velocity should come from ThrusterPlugin // Propeller angular velocity auto propAngVelComp = @@ -542,26 +548,26 @@ void TethysCommPlugin::PostUpdate( // Buoyancy position stateMsg.set_buoyancyposition_(this->buoyancyBladderVolume); - // Depth - stateMsg.set_depth_(-modelPose.Pos().Z()); - - // Roll, pitch, heading - auto rph = modelPose.Rot().Euler(); - // The LRAUV application seems not to use standard FSK when defining rotation. - // In particular it uses the following convention: - // - pitch +Ve - up - // - roll +Ve - starboard - // - yaw +Ve - starboard // Ignition/ROS coordinates are +Ve - port - rph.Z(-rph.Z()); - ignition::msgs::Set(stateMsg.mutable_rph_(), rph); - - // Speed - auto linearVelocity = - _ecm.Component( - this->modelLink); - stateMsg.set_speed_(linearVelocity->Data().Length()); + /////////////////////////////////// + // Position + + // Gazebo is using ENU, controller expects NED + auto modelPoseENU = ignition::gazebo::worldPose(this->modelLink, _ecm); + auto modelPoseNED = ENU_TO_NED * modelPoseENU; + if (!this->initialModelPoseNED) + { + this->initialModelPoseNED = modelPoseNED; + } + + stateMsg.set_depth_(-modelPoseENU.Pos().Z()); + + // w.r.t. initial pose in NED + auto poseOffsetNED = modelPoseNED * + this->initialModelPoseNED.value().Inverse(); + ignition::msgs::Set(stateMsg.mutable_pos_(), poseOffsetNED.Pos()); + ignition::msgs::Set(stateMsg.mutable_rph_(), poseOffsetNED.Rot().Euler()); + ignition::msgs::Set(stateMsg.mutable_posrph_(), poseOffsetNED.Rot().Euler()); - // Lat long auto latlon = ignition::gazebo::sphericalCoordinates(this->modelLink, _ecm); if (latlon) { @@ -569,24 +575,39 @@ void TethysCommPlugin::PostUpdate( stateMsg.set_longitudedeg_(latlon.value().Y()); } - // Robot position - ignition::math::Vector3d pos = modelPose.Pos(); - ROSToFSK(pos); - ignition::msgs::Set(stateMsg.mutable_pos_(), pos); + /////////////////////////////////// + // Velocity - // Robot linear velocity wrt ground - ignition::math::Vector3d veloGround = linearVelocity->Data(); - ROSToFSK(veloGround); - ignition::msgs::Set(stateMsg.mutable_posdot_(), veloGround); + auto linVelComp = + _ecm.Component( + this->modelLink); + if (nullptr != linVelComp) + { + auto linVelENU = linVelComp->Data(); + stateMsg.set_speed_(linVelENU.Length()); - // Water velocity - // rateUVW - // TODO(anyone) + // World frame in NED + auto linVelNED = ENU_TO_NED.Rot() * linVelENU; + ignition::msgs::Set(stateMsg.mutable_posdot_(), linVelNED); - // Rate of robot roll, pitch, yaw - // ratePQR - // TODO(anyone) + // Vehicle frame in FSK + auto linVelFSK = MODEL_TO_FSK.Rot() * linVelENU; + ignition::msgs::Set(stateMsg.mutable_rateuvw_(), linVelFSK); + } + + auto angVelComp = + _ecm.Component( + this->modelLink); + if (nullptr != angVelComp) + { + auto angVelENU = angVelComp->Data(); + + // Vehicle frame in FSK + auto angVelFSK = MODEL_TO_FSK.Rot() * angVelENU; + ignition::msgs::Set(stateMsg.mutable_ratepqr_(), angVelFSK); + } + /////////////////////////////////// // Sensor data stateMsg.set_salinity_(this->latestSalinity); stateMsg.set_temperature_(this->latestTemperature.Celsius()); @@ -595,7 +616,7 @@ void TethysCommPlugin::PostUpdate( double pressure = 0.0; if (latlon) { - auto calcPressure = pressureFromDepthLatitude(-modelPose.Pos().Z(), + auto calcPressure = pressureFromDepthLatitude(-modelPoseENU.Pos().Z(), latlon.value().X()); if (calcPressure >= 0) pressure = calcPressure; @@ -605,7 +626,8 @@ void TethysCommPlugin::PostUpdate( stateMsg.set_eastcurrent_(this->latestCurrent.X()); stateMsg.set_northcurrent_(this->latestCurrent.Y()); - stateMsg.set_vertcurrent_(this->latestCurrent.Z()); + // Not populating vertCurrent because we're not getting it from the science + // data this->statePub.Publish(stateMsg); diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.hh b/lrauv_ignition_plugins/src/TethysCommPlugin.hh index d0f46787..99730cc1 100644 --- a/lrauv_ignition_plugins/src/TethysCommPlugin.hh +++ b/lrauv_ignition_plugins/src/TethysCommPlugin.hh @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -190,8 +191,11 @@ namespace tethys /// sanity check private: std::chrono::steady_clock::duration prevPubPrintTime = std::chrono::steady_clock::duration::zero(); - private: std::chrono::steady_clock::duration prevSubPrintTime = - std::chrono::steady_clock::duration::zero(); + + /// \brief Initial model pose expressed in North-East-Down. + /// The first vehicle in simulation usually has zero pose expressed in ENU, + /// so the initial NED pose is non-zero. + private: std::optional initialModelPoseNED; /// Transport node for message passing private: ignition::transport::Node node; diff --git a/lrauv_ignition_plugins/test/test_state_msg.cc b/lrauv_ignition_plugins/test/test_state_msg.cc index 1e968fb3..a65dc6eb 100644 --- a/lrauv_ignition_plugins/test/test_state_msg.cc +++ b/lrauv_ignition_plugins/test/test_state_msg.cc @@ -27,20 +27,67 @@ #include "lrauv_command.pb.h" #include "lrauv_state.pb.h" +// Check actuators that don't change throughout the test +void unchangedActuators(const lrauv_ignition_plugins::msgs::LRAUVState &_msg) +{ + EXPECT_NEAR(0.0, _msg.elevatorangle_(), 1e-4); + EXPECT_NEAR(0.0, _msg.massposition_(), 1e-6); + EXPECT_NEAR(0.0005, _msg.buoyancyposition_(), 1e-6); +} + ////////////////////////////////////////////////// TEST_F(LrauvTestFixture, Command) { - // TODO(chapulina) Test other fields, see - // https://github.com/osrf/lrauv/pull/81 - // Initial state this->fixture->Server()->Run(true, 100, false); EXPECT_EQ(100, this->stateMsgs.size()); auto latest = this->stateMsgs.back(); + + // Actuators + unchangedActuators(latest); EXPECT_NEAR(0.0, latest.propomega_(), 1e-6); + EXPECT_NEAR(0.0, latest.rudderangle_(), 1e-6); + + // Position + // TODO(chapulina) Shouldn't start sinking + EXPECT_NEAR(0.0, latest.depth_(), 0.02); + EXPECT_NEAR(0.0, latest.rph_().x(), 0.007); + // TODO(chapulina) Shouldn't be pitching + EXPECT_NEAR(0.0, latest.rph_().y(), 1e-2); + EXPECT_NEAR(0.0, latest.rph_().z(), 1e-6); + // TODO(chapulina) Shouldn't be moving + EXPECT_NEAR(0.0, latest.speed_(), 1e-2); + EXPECT_NEAR(0.0, latest.latitudedeg_(), 1e-6); + EXPECT_NEAR(0.0, latest.longitudedeg_(), 1e-6); + EXPECT_NEAR(0.0, latest.pos_().x(), 1e-6); + EXPECT_NEAR(0.0, latest.pos_().y(), 1e-6); + EXPECT_NEAR(0.0, latest.pos_().z(), 0.015); + EXPECT_NEAR(0.0, latest.posrph_().x(), 0.007); + EXPECT_NEAR(0.0, latest.posrph_().y(), 1e-2); + EXPECT_NEAR(0.0, latest.posrph_().z(), 1e-6); + EXPECT_NEAR(0.0, latest.posdot_().x(), 1e-6); + EXPECT_NEAR(0.0, latest.posdot_().y(), 1e-5); + // TODO(chapulina) Shouldn't start sinking + EXPECT_NEAR(0.0, latest.posdot_().z(), 0.006); + EXPECT_NEAR(0.0, latest.rateuvw_().x(), 1e-5); + EXPECT_NEAR(0.0, latest.rateuvw_().y(), 1e-6); + EXPECT_NEAR(0.0, latest.rateuvw_().z(), 0.06); + EXPECT_NEAR(0.0, latest.ratepqr_().x(), 1e-6); + EXPECT_NEAR(0.0, latest.ratepqr_().y(), 0.002); + EXPECT_NEAR(0.0, latest.ratepqr_().z(), 1e-6); + // TODO(chapulina) Check sensor data once interpolation is complete + // EXPECT_NEAR(0.0, latest.northcurrent_(), 1e-6); + // EXPECT_NEAR(0.0, latest.eastcurrent_(), 1e-6); + // EXPECT_NEAR(0.0, latest.temperature_(), 1e-6); + // EXPECT_NEAR(0.0, latest.salinity_(), 1e-6); + // EXPECT_NEAR(0.0, latest.density_(), 1e-6); + // EXPECT_NEAR(0.0, latest.values_(0), 1e-6); + // EXPECT_NEAR(0.0, latest.values_(1), 1e-6); // 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_(10 * IGN_PI); @@ -54,7 +101,120 @@ TEST_F(LrauvTestFixture, Command) return this->stateMsgs.size() < 2000; }); + // The vehicle starts facing West and is propelled forward. + // We expect to see the position and velocity increase on: + // ENU: -X + // NED: -Y + // FSK: +X + latest = this->stateMsgs.back(); + + // Actuators + unchangedActuators(latest); + // TODO(chapulina) + EXPECT_NEAR(10.0 * IGN_PI, latest.propomega_(), 1e-3); + EXPECT_NEAR(0.0, latest.rudderangle_(), 1e-3); + + + // TODO(chapulina) Shouldn't start sinking + EXPECT_NEAR(0.0, latest.depth_(), 0.03); + EXPECT_NEAR(0.0, latest.rph_().x(), 1e-3); + // TODO(chapulina) Shouldn't be pitching + EXPECT_NEAR(0.0, latest.rph_().y(), 1e-2); + EXPECT_NEAR(0.0, latest.rph_().z(), 1e-5); + // TODO(chapulina) Shouldn't be moving + EXPECT_NEAR(1.0, latest.speed_(), 0.16); + EXPECT_NEAR(0.0, latest.latitudedeg_(), 1e-6); + EXPECT_NEAR(0.0, latest.longitudedeg_(), 1e-3); + EXPECT_NEAR(0.0, latest.pos_().x(), 1e-3); + EXPECT_GT(-30.0, latest.pos_().y()); + EXPECT_NEAR(0.0, latest.pos_().z(), 0.03); + EXPECT_NEAR(0.0, latest.posrph_().x(), 1e-3); + EXPECT_NEAR(0.0, latest.posrph_().y(), 1e-2); + EXPECT_NEAR(0.0, latest.posrph_().z(), 1e-5); + EXPECT_NEAR(0.0, latest.posdot_().x(), 1e-4); + EXPECT_NEAR(-1.0, latest.posdot_().y(), 0.16); + // TODO(chapulina) Shouldn't start sinking + EXPECT_NEAR(0.0, latest.posdot_().z(), 0.006); + EXPECT_NEAR(1.0, latest.rateuvw_().x(), 0.16); + EXPECT_NEAR(0.0, latest.rateuvw_().y(), 1e-4); + EXPECT_NEAR(0.0, latest.rateuvw_().z(), 1e-4); + EXPECT_NEAR(0.0, latest.ratepqr_().x(), 0.017); + EXPECT_NEAR(0.0, latest.ratepqr_().y(), 1e-5); + EXPECT_NEAR(0.0, latest.ratepqr_().z(), 1e-4); + // TODO(chapulina) Check sensor data once interpolation is complete + // EXPECT_NEAR(0.0, latest.northcurrent_(), 1e-6); + // EXPECT_NEAR(0.0, latest.eastcurrent_(), 1e-6); + // EXPECT_NEAR(0.0, latest.temperature_(), 1e-6); + // EXPECT_NEAR(0.0, latest.salinity_(), 1e-6); + // EXPECT_NEAR(0.0, latest.density_(), 1e-6); + // EXPECT_NEAR(0.0, latest.values_(0), 1e-6); + // EXPECT_NEAR(0.0, latest.values_(1), 1e-6); + + // Keep propelling vehicle forward + cmdMsg.set_propomegaaction_(10 * IGN_PI); + + // Rotate rudder counter-clockwise when looking from the top, which causes the + // vehicle to move in a clockwise arch + cmdMsg.set_rudderangleaction_(-0.5); + + // Neutral buoyancy + cmdMsg.set_buoyancyaction_(0.0005); + cmdMsg.set_dropweightstate_(true); + + // Run server until we collect more states + this->stateMsgs.clear(); + this->PublishCommandWhile(cmdMsg, [&]() + { + return this->stateMsgs.size() < 300; + }); + + // We expect the vehicle to rotate towards North, + // The vehicle starts facing West and is propelled forward. + // We expect to see the position and velocity increase on: + // ENU: -X, +Y and -yaw + // NED: -Y, +X and +yaw + // FSK: +X, +Y and +yaw latest = this->stateMsgs.back(); - EXPECT_NEAR(10.0 * IGN_PI, latest.propomega_(), 1e-6); + unchangedActuators(latest); + // TODO(chapulina) + // EXPECT_NEAR(10.0 * IGN_PI, latest.propomega_(), 1e-3); + EXPECT_NEAR(-0.5, latest.rudderangle_(), 0.06); + + // Position + // TODO(chapulina) Shouldn't start sinking + EXPECT_NEAR(0.0, latest.depth_(), 0.041); + EXPECT_NEAR(0.0, latest.rph_().x(), 0.0051); + // TODO(chapulina) Shouldn't be pitching + EXPECT_NEAR(0.0, latest.rph_().y(), 1e-2); + EXPECT_LT(1.2, latest.rph_().z()); + // TODO(chapulina) Shouldn't be moving + EXPECT_NEAR(1.0, latest.speed_(), 0.16); + EXPECT_NEAR(0.0, latest.latitudedeg_(), 1e-4); + EXPECT_NEAR(0.0, latest.longitudedeg_(), 1e-3); + EXPECT_LT(1.9, latest.pos_().x()); + EXPECT_GT(-30.0, latest.pos_().y()); + EXPECT_NEAR(0.0, latest.pos_().z(), 0.05); + EXPECT_NEAR(0.0, latest.posrph_().x(), 0.0051); + EXPECT_NEAR(0.0, latest.posrph_().y(), 1e-2); + EXPECT_LT(1.2, latest.posrph_().z()); + EXPECT_LT(0.75, latest.posdot_().x()); + // EXPECT_NEAR(-1.0, latest.posdot_().y(), 0.16); // ? + // TODO(chapulina) Shouldn't start sinking + EXPECT_NEAR(0.0, latest.posdot_().z(), 0.006); + EXPECT_LT(0.4, latest.rateuvw_().x()); + EXPECT_GT(1.0, latest.rateuvw_().x()); + EXPECT_LT(0.7, latest.rateuvw_().y()); + EXPECT_NEAR(0.0, latest.rateuvw_().z(), 0.006); + EXPECT_NEAR(0.0, latest.ratepqr_().x(), 0.017); + EXPECT_NEAR(0.0, latest.ratepqr_().y(), 0.017); + EXPECT_LT(0.2, latest.ratepqr_().z()); + // TODO(chapulina) Check sensor data once interpolation is complete + // EXPECT_NEAR(0.0, latest.northcurrent_(), 1e-6); + // EXPECT_NEAR(0.0, latest.eastcurrent_(), 1e-6); + // EXPECT_NEAR(0.0, latest.temperature_(), 1e-6); + // EXPECT_NEAR(0.0, latest.salinity_(), 1e-6); + // EXPECT_NEAR(0.0, latest.density_(), 1e-6); + // EXPECT_NEAR(0.0, latest.values_(0), 1e-6); + // EXPECT_NEAR(0.0, latest.values_(1), 1e-6); } diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index 202555be..fc538269 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -180,6 +180,19 @@ false + + + + + + + false + 5 + 5 + floating + false + + @@ -309,5 +322,19 @@ tethys_equipped + + ENU + true + 5 5 0 0 0 0 + axes + + + + NED + true + 0 0 0 180 0 90 + axes + +