IMU sensor API to get world ref frame and heading offset

include/ignition/sensors/ImuSensor.hh

@@ -35,6 +35,27 @@ namespace ignition
   {
     // Inline bracket to help doxygen filtering.
     inline namespace IGNITION_SENSORS_VERSION_NAMESPACE {
+
+    // \brief Reference frames enum
+    enum class IGNITION_SENSORS_VISIBLE WorldFrameEnumType
+    {
+      // \brief NONE : To be used only when <localization>
+      // reference orintation tag is empty.
+      NONE = 0,
+
+      // \brief ENU (East-North-Up): x - East, y - North, z - Up.
+      ENU = 1,
+
+      // \brief NED (North-East-Down): x - North, y - East, z - Down.
+      NED = 2,
+
+      // \brief NWU (North-West-Up): x - North, y - West, z - Up.
+      NWU = 3,
+
+      // \brief CUSTOM : The frame is described using custom_rpy tag.
+      CUSTOM = 4
+    };
+
     //
     /// \brief forward declarations
     class ImuSensorPrivate;
@@ -136,6 +157,13 @@ namespace ignition
       /// \return Gravity vectory in meters per second squared.
       public: math::Vector3d Gravity() const;
 
+      /// \brief Specify the rotation offset of the coordinates of the World
+      //  frame relative to a geo-referenced frame
+      /// \param[in] _rot rotation offset
+      /// \param[in] _relativeTo world frame orintation, ENU by default
+      public: void SetWorldFrameOrientation(
+        const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo);
+
       IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING
       /// \brief Data pointer for private data
       /// \internal

src/ImuSensor.cc

@@ -71,6 +71,23 @@ class ignition::sensors::ImuSensorPrivate
   /// \brief Flag for if time has been initialized
   public: bool timeInitialized = false;
 
+  /// \brief Orientation of world frame relative to a specified frame
+  public: ignition::math::Quaterniond worldRelativeOrientation;
+
+  /// \brief Frame relative-to which the worldRelativeOrientation
+  //  is defined
+  public: WorldFrameEnumType worldFrameRelativeTo = WorldFrameEnumType::ENU;
+
+  /// \brief Frame relative-to which the sensor orientation is reported
+  public: WorldFrameEnumType sensorOrientationRelativeTo
+         = WorldFrameEnumType::NONE;
+
+  /// \brief Frame realtive to which custom_rpy is specified
+  public: std::string CustomRpyParentFrame;
+
+  /// \brief Quaternion for to store custom_rpy
+  public: ignition::math::Quaterniond CustomRpyQuaternion;
+
   /// \brief Previous update time step.
   public: std::chrono::steady_clock::duration prevStep
     {std::chrono::steady_clock::duration::zero()};
@@ -116,24 +133,6 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
-  // Set orientation reference frame
-  // TODO(adityapande-1995) : Add support for named frames like
-  // ENU using ign-gazebo
-  if (_sdf.ImuSensor()->Localization() == "CUSTOM")
-  {
-    if (_sdf.ImuSensor()->CustomRpyParentFrame() == "")
-    {
-      this->SetOrientationReference(ignition::math::Quaterniond(
-        _sdf.ImuSensor()->CustomRpy()));
-    }
-    else
-    {
-      ignwarn << "custom_rpy parent frame must be set to empty "
-                "string. Setting it to any other frame is not "
-                "supported yet." << std::endl;
-    }
-  }
-
   if (this->Topic().empty())
     this->SetTopic("/imu");
 
@@ -166,6 +165,26 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf)
     }
   }
 
+  std::string localization = _sdf.ImuSensor()->Localization();
+
+  if (localization == "ENU")
+  {
+    this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::ENU;
+  } else if (localization == "NED") {
+    this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NED;
+  } else if (localization == "NWU") {
+    this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NWU;
+  } else if (localization == "CUSTOM") {
+    this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::CUSTOM;
+  } else {
+    this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NONE;
+  }
+
+  this->dataPtr->CustomRpyParentFrame =
+      _sdf.ImuSensor()->CustomRpyParentFrame();
+  this->dataPtr->CustomRpyQuaternion = ignition::math::Quaterniond(
+      _sdf.ImuSensor()->CustomRpy());
+
   this->dataPtr->initialized = true;
   return true;
 }
