IMU sensor API to get world ref frame and heading offset

include/ignition/sensors/ImuSensor.hh

@@ -35,6 +35,17 @@ namespace ignition
   {
     // Inline bracket to help doxygen filtering.
     inline namespace IGNITION_SENSORS_VERSION_NAMESPACE {
+    //
+    // \brief Reference frames enum
+    enum class IGNITION_SENSORS_VISIBLE WorldFrameEnumType
+    {
+      NONE = 0,
+      ENU = 1,
+      NED = 2,
+      NWU = 3,
+      CUSTOM = 4
+    };
+
     //
     /// \brief forward declarations
     class ImuSensorPrivate;
@@ -136,6 +147,21 @@ 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);
+
+      // \brief Read the localization sdf tag
+      // \param[in] _sdf Imu sdf data as sdf::Sensor
+      public: void ReadLocalizationTag(const sdf::Sensor &_sdf);
+
+      // \brief Read the localization sdf tag
+      // \param[in] _sdf Imu sdf data as sdf:;ElementPtr
+      public: void ReadLocalizationTag(sdf::ElementPtr _sdf);
+
       IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING
       /// \brief Data pointer for private data
       /// \internal

src/ImuSensor.cc

@@ -71,6 +71,15 @@ class ignition::sensors::ImuSensorPrivate
   /// \brief Flag for if time has been initialized
   public: bool timeInitialized = false;
 
+  /// \brief Store world heading offset
+  public: ignition::math::Quaterniond headingOffset;
+
+  /// \brief Store world orientation
+  public: WorldFrameEnumType worldFrameType = WorldFrameEnumType::ENU;
+
+  /// \brief Store localization orientation
+  public: WorldFrameEnumType localizationEnum = WorldFrameEnumType::NONE;
+
   /// \brief Previous update time step.
   public: std::chrono::steady_clock::duration prevStep
     {std::chrono::steady_clock::duration::zero()};
@@ -116,23 +125,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");
@@ -292,6 +284,101 @@ math::Pose3d ImuSensor::WorldPose() const
   return this->dataPtr->worldPose;
 }
 
+//////////////////////////////////////////////////
+void ImuSensor::ReadLocalizationTag(const sdf::Sensor &_sdf)
+{
+  std::string localization = _sdf.ImuSensor()->Localization();
+
+  if (localization == "ENU")
+  {
+    this->dataPtr->localizationEnum = WorldFrameEnumType::ENU;
+  } else if (localization == "NED") {
+    this->dataPtr->localizationEnum = WorldFrameEnumType::NED;
+  } else if (localization == "NWU") {
+    this->dataPtr->localizationEnum = WorldFrameEnumType::NWU;
+  } else if (localization == "CUSTOM") {
+    this->dataPtr->localizationEnum = WorldFrameEnumType::CUSTOM;
+  } else {
+    this->dataPtr->localizationEnum = WorldFrameEnumType::NONE;
+  }
+
+  // Set orientation reference frame if custom_rpy was supplied
+  if (this->dataPtr->localizationEnum == WorldFrameEnumType::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;
+    }
+  }
+}
+
+
+//////////////////////////////////////////////////
+void ImuSensor::ReadLocalizationTag(sdf::ElementPtr _sdf)
+{
+  sdf::Sensor sdfSensor;
+  sdfSensor.Load(_sdf);
+  return this->ReadLocalizationTag(sdfSensor);
+}
+
+//////////////////////////////////////////////////
+void ImuSensor::SetWorldFrameOrientation(
+  const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo)
+{
+  this->dataPtr->headingOffset = _rot;
+  this->dataPtr->worldFrameType = _relativeTo;
+
+  // Table to hold frame transformations
+  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->localizationEnum != WorldFrameEnumType::NONE &&
+      this->dataPtr->localizationEnum != WorldFrameEnumType::CUSTOM)
+  {
+    // A valid named localization tag is supplied in the sdf
+    auto tranformation =
+      transformTable[this->dataPtr->worldFrameType][
+      this->dataPtr->localizationEnum];
+    this->SetOrientationReference(this->dataPtr->headingOffset * tranformation);
+  }
+}
+
 //////////////////////////////////////////////////
 void ImuSensor::SetOrientationReference(const math::Quaterniond &_orient)
 {

src/ImuSensor_TEST.cc

@@ -440,6 +440,8 @@ TEST(ImuSensor_TEST, OrientationReference)
   // Make sure the above dynamic cast worked.
   ASSERT_NE(nullptr, sensor);
 
+  sensor->ReadLocalizationTag(imuSDF);
+
   sensor->Update(std::chrono::steady_clock::duration(
     std::chrono::nanoseconds(10000000)));
 
@@ -499,6 +501,8 @@ TEST(ImuSensor_TEST, CustomRpyParentFrame)
   // Make sure the above dynamic cast worked.
   ASSERT_NE(nullptr, sensor);
 
+  sensor->ReadLocalizationTag(imuSDF);
+
   sensor->Update(std::chrono::steady_clock::duration(
     std::chrono::nanoseconds(10000000)));
 
@@ -511,6 +515,177 @@ 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);
+  sensorENU->ReadLocalizationTag(generateSensor("ENU"));
+
+  // 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);
+  sensorNWU->ReadLocalizationTag(generateSensor("NWU"));
+
+  // 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);
+  sensorNED->ReadLocalizationTag(generateSensor("NED"));
+
+  // 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)
 {