diff --git a/include/ignition/sensors/ImuSensor.hh b/include/ignition/sensors/ImuSensor.hh index a0212b72..3ab96ba1 100644 --- a/include/ignition/sensors/ImuSensor.hh +++ b/include/ignition/sensors/ImuSensor.hh @@ -33,9 +33,30 @@ namespace ignition { namespace sensors { - // Inline bracket to help doxygen filtering. + /// 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 + /// reference orientation 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 orientation, 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 diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index a88ed2be..7474759f 100644 --- a/src/ImuSensor.cc +++ b/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> + 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) { diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index 0f4aa16d..c035a376 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -436,6 +436,8 @@ TEST(ImuSensor_TEST, OrientationReference) // Create an ImuSensor auto sensor = mgr.CreateSensor( 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( 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,304 @@ TEST(ImuSensor_TEST, CustomRpyParentFrame) EXPECT_EQ(defultOrientValue, sensor->Orientation()); } +sdf::ElementPtr sensorWithLocalization( + std::string _orientationLocalization) +{ + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " imu_test" + << " 1" + << " " + << " " + << " "+_orientationLocalization+"" + << " " + << " " + << " 1" + << " true" + << " " + << " " + << " " + << ""; + + 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( + sensorWithLocalization("ENU")); + ASSERT_NE(nullptr, sensorENU); + + // Case A.1 : Localization ref: ENU, World : ENU + // Sensor aligns with ENU, we're measuring wrt ENU + { + const math::Quaterniond worldFrameOrientation(0, 0, 0); + const sensors::WorldFrameEnumType worldRelativeTo = + sensors::WorldFrameEnumType::ENU; + const math::Quaterniond expectedSensorOrientation(0, 0, 0); + + sensorENU->SetWorldFrameOrientation(worldFrameOrientation, + worldRelativeTo); + sensorENU->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + EXPECT_EQ(expectedSensorOrientation, sensorENU->Orientation()); + } + + // Case A.2 Localization ref: ENU, World : ENU + rotation offset + { + const math::Quaterniond worldFrameOrientation(0, 0, IGN_PI/4); + const sensors::WorldFrameEnumType worldRelativeTo = + sensors::WorldFrameEnumType::ENU; + const math::Quaterniond expectedSensorOrientation(0, 0, -IGN_PI/4); + + sensorENU->SetWorldFrameOrientation(worldFrameOrientation, + worldRelativeTo); + sensorENU->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + EXPECT_EQ(expectedSensorOrientation, sensorENU->Orientation()); + } + + // Case A.3 Localization ref: ENU, World : NWU + // Sensor aligns with NWU, we're measuring wrt ENU + { + const math::Quaterniond worldFrameOrientation(0, 0, 0); + const sensors::WorldFrameEnumType worldRelativeTo = + sensors::WorldFrameEnumType::NWU; + const math::Quaterniond expectedSensorOrientation(0, 0, IGN_PI/2); + + sensorENU->SetWorldFrameOrientation(worldFrameOrientation, + worldRelativeTo); + sensorENU->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + EXPECT_EQ(expectedSensorOrientation, sensorENU->Orientation()); + } + + // Case A.4 Localization ref: ENU, World : NED + // Sensor aligns with NED, we're measuring wrt ENU + { + const math::Quaterniond worldFrameOrientation(0, 0, 0); + const sensors::WorldFrameEnumType worldRelativeTo = + sensors::WorldFrameEnumType::NED; + const math::Quaterniond expectedSensorOrientation(IGN_PI, 0, IGN_PI/2); + + sensorENU->SetWorldFrameOrientation(worldFrameOrientation, + worldRelativeTo); + sensorENU->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + EXPECT_EQ(expectedSensorOrientation, sensorENU->Orientation()); + } + + // B. Localization tag is set to NWU + // --------------------------------- + auto sensorNWU = mgr.CreateSensor( + sensorWithLocalization("NWU")); + ASSERT_NE(nullptr, sensorNWU); + + // Case B.1 : Localization ref: NWU, World : NWU + // Sensor aligns with NWU, we're measuring wrt NWU + { + const math::Quaterniond worldFrameOrientation(0, 0, 0); + const sensors::WorldFrameEnumType worldRelativeTo = + sensors::WorldFrameEnumType::NWU; + const math::Quaterniond expectedSensorOrientation(0, 0, 0); + + sensorNWU->SetWorldFrameOrientation(worldFrameOrientation, + worldRelativeTo); + sensorNWU->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + EXPECT_EQ(expectedSensorOrientation, sensorNWU->Orientation()); + } + + // Case B.2 : Localization ref: NWU, World : NWU + rotation offset + { + const math::Quaterniond worldFrameOrientation(0, 0, IGN_PI/4); + const sensors::WorldFrameEnumType worldRelativeTo = + sensors::WorldFrameEnumType::NWU; + const math::Quaterniond expectedSensorOrientation(0, 0, -IGN_PI/4); + + sensorNWU->SetWorldFrameOrientation(worldFrameOrientation, + worldRelativeTo); + sensorNWU->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + EXPECT_EQ(expectedSensorOrientation, sensorNWU->Orientation()); + } + + // Case B.3 : Localization ref: NWU, World : ENU + // Sensor aligns with ENU, we're measuring wrt NWU + { + const math::Quaterniond worldFrameOrientation(0, 0, 0); + const sensors::WorldFrameEnumType worldRelativeTo = + sensors::WorldFrameEnumType::ENU; + const math::Quaterniond expectedSensorOrientation(0, 0, -IGN_PI/2); + + sensorNWU->SetWorldFrameOrientation(worldFrameOrientation, + worldRelativeTo); + sensorNWU->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + EXPECT_EQ(expectedSensorOrientation, sensorNWU->Orientation()); + } + + // Case B.4 : Localization ref: NWU, World : NED + // Sensor aligns with NED, we're measuring wrt NWU + { + const math::Quaterniond worldFrameOrientation(0, 0, 0); + const sensors::WorldFrameEnumType worldRelativeTo = + sensors::WorldFrameEnumType::NED; + const math::Quaterniond expectedSensorOrientation(IGN_PI, 0, 0); + + sensorNWU->SetWorldFrameOrientation(worldFrameOrientation, + worldRelativeTo); + sensorNWU->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + EXPECT_EQ(expectedSensorOrientation, sensorNWU->Orientation()); + } + + // C. Localization tag is set to NED + // --------------------------------- + auto sensorNED = mgr.CreateSensor( + sensorWithLocalization("NED")); + ASSERT_NE(nullptr, sensorNED); + + // Case C.1 : Localization ref: NED, World : NED + // Sensor aligns with NED, we're measuring wrt NED + { + const math::Quaterniond worldFrameOrientation(0, 0, 0); + const sensors::WorldFrameEnumType worldRelativeTo = + sensors::WorldFrameEnumType::NED; + const math::Quaterniond expectedSensorOrientation(0, 0, 0); + + sensorNED->SetWorldFrameOrientation(worldFrameOrientation, + worldRelativeTo); + sensorNED->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + EXPECT_EQ(expectedSensorOrientation, sensorNED->Orientation()); + } + + // Case C.2 : Localization ref: NED, World : NED + rotation offset + { + const math::Quaterniond worldFrameOrientation(0, 0, IGN_PI/4); + const sensors::WorldFrameEnumType worldRelativeTo = + sensors::WorldFrameEnumType::NED; + const math::Quaterniond expectedSensorOrientation(0, 0, -IGN_PI/4); + + sensorNED->SetWorldFrameOrientation(worldFrameOrientation, + worldRelativeTo); + sensorNED->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + EXPECT_EQ(expectedSensorOrientation, sensorNED->Orientation()); + } + + // Case C.3 : Localization ref: NED, World : NWU + // Sensor aligns with NWU, we're measuring wrt NED + { + const math::Quaterniond worldFrameOrientation(0, 0, 0); + const sensors::WorldFrameEnumType worldRelativeTo = + sensors::WorldFrameEnumType::NWU; + const math::Quaterniond expectedSensorOrientation(-IGN_PI, 0, 0); + + sensorNED->SetWorldFrameOrientation(worldFrameOrientation, + worldRelativeTo); + sensorNED->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + EXPECT_EQ(expectedSensorOrientation, sensorNED->Orientation()); + } + + // Case C.4 : Localization ref: NED, World : ENU + // Sensor aligns with ENU, we're measuring wrt NED + { + const math::Quaterniond worldFrameOrientation(0, 0, 0); + const sensors::WorldFrameEnumType worldRelativeTo = + sensors::WorldFrameEnumType::ENU; + const math::Quaterniond expectedSensorOrientation(-IGN_PI, 0, IGN_PI/2); + + sensorNED->SetWorldFrameOrientation(worldFrameOrientation, + worldRelativeTo); + sensorNED->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + EXPECT_EQ(expectedSensorOrientation, sensorNED->Orientation()); + } +} + +////////////////////////////////////////////////// +TEST(ImuSensor_TEST, LocalizationTagInvalid) +{ + // Create a sensor manager + ignition::sensors::Manager mgr; + + sdf::ElementPtr imuSDF; + + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " imu_test" + << " 1" + << " " + << " " + << " UNRECOGNIZED" + << " " + << " " + << " 1" + << " true" + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + if (!sdf::readString(stream.str(), sdfParsed)) + { + imuSDF = sdf::ElementPtr(); + } + else + { + imuSDF = sdfParsed->Root()->GetElement("model")->GetElement("link") + ->GetElement("sensor"); + } + + // Create an ImuSensor + auto sensor = mgr.CreateSensor( + imuSDF); + sensor->SetWorldFrameOrientation(math::Quaterniond(0, 0, 0), + sensors::WorldFrameEnumType::ENU); + + // Make sure the above dynamic cast worked. + ASSERT_NE(nullptr, sensor); + + sensor->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + + math::Quaterniond orientValue(math::Vector3d(0, 0, 0)); + EXPECT_EQ(orientValue, sensor->Orientation()); +} + ////////////////////////////////////////////////// int main(int argc, char **argv) {