tethys_equipped face North by default (#154)

* Spawn vehicle at requested orientation Signed-off-by: Louise Poubel <[email protected]> * NED Signed-off-by: Louise Poubel <[email protected]> * Vehicle face North Signed-off-by: Louise Poubel <[email protected]> * More tweaks Signed-off-by: Louise Poubel <[email protected]> * PR feedback Signed-off-by: Louise Poubel <[email protected]> * fix sink test Signed-off-by: Louise Poubel <[email protected]> * increase tolerances Signed-off-by: Louise Poubel <[email protected]> Co-authored-by: Arjo Chakravarty <[email protected]>
osrf · Feb 3, 2022 · 3ecc9e0 · 3ecc9e0
1 parent 9f694be
commit 3ecc9e0
Show file tree

Hide file tree

Showing 15 changed files with 274 additions and 73 deletions.
diff --git a/lrauv_description/models/tethys/model.sdf.in b/lrauv_description/models/tethys/model.sdf.in
@@ -3,6 +3,11 @@
   Development of this module has been funded by the Monterey Bay Aquarium
   Research Institute (MBARI) and the David and Lucile Packard Foundation
 -->
+
+<!--
+  This model is oriented so that it faces -X. When inserted into the world
+  (which uses ENU) with a zero pose, it faces West.
+-->
 <sdf version="1.6">
   <model name="tethys">
     <!-- Body -->

diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf
@@ -6,6 +6,8 @@
 <sdf version="1.9">
   <model name="tethys">
     <include merge="true">
+      <!-- align with NED (face North) -->
+      <pose degrees="true">0 0 0   0 0 -90</pose>
       <uri>tethys</uri>
 
       <!-- Sensors -->

diff --git a/lrauv_ignition_plugins/proto/lrauv_state.proto b/lrauv_ignition_plugins/proto/lrauv_state.proto
@@ -33,7 +33,22 @@ import "ignition/msgs/header.proto";
 import "ignition/msgs/vector3d.proto";
 
 // Mirrors SimResultStruct
-// \TODO(chapulina) Update coordinate frame convention for all pose fields:
+//
+// Coordinate frame conventions:
+//
+// Reference frames:
+//
+// * 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
+//
+// TODO(chapulina)
+// * Vehicle frame: FSK 
+//
 message LRAUVState
 {
   /// \brief Optional header data
@@ -100,10 +115,10 @@ message LRAUVState
   /// \brief Not populated
   ignition.msgs.Vector3d force_   = 18;
 
-  /// \brief \TODO(chapulina)
+  /// \brief Vehicle's position in "world frame" (see above) Unit: meters
   ignition.msgs.Vector3d pos_     = 19;
 
-  /// \brief \TODO(chapulina)
+  /// \brief Vehicle's orientation in "world frame" (see above) Unit: rad
   ignition.msgs.Vector3d posRPH_  = 20;
 
   /// \brief \TODO(chapulina)

diff --git a/lrauv_ignition_plugins/src/ReferenceAxis.cc b/lrauv_ignition_plugins/src/ReferenceAxis.cc
@@ -265,7 +265,7 @@ void ReferenceAxisPrivate::OnPreRender()
     vehicleVis->AddChild(fskVis);
     // TODO(chapulina) This rotation won't be needed if we update the model
     // https://github.com/osrf/lrauv/issues/80
-    fskVis->SetLocalRotation(0, IGN_PI, 0);
+    fskVis->SetLocalRotation(IGN_PI, 0, IGN_PI * 0.5);
 
     // Ogre2 doesn't support text yet
     auto textGeom = this->scene->CreateText();

diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.cc b/lrauv_ignition_plugins/src/TethysCommPlugin.cc
@@ -173,6 +173,15 @@ void ROSToFSK(ignition::math::Vector3d &_vec)
   _vec.Z(-_vec.Z());
 }
 
+/// \brief Convert a pose in ENU to NED.
+/// \param[in] _enu A pose in ENU (Gazebo's world frame)
+/// \return A pose in NED (what the controller expects)
+ignition::math::Pose3d ENUToNED(const ignition::math::Pose3d &_enu)
+{
+  return {_enu.Y(), _enu.X(), -_enu.Z(),
+          _enu.Pitch(), _enu.Roll(), -_enu.Yaw()};
+}
+
 void TethysCommPlugin::Configure(
   const ignition::gazebo::Entity &_entity,
   const std::shared_ptr<const sdf::Element> &_sdf,
@@ -371,9 +380,11 @@ void TethysCommPlugin::SetupEntities(
     this->massShifterJointName = _sdf->Get<std::string>("mass_shifter_joint");
   }
 
+  this->modelEntity = _entity;
+
   auto model = ignition::gazebo::Model(_entity);
 
-  this->modelLink = model.LinkByName(_ecm, this->baseLinkName);
+  this->baseLink = model.LinkByName(_ecm, this->baseLinkName);
 
   this->thrusterLink = model.LinkByName(_ecm, this->thrusterLinkName);
   this->thrusterJoint = model.JointByName(_ecm, thrusterJointName);
@@ -383,12 +394,12 @@ void TethysCommPlugin::SetupEntities(
   this->massShifterJoint = model.JointByName(_ecm, this->massShifterJointName);
 
   AddAngularVelocityComponent(this->thrusterLink, _ecm);
-  AddWorldPose(this->modelLink, _ecm);
+  AddWorldPose(this->baseLink, _ecm);
   AddJointPosition(this->rudderJoint, _ecm);
   AddJointPosition(this->elevatorJoint, _ecm);
   AddJointPosition(this->massShifterJoint, _ecm);
   AddJointVelocity(this->thrusterJoint, _ecm);
-  AddWorldLinearVelocity(this->modelLink, _ecm);
+  AddWorldLinearVelocity(this->baseLink, _ecm);
 }
 
 void TethysCommPlugin::CommandCallback(
@@ -476,12 +487,16 @@ void TethysCommPlugin::PostUpdate(
   // 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
   auto propAngVelComp =
     _ecm.Component<ignition::gazebo::components::JointVelocity>(thrusterJoint);
   if (propAngVelComp->Data().size() != 1)
@@ -526,38 +541,34 @@ 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());
+  ///////////////////////////////////
+  // Position
 
-  // 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);
+  // Gazebo is using ENU, controller expects NED
+  auto modelPoseENU = ignition::gazebo::worldPose(this->modelEntity, _ecm);
+  auto modelPoseNED = ENUToNED(modelPoseENU);
 
-  // Speed
-  auto linearVelocity =
-    _ecm.Component<ignition::gazebo::components::WorldLinearVelocity>(
-    this->modelLink);
-  stateMsg.set_speed_(linearVelocity->Data().Length());
+  stateMsg.set_depth_(-modelPoseENU.Pos().Z());
+
+  ignition::msgs::Set(stateMsg.mutable_pos_(), modelPoseNED.Pos());
+  ignition::msgs::Set(stateMsg.mutable_rph_(), modelPoseNED.Rot().Euler());
+  ignition::msgs::Set(stateMsg.mutable_posrph_(), modelPoseNED.Rot().Euler());
 
-  // Lat long
-  auto latlon = ignition::gazebo::sphericalCoordinates(this->modelLink, _ecm);
+  auto latlon = ignition::gazebo::sphericalCoordinates(this->modelEntity, _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
+
+  // Speed
+  auto linearVelocity =
+    _ecm.Component<ignition::gazebo::components::WorldLinearVelocity>(
+    this->baseLink);
+  stateMsg.set_speed_(linearVelocity->Data().Length());
 
   // Robot linear velocity wrt ground
   ignition::math::Vector3d veloGround = linearVelocity->Data();
@@ -567,7 +578,7 @@ void TethysCommPlugin::PostUpdate(
   // Water velocity
   // rateUVW
   // TODO(arjo): include currents in water velocity?
-  auto localVel = modelPose.Rot().Inverse() * veloGround;
+  auto localVel = modelPoseENU.Rot().Inverse() * veloGround;
   //TODO(louise) check for translation/position effects
   ROSToFSK(localVel);
   ignition::msgs::Set(stateMsg.mutable_rateuvw_(), localVel);
@@ -587,7 +598,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;

diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.hh b/lrauv_ignition_plugins/src/TethysCommPlugin.hh
@@ -197,8 +197,11 @@ namespace tethys
     /// Transport node for message passing
     private: ignition::transport::Node node;
 
+    /// The model's entity
+    private: ignition::gazebo::Entity modelEntity;
+
     /// The model's base link
-    private: ignition::gazebo::Entity modelLink;
+    private: ignition::gazebo::Entity baseLink;
 
     /// The thruster link
     private: ignition::gazebo::Entity thrusterLink;

diff --git a/lrauv_ignition_plugins/test/test_buoyancy_action.cc b/lrauv_ignition_plugins/test/test_buoyancy_action.cc
@@ -56,15 +56,16 @@ TEST_F(LrauvTestFixture, Sink)
   // The vehicle sinks
   EXPECT_GT(targetZ, this->tethysPoses.back().Pos().Z());
 
-  // It pitches backwards because the buoyancy engine is closer to the back
-  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Pitch(), 0.13);
+  // It pitches (world roll) backwards because the buoyancy engine is closer to
+  // the back
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Roll(), 0.1);
 
-  // Roll and yaw aren't affected
-  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Roll(), 1e-6);
+  // Roll and yaw (in world frame) aren't affected
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Pitch(), 1e-6);
   EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Yaw(), 1e-6);
 
   // TODO(anyone) Fix residual movement
