Update posDot state velocity to NED

lrauv_ignition_plugins/proto/lrauv_state.proto

@@ -100,7 +100,8 @@ message LRAUVState
   /// \brief Duplicate of posRPH_
   ignition.msgs.Vector3d rph_     = 13;
 
-  /// \brief TODO(chapulina)
+  /// \brief Magnitude of vehicle's linear velocity in the world frame.
+  /// It's always positive. Unit: m/s
   float speed_                    = 14;
 
   /// \brief Latitude in degrees
@@ -121,7 +122,8 @@ message LRAUVState
   /// \brief Vehicle's orientation in "world frame" (see above) Unit: rad
   ignition.msgs.Vector3d posRPH_  = 20;
 
-  /// \brief \TODO(chapulina)
+  /// \brief Vehicle's instantaneous linear velocity in "world frame" 
+  /// (see above). Units: m / s
   ignition.msgs.Vector3d posDot_  = 21;
 
   /// \brief \TODO(chapulina)

lrauv_ignition_plugins/src/TethysCommPlugin.cc

@@ -182,6 +182,12 @@ ignition::math::Pose3d ENUToNED(const ignition::math::Pose3d &_enu)
           _enu.Pitch(), _enu.Roll(), -_enu.Yaw()};
 }
 
+// Convert a vector in ENU to NED.
+ignition::math::Vector3d ENUToNED(const ignition::math::Vector3d &_enu)
+{
+  return {_enu.Y(), _enu.X(), -_enu.Z()};
+}
+
 void TethysCommPlugin::Configure(
   const ignition::gazebo::Entity &_entity,
   const std::shared_ptr<const sdf::Element> &_sdf,
@@ -565,20 +571,20 @@ void TethysCommPlugin::PostUpdate(
   // Velocity
 
   // Speed
-  auto linearVelocity =
+  auto linVelComp =
     _ecm.Component<ignition::gazebo::components::WorldLinearVelocity>(
     this->baseLink);
-  stateMsg.set_speed_(linearVelocity->Data().Length());
+  auto linVelENU = linVelComp->Data();
+  stateMsg.set_speed_(linVelENU.Length());
 
-  // Robot linear velocity wrt ground
-  ignition::math::Vector3d veloGround = linearVelocity->Data();
-  ROSToFSK(veloGround);
-  ignition::msgs::Set(stateMsg.mutable_posdot_(), veloGround);
+  // Velocity in NED world frame
+  auto linVelNED = ENUToNED(linVelENU);
+  ignition::msgs::Set(stateMsg.mutable_posdot_(), linVelNED);
 
   // Water velocity
   // rateUVW
   // TODO(arjo): include currents in water velocity?
-  auto localVel = modelPoseENU.Rot().Inverse() * veloGround;
+  auto localVel = modelPoseENU.Rot().Inverse() * linVelENU;
   //TODO(louise) check for translation/position effects
   ROSToFSK(localVel);
   ignition::msgs::Set(stateMsg.mutable_rateuvw_(), localVel);

lrauv_ignition_plugins/test/test_state_msg.cc

@@ -81,6 +81,14 @@ TEST_F(LrauvTestFixture, State)
   EXPECT_NEAR(0.0, latest.posrph_().y(), 1e-6);
   EXPECT_NEAR(0.0, latest.posrph_().z(), 1e-6);
 
+  // Velocity
+  EXPECT_NEAR(0.0, latest.speed_(), 1e-6);
+
+  // NED world frame
+  EXPECT_NEAR(0.0, latest.posdot_().x(), 1e-6);
+  EXPECT_NEAR(0.0, latest.posdot_().y(), 1e-6);
+  EXPECT_NEAR(0.0, latest.posdot_().z(), 1e-6);
+
   // TODO(chapulina) Check sensor data once interpolation is complete
   // https://github.com/osrf/lrauv/issues/5
 
@@ -128,6 +136,14 @@ TEST_F(LrauvTestFixture, State)
   EXPECT_NEAR(0.0, latest.posrph_().y(), 1e-6);
   EXPECT_NEAR(0.0, latest.posrph_().z(), 1e-5);
 
+  // Velocity
+  EXPECT_NEAR(1.0, latest.speed_(), 0.02);
+
+  // NED world frame: vehicle is going North
+  EXPECT_NEAR(1.0, latest.posdot_().x(), 0.02);
+  EXPECT_NEAR(0.0, latest.posdot_().y(), 1e-4);
+  EXPECT_NEAR(0.0, latest.posdot_().z(), 1e-6);
+
   // Keep propelling vehicle forward
   cmdMsg.set_propomegaaction_(10 * IGN_PI);
 
@@ -175,6 +191,14 @@ TEST_F(LrauvTestFixture, State)
   EXPECT_NEAR(0.0, latest.posrph_().y(), 1e-3);
   EXPECT_LT(0.5, latest.posrph_().z());
 
+  // Velocity
+  EXPECT_NEAR(1.0, latest.speed_(), 0.15);
+
+  // NED world frame: vehicle is going North East
+  EXPECT_LT(0.6, latest.posdot_().x());
+  EXPECT_LT(0.3, latest.posdot_().y());
+  EXPECT_NEAR(0.0, latest.posdot_().z(), 1e-3);
+
   // Stop propelling and rotating vehicle
   cmdMsg.set_propomegaaction_(0);
   cmdMsg.set_rudderangleaction_(0);
@@ -204,5 +228,9 @@ TEST_F(LrauvTestFixture, State)
 
   // NED world frame: higher Z is deeper
   EXPECT_LT(0.3, latest.pos_().z());
+
+  // Velocity
+  // NED world frame: sinking to higher Z
+  EXPECT_LT(0.02, latest.posdot_().z());
 }