@@ -292,6 +311,75 @@ math::Pose3d ImuSensor::WorldPose() const
   return this->dataPtr->worldPose;
 }
 
+//////////////////////////////////////////////////
+void ImuSensor::SetWorldFrameOrientation(
+  const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo)
+{
+  this->dataPtr->worldRelativeOrientation = _rot;
+  this->dataPtr->worldFrameRelativeTo = _relativeTo;
+
+  // Set orientation reference frame if custom_rpy was supplied
+  if (this->dataPtr->sensorOrientationRelativeTo == WorldFrameEnumType::CUSTOM)
+  {
+    if (this->dataPtr->CustomRpyParentFrame == "")
+    {
+      this->SetOrientationReference(this->dataPtr->worldRelativeOrientation *
+        this->dataPtr->CustomRpyQuaternion);
+    }
+    else
+    {
+      ignwarn << "custom_rpy parent frame must be set to empty "
+                "string. Setting it to any other frame is not "
+                "supported yet." << std::endl;
+    }
+    return;
+  }
+
+  // Table to hold frame transformations
+  static const std::map<WorldFrameEnumType,
+    std::map<WorldFrameEnumType, ignition::math::Quaterniond>>
+      transformTable =
+    {
+      {WorldFrameEnumType::ENU,
+        {
+          {WorldFrameEnumType::ENU, ignition::math::Quaterniond(0, 0, 0)},
+          {WorldFrameEnumType::NED, ignition::math::Quaterniond(
+            IGN_PI, 0, IGN_PI/2)},
+          {WorldFrameEnumType::NWU, ignition::math::Quaterniond(
+            0, 0, IGN_PI/2)},
+        }
+      },
+      {WorldFrameEnumType::NED,
+        {
+          {WorldFrameEnumType::ENU, ignition::math::Quaterniond(
+            IGN_PI, 0, IGN_PI/2).Inverse()},
+          {WorldFrameEnumType::NED, ignition::math::Quaterniond(0, 0, 0)},
+          {WorldFrameEnumType::NWU, ignition::math::Quaterniond(
+            -IGN_PI, 0, 0)},
+        }
+      },
+      {WorldFrameEnumType::NWU,
+        {
+          {WorldFrameEnumType::ENU, ignition::math::Quaterniond(
+            0, 0, -IGN_PI/2)},
+          {WorldFrameEnumType::NED, ignition::math::Quaterniond(IGN_PI, 0, 0)},
+          {WorldFrameEnumType::NWU, ignition::math::Quaterniond(0, 0, 0)},
+        }
+      }
+    };
+
+  if (this->dataPtr->sensorOrientationRelativeTo != WorldFrameEnumType::NONE &&
+      this->dataPtr->sensorOrientationRelativeTo != WorldFrameEnumType::CUSTOM)
+  {
+    // A valid named localization tag is supplied in the sdf
+    auto tranformation =
+      transformTable.at(this->dataPtr->worldFrameRelativeTo).at
+      (this->dataPtr->sensorOrientationRelativeTo);
+    this->SetOrientationReference(this->dataPtr->worldRelativeOrientation *
+      tranformation);
+  }
+}
+
 //////////////////////////////////////////////////
 void ImuSensor::SetOrientationReference(const math::Quaterniond &_orient)
 {

src/ImuSensor_TEST.cc

@@ -436,6 +436,8 @@ TEST(ImuSensor_TEST, OrientationReference)
   // Create an ImuSensor
   auto sensor = mgr.CreateSensor<ignition::sensors::ImuSensor>(
       imuSDF);
+  sensor->SetWorldFrameOrientation(math::Quaterniond(0, 0, 0),
+    sensors::WorldFrameEnumType::ENU);
 
   // Make sure the above dynamic cast worked.
   ASSERT_NE(nullptr, sensor);
@@ -496,6 +498,9 @@ TEST(ImuSensor_TEST, CustomRpyParentFrame)
   auto sensor = mgr.CreateSensor<ignition::sensors::ImuSensor>(
       imuSDF);
 
+  sensor->SetWorldFrameOrientation(math::Quaterniond(0, 0, 0),
+    sensors::WorldFrameEnumType::ENU);
+
   // Make sure the above dynamic cast worked.
   ASSERT_NE(nullptr, sensor);
 
@@ -511,6 +516,174 @@ TEST(ImuSensor_TEST, CustomRpyParentFrame)
   EXPECT_EQ(defultOrientValue, sensor->Orientation());
 }
 
