Fix coordinate frames on state message

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
 }

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,62 @@ 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;  // Density of the surrounding water in Kg / m ^ 3
-  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. Unit: meters
+
+  ignition.msgs.Vector3d posRPH_  = 20;  // Vehicle's offset orientation in "world frame"
+                                         // (see above) with respect to its initial
+                                         // orientation. 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;  // Density of the surrounding water in kg / m ^ 3
+
+  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
 }

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<const sdf::Element> &_sdf,
@@ -394,15 +401,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
@@ -491,15 +493,19 @@ void TethysCommPlugin::PostUpdate(
   if (_info.paused)
     return;
 
-  // Publish state
   lrauv_ignition_plugins::msgs::LRAUVState stateMsg;
 
+  ///////////////////////////////////
+  // Header
   stateMsg.mutable_header()->mutable_stamp()->set_sec(
     std::chrono::duration_cast<std::chrono::seconds>(_info.simTime).count());
   stateMsg.mutable_header()->mutable_stamp()->set_nsec(
     int(std::chrono::duration_cast<std::chrono::nanoseconds>(
     _info.simTime).count()) - stateMsg.header().stamp().sec() * 1000000000);
 
+  ///////////////////////////////////
+  // Actuators
+
   // TODO(anyone) Maybe angular velocity should come from ThrusterPlugin
   // Propeller angular velocity
   auto propAngVelComp =
@@ -546,56 +552,78 @@ void TethysCommPlugin::PostUpdate(
   // Buoyancy position
   stateMsg.set_buoyancyposition_(this->buoyancyBladderVolume);
 
-  // Depth
-  auto modelPose = ignition::gazebo::worldPose(this->modelLink, _ecm);
-  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
+  ///////////////////////////////////
+  // 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());
+
+  // TODO(chapulina) The controller fails with the orientation above. Example error:
+  //
+  // [VerticalControl](CRITICAL): Excessive depth excursion=10.573917 m, failToGoUpDepth_=17.503380 m, depthRate=0.211946 m/s, pitch =0.114867 deg.
+  //
+  // The difference between poseOffsetNED.Rot() and rph is that roll and pitch are swapped.
+  auto rph = modelPoseENU.Rot().Euler();
   rph.Z(-rph.Z());
   ignition::msgs::Set(stateMsg.mutable_rph_(), rph);
+  ignition::msgs::Set(stateMsg.mutable_posrph_(), rph);
 
-  // Speed
-  auto linearVelocity =
-    _ecm.Component<ignition::gazebo::components::WorldLinearVelocity>(
-    this->modelLink);
-  stateMsg.set_speed_(linearVelocity->Data().Length());
+//ignerr << "NED: " << poseOffsetNED.Rot() << " -- ENU with flipped Z: " << " " << rph << std::endl;
 
-  // Lat long
   auto latlon = ignition::gazebo::sphericalCoordinates(this->modelLink, _ecm);
   if (latlon)
   {
     stateMsg.set_latitudedeg_(latlon.value().X());
     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<ignition::gazebo::components::WorldLinearVelocity>(
+    this->modelLink);
+  if (nullptr != linVelComp)
+  {
+    auto linVelENU = linVelComp->Data();
+    stateMsg.set_speed_(linVelENU.Length());
 
-  // Water velocity
-  // rateUVW
-  // TODO(arjo): include currents in water velocity?
-  auto localVel = modelPose.Rot().Inverse() * veloGround;
-  //TODO(louise) check for translation/position effects
-  ROSToFSK(localVel);
-  ignition::msgs::Set(stateMsg.mutable_rateuvw_(), localVel);
+    // 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<ignition::gazebo::components::WorldAngularVelocity>(
+    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());
@@ -607,7 +635,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;
@@ -617,7 +645,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);
 

lrauv_ignition_plugins/src/TethysCommPlugin.hh

@@ -27,6 +27,7 @@
 
 #include <ignition/gazebo/Link.hh>
 #include <ignition/gazebo/System.hh>
+#include <ignition/math/Pose3.hh>
 #include <ignition/math/Temperature.hh>
 #include <ignition/transport/Node.hh>
 
@@ -193,8 +194,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<ignition::math::Pose3d> initialModelPoseNED;
 
     /// Transport node for message passing
     private: ignition::transport::Node node;