-  // https://github.com/osrf/lrauv/issues/132
+  // https://github.com/osrf/lrauv/issues/47
   EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().X(), 2.5);
   EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().Y(), 1.1);
 }

diff --git a/lrauv_ignition_plugins/test/test_controller.cc b/lrauv_ignition_plugins/test/test_controller.cc
@@ -48,20 +48,20 @@ TEST_F(LrauvTestFixture, Command)
   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{-100.0};
+  // point ahead (the robot moves towards +Y)
+  double targetY{100.0};
   this->PublishCommandWhile(cmdMsg, [&]()
   {
-    return this->tethysPoses.back().Pos().X() > targetX;
+    return this->tethysPoses.back().Pos().Y() < targetY;
   });
 
   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().X(), 1e-3);
+  EXPECT_LT(targetY, this->tethysPoses.back().Pos().Y());
   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);

diff --git a/lrauv_ignition_plugins/test/test_mass_shifter.cc b/lrauv_ignition_plugins/test/test_mass_shifter.cc
@@ -31,27 +31,33 @@
 //////////////////////////////////////////////////
 TEST_F(LrauvTestFixture, MassShifterTilt)
 {
-  // Check initial pitch
+  // Check initial orientation
   this->fixture->Server()->Run(true, 100, false);
   EXPECT_EQ(100, this->iterations);
   EXPECT_EQ(100, this->tethysPoses.size());
-  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Pitch(), 0.01);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Roll(), 1e-6);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Pitch(), 1e-6);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Yaw(), 1e-6);
 
   // Tell the vehicle to tilt downward by moving the mass forward (positive command)
   lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg;
   cmdMsg.set_masspositionaction_(0.01);
   cmdMsg.set_buoyancyaction_(0.0005);
 
   // Run server until the command is processed and the model tilts to a