+sdf::ElementPtr generateSensor(std::string namedFrame)
+{
+  std::ostringstream stream;
+  stream
+    << "<?xml version='1.0'?>"
+    << "<sdf version='1.6'>"
+    << " <model name='m1'>"
+    << "  <link name='link1'>"
+    << "    <sensor name='imu_sensor' type='imu'>"
+    << "      <topic>imu_test</topic>"
+    << "      <update_rate>1</update_rate>"
+    << "      <imu>"
+    << "      <orientation_reference_frame>"
+    << "      <localization>"+namedFrame+"</localization>"
+    << "      </orientation_reference_frame>"
+    << "      </imu>"
+    << "      <always_on>1</always_on>"
+    << "      <visualize>true</visualize>"
+    << "    </sensor>"
+    << "  </link>"
+    << " </model>"
+    << "</sdf>";
+
+  sdf::SDFPtr sdfParsed(new sdf::SDF());
+  sdf::init(sdfParsed);
+  if (!sdf::readString(stream.str(), sdfParsed))
+  {
+    return sdf::ElementPtr();
+  }
+  else
+  {
+    return sdfParsed->Root()->GetElement("model")->GetElement("link")
+      ->GetElement("sensor");
+  }
+}
+
+//////////////////////////////////////////////////
+TEST(ImuSensor_TEST, NamedFrameOrientationReference)
+{
+  // Create a sensor manager
+  ignition::sensors::Manager mgr;
+
+  math::Quaterniond orientValue;
+
+  // A. Localization tag is set to ENU
+  // ---------------------------------
+  auto sensorENU = mgr.CreateSensor<ignition::sensors::ImuSensor>(
+      generateSensor("ENU"));
+  ASSERT_NE(nullptr, sensorENU);
+
+  // Case A.1 : Localization ref: ENU, World : ENU
+  // Sensor aligns with ENU, we're measuring wrt ENU
+  sensorENU->SetWorldFrameOrientation(math::Quaterniond(0, 0, 0),
+    sensors::WorldFrameEnumType::ENU);
+  sensorENU->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(10000000)));
+  orientValue = math::Quaterniond(math::Vector3d(0, 0, 0));
+  EXPECT_EQ(orientValue, sensorENU->Orientation());
+
+  // Case A.2 Localization ref: ENU, World : ENU + rotation offset
+  sensorENU->SetWorldFrameOrientation(math::Quaterniond(0, 0, IGN_PI/4),
+    sensors::WorldFrameEnumType::ENU);
+  sensorENU->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(10000000)));
+  orientValue = math::Quaterniond(math::Vector3d(0, 0, -IGN_PI/4));
+  EXPECT_EQ(orientValue, sensorENU->Orientation());
+
+  // Case A.3 Localization ref: ENU, World : NWU
+  // Sensor aligns with NWU, we're measuring wrt ENU
+  sensorENU->SetWorldFrameOrientation(math::Quaterniond(0, 0, 0),
+    sensors::WorldFrameEnumType::NWU);
+  sensorENU->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(10000000)));
+  orientValue = math::Quaterniond(math::Vector3d(0, 0, IGN_PI/2));
+  EXPECT_EQ(orientValue, sensorENU->Orientation());
+
+  // Case A.4 Localization ref: ENU, World : NED
+  // Sensor aligns with NED, we're measuring wrt ENU
+  sensorENU->SetWorldFrameOrientation(math::Quaterniond(0, 0, 0),
+    sensors::WorldFrameEnumType::NED);
+  sensorENU->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(10000000)));
+  orientValue = math::Quaterniond(math::Vector3d(IGN_PI, 0, IGN_PI/2));
+  EXPECT_EQ(orientValue, sensorENU->Orientation());
+
+  // B. Localization tag is set to NWU
+  // ---------------------------------
+  auto sensorNWU = mgr.CreateSensor<ignition::sensors::ImuSensor>(
+      generateSensor("NWU"));
+  ASSERT_NE(nullptr, sensorNWU);
+
+  // Case B.1 : Localization ref: NWU, World : NWU
+  // Sensor aligns with NWU, we're measuring wrt NWU
+  sensorNWU->SetWorldFrameOrientation(math::Quaterniond(0, 0, 0),
+    sensors::WorldFrameEnumType::NWU);
+  sensorNWU->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(10000000)));
+  orientValue = math::Quaterniond(math::Vector3d(0, 0, 0));
+  EXPECT_EQ(orientValue, sensorNWU->Orientation());
+
+  // Case B.2 : Localization ref: NWU, World : NWU + rotation offset
+  sensorNWU->SetWorldFrameOrientation(math::Quaterniond(0, 0, IGN_PI/4),
+    sensors::WorldFrameEnumType::NWU);
+  sensorNWU->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(10000000)));
+  orientValue = math::Quaterniond(math::Vector3d(0, 0, -IGN_PI/4));
+  EXPECT_EQ(orientValue, sensorNWU->Orientation());
+
+  // Case B.3 : Localization ref: NWU, World : ENU
+  // Sensor aligns with ENU, we're measuring wrt NWU
+  sensorNWU->SetWorldFrameOrientation(math::Quaterniond(0, 0, 0),
+    sensors::WorldFrameEnumType::ENU);
+  sensorNWU->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(10000000)));
+  orientValue = math::Quaterniond(math::Vector3d(0, 0, -IGN_PI/2));
+  EXPECT_EQ(orientValue, sensorNWU->Orientation());
+
+  // Case B.4 : Localization ref: NWU, World : NED
+  // Sensor aligns with NED, we're measuring wrt NWU
+  sensorNWU->SetWorldFrameOrientation(math::Quaterniond(0, 0, 0),
+    sensors::WorldFrameEnumType::NED);
+  sensorNWU->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(10000000)));
+  orientValue = math::Quaterniond(math::Vector3d(IGN_PI, 0, 0));
+  EXPECT_EQ(orientValue, sensorNWU->Orientation());
+
+  // C. Localization tag is set to NED
+  // ---------------------------------
+  auto sensorNED = mgr.CreateSensor<ignition::sensors::ImuSensor>(
+      generateSensor("NED"));
+  ASSERT_NE(nullptr, sensorNED);
+
+  // Case C.1 : Localization ref: NED, World : NED
+  // Sensor aligns with NED, we're measuring wrt NED
+  sensorNED->SetWorldFrameOrientation(math::Quaterniond(0, 0, 0),
+    sensors::WorldFrameEnumType::NED);
+  sensorNED->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(10000000)));
+  orientValue = math::Quaterniond(math::Vector3d(0, 0, 0));
+  EXPECT_EQ(orientValue, sensorNED->Orientation());
+
+  // Case C.2 : Localization ref: NED, World : NED + rotation offset
+  sensorNED->SetWorldFrameOrientation(math::Quaterniond(0, 0, IGN_PI/4),
+    sensors::WorldFrameEnumType::NED);
+  sensorNED->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(10000000)));
+  orientValue = math::Quaterniond(math::Vector3d(0, 0, -IGN_PI/4));
+  EXPECT_EQ(orientValue, sensorNED->Orientation());
+
+  // Case C.3 : Localization ref: NED, World : NWU
+  // Sensor aligns with NWU, we're measuring wrt NED
+  sensorNED->SetWorldFrameOrientation(math::Quaterniond(0, 0, 0),
+    sensors::WorldFrameEnumType::NWU);
+  sensorNED->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(10000000)));
+  orientValue = math::Quaterniond(math::Vector3d(-IGN_PI, 0, 0));
+  EXPECT_EQ(orientValue, sensorNED->Orientation());
+
+  // Case C.4 : Localization ref: NED, World : ENU
+  // Sensor aligns with ENU, we're measuring wrt NED
+  sensorNED->SetWorldFrameOrientation(math::Quaterniond(0, 0, 0),
+    sensors::WorldFrameEnumType::ENU);
+  sensorNED->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(10000000)));
+  orientValue = math::Quaterniond(math::Vector3d(-IGN_PI, 0, IGN_PI/2));
+  EXPECT_EQ(orientValue, sensorNED->Orientation());
+}
+
 //////////////////////////////////////////////////
 int main(int argc, char **argv)
 {