-  // certain pitch. Negative pitch means vehicle's nose is pointing down.
-  double targetPitch{-0.15};
+  // certain angle.
+  // Because the vehicle is facing North (+Y in ENU), the nose tilts down with
+  // negative roll in the world frame (about East axis)
+  double targetWorldRoll{-0.15};
   this->PublishCommandWhile(cmdMsg, [&]()
   {
-    return this->tethysPoses.back().Rot().Pitch() > targetPitch;
+    return this->tethysPoses.back().Rot().Roll() > targetWorldRoll;
   });
 
   EXPECT_LT(100, this->iterations);
   EXPECT_LT(100, this->tethysPoses.size());
-  EXPECT_GT(targetPitch, this->tethysPoses.back().Rot().Pitch());
+  EXPECT_GT(targetWorldRoll, this->tethysPoses.back().Rot().Roll());
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Pitch(), 1e-6);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Yaw(), 1e-6);
 }
 
diff --git a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc
@@ -35,11 +35,16 @@ TEST_F(LrauvTestFixture, DepthVBS)
 
   ignition::common::chdir(std::string(LRAUV_APP_PATH));
 
-  // Get initial X
+  // Get initial pose (facing North)
   this->fixture->Server()->Run(true, 10, false);
   EXPECT_EQ(10, this->iterations);
   EXPECT_EQ(10, this->tethysPoses.size());
   EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().X(), 1e-6);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().Y(), 1e-6);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().Z(), 1e-6);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Roll(), 1e-6);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Pitch(), 1e-6);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Yaw(), 1e-6);
 
   // Run non blocking
   this->fixture->Server()->Run(false, 0, false);
@@ -77,14 +82,12 @@ TEST_F(LrauvTestFixture, DepthVBS)
   ignition::math::Vector3d maxVel(0, 0, 0);
   for (int i = 1; i <= this->tethysPoses.size(); i ++)
   {
-    // Vehicle roll should be constant
-    EXPECT_NEAR(tethysPoses[i].Rot().Euler().X(), 0, 1e-2);
+    // Vehicle roll (about +Y since vehicle is facing North) should be constant
+    EXPECT_NEAR(tethysPoses[i].Rot().Euler().Y(), 0, 1e-2);
 
-    // Vehicle should not go vertical
-    EXPECT_LT(tethysPoses[i].Rot().Euler().Y(), IGN_DTOR(40));
-
-    // Vehicle should not go vertical
-    EXPECT_GT(tethysPoses[i].Rot().Euler().Y(), IGN_DTOR(-40));
+    // Vehicle should not go vertical when tilting nose
+    EXPECT_LT(tethysPoses[i].Rot().Euler().X(), IGN_DTOR(40));
+    EXPECT_GT(tethysPoses[i].Rot().Euler().X(), IGN_DTOR(-40));
 
     // Vehicle should not exceed 20m depth
     // Note: Although the mission has a target depth of 10m, the vehicle has