From 17ca1ac1bf0ae0e7d438f4bb4b611bbe4d95f791 Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 11 Jan 2022 07:18:51 -0800 Subject: [PATCH 01/22] Added basic logic to parse localization frames Signed-off-by: Aditya --- src/ImuSensor.cc | 70 +++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 69 insertions(+), 1 deletion(-) diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index a88ed2be..2b3920c8 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -70,6 +70,12 @@ class ignition::sensors::ImuSensorPrivate /// \brief Flag for if time has been initialized public: bool timeInitialized = false; + + /// \brief Store world heading offset + public: Quaterniond headingOffset; + + /// \brief Store world orientation + public: std::string worldFrameType = "ENU"; /// \brief Previous update time step. public: std::chrono::steady_clock::duration prevStep @@ -119,7 +125,8 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) // Set orientation reference frame // TODO(adityapande-1995) : Add support for named frames like // ENU using ign-gazebo - if (_sdf.ImuSensor()->Localization() == "CUSTOM") + std::string localization = _sdf.ImuSensor()->Localization(); + if (localization == "CUSTOM") { if (_sdf.ImuSensor()->CustomRpyParentFrame() == "") { @@ -134,6 +141,60 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) } } + // Table to hold frame transformations + std::map> transformTable = + { + "ENU": + { + "ENU": Quaterniond(0, 0, 0), + "NED": Quaterniond(0, 0, 0), + "NWU": Quaterniond(0, 0, 0), + "GRAV_UP": Quaterniond(0, 0, 0), + "GRAV_DOWN": Quaterniond(0, 0, 0) + }, + "NED": + { + "ENU": Quaterniond(0, 0, 0), + "NED": Quaterniond(0, 0, 0), + "NWU": Quaterniond(0, 0, 0), + "GRAV_UP": Quaterniond(0, 0, 0), + "GRAV_DOWN": Quaterniond(0, 0, 0) + }, + "NWU": + { + "ENU": Quaterniond(0, 0, 0), + "NED": Quaterniond(0, 0, 0), + "NWU": Quaterniond(0, 0, 0), + "GRAV_UP": Quaterniond(0, 0, 0), + "GRAV_DOWN": Quaterniond(0, 0, 0) + }, + "GRAV_UP": + { + "ENU": Quaterniond(0, 0, 0), + "NED": Quaterniond(0, 0, 0), + "NWU": Quaterniond(0, 0, 0), + "GRAV_UP": Quaterniond(0, 0, 0), + "GRAV_DOWN": Quaterniond(0, 0, 0) + }, + "GRAV_DOWN": + { + "ENU": Quaterniond(0, 0, 0), + "NED": Quaterniond(0, 0, 0), + "NWU": Quaterniond(0, 0, 0), + "GRAV_UP": Quaterniond(0, 0, 0), + "GRAV_DOWN": Quaterniond(0, 0, 0) + }, + }; + + if (transformTable.count(localization) != 0) + { + // A valid localization tag is supplied in the sdf + auto tranformation = transformTable[this->dataPtr->worldFrameType][localization]; + this->SetOrientationReference(this->dataPtr->headingOffset * tranformation); + } else { + ignwarn << "This localization tag is not supported: " << localization << std::endl; + } + if (this->Topic().empty()) this->SetTopic("/imu"); @@ -292,6 +353,13 @@ math::Pose3d ImuSensor::WorldPose() const return this->dataPtr->worldPose; } +////////////////////////////////////////////////// +void ImuSensor::SetWorldFrameOrientation(const Quaterniond &_rot, std::string _relativeTo) const +{ + this->dataPtr->headingOffset = _rot; + this->dataPtr->worldFrameType = _relativeTo; +} + ////////////////////////////////////////////////// void ImuSensor::SetOrientationReference(const math::Quaterniond &_orient) { From d4aa6598f9efbb371b5cb2e7cbc9837b03421ce7 Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 11 Jan 2022 08:36:11 -0800 Subject: [PATCH 02/22] Minor cleanup Signed-off-by: Aditya --- include/ignition/sensors/ImuSensor.hh | 6 ++ src/ImuSensor.cc | 81 ++++++++++++++------------- 2 files changed, 49 insertions(+), 38 deletions(-) diff --git a/include/ignition/sensors/ImuSensor.hh b/include/ignition/sensors/ImuSensor.hh index a0212b72..75ac58d8 100644 --- a/include/ignition/sensors/ImuSensor.hh +++ b/include/ignition/sensors/ImuSensor.hh @@ -135,6 +135,12 @@ namespace ignition /// \brief Get the gravity vector /// \return Gravity vectory in meters per second squared. public: math::Vector3d Gravity() const; + + /// \brief Convey worl frame's orientation and heading offset + /// \param[in] _rot heading offset + /// \param[in] _relativeTo world frame orintation, "ENU" by default + public: void SetWorldFrameOrientation( + const math::Quaterniond &_rot, std::string _relativeTo); IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 2b3920c8..6a0d6db6 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -72,7 +72,7 @@ class ignition::sensors::ImuSensorPrivate public: bool timeInitialized = false; /// \brief Store world heading offset - public: Quaterniond headingOffset; + public: ignition::math::Quaterniond headingOffset; /// \brief Store world orientation public: std::string worldFrameType = "ENU"; @@ -142,48 +142,53 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) } // Table to hold frame transformations - std::map> transformTable = + std::map> transformTable = { - "ENU": + {"ENU", { - "ENU": Quaterniond(0, 0, 0), - "NED": Quaterniond(0, 0, 0), - "NWU": Quaterniond(0, 0, 0), - "GRAV_UP": Quaterniond(0, 0, 0), - "GRAV_DOWN": Quaterniond(0, 0, 0) - }, - "NED": + {"ENU", ignition::math::Quaterniond(0, 0, 0)}, + {"NED", ignition::math::Quaterniond(0, 0, 0)}, + {"NWU", ignition::math::Quaterniond(0, 0, 0)}, + {"GRAV_UP", ignition::math::Quaterniond(0, 0, 0)}, + {"GRAV_DOWN", ignition::math::Quaterniond(0, 0, 0)} + } + }, + {"NED", { - "ENU": Quaterniond(0, 0, 0), - "NED": Quaterniond(0, 0, 0), - "NWU": Quaterniond(0, 0, 0), - "GRAV_UP": Quaterniond(0, 0, 0), - "GRAV_DOWN": Quaterniond(0, 0, 0) - }, - "NWU": + {"ENU", ignition::math::Quaterniond(0, 0, 0)}, + {"NED", ignition::math::Quaterniond(0, 0, 0)}, + {"NWU", ignition::math::Quaterniond(0, 0, 0)}, + {"GRAV_UP", ignition::math::Quaterniond(0, 0, 0)}, + {"GRAV_DOWN", ignition::math::Quaterniond(0, 0, 0)} + } + }, + {"NWU", { - "ENU": Quaterniond(0, 0, 0), - "NED": Quaterniond(0, 0, 0), - "NWU": Quaterniond(0, 0, 0), - "GRAV_UP": Quaterniond(0, 0, 0), - "GRAV_DOWN": Quaterniond(0, 0, 0) - }, - "GRAV_UP": + {"ENU", ignition::math::Quaterniond(0, 0, 0)}, + {"NED", ignition::math::Quaterniond(0, 0, 0)}, + {"NWU", ignition::math::Quaterniond(0, 0, 0)}, + {"GRAV_UP", ignition::math::Quaterniond(0, 0, 0)}, + {"GRAV_DOWN", ignition::math::Quaterniond(0, 0, 0)} + } + }, + {"GRAV_UP", { - "ENU": Quaterniond(0, 0, 0), - "NED": Quaterniond(0, 0, 0), - "NWU": Quaterniond(0, 0, 0), - "GRAV_UP": Quaterniond(0, 0, 0), - "GRAV_DOWN": Quaterniond(0, 0, 0) - }, - "GRAV_DOWN": + {"ENU", ignition::math::Quaterniond(0, 0, 0)}, + {"NED", ignition::math::Quaterniond(0, 0, 0)}, + {"NWU", ignition::math::Quaterniond(0, 0, 0)}, + {"GRAV_UP", ignition::math::Quaterniond(0, 0, 0)}, + {"GRAV_DOWN", ignition::math::Quaterniond(0, 0, 0)} + } + }, + {"GRAV_DOWN", { - "ENU": Quaterniond(0, 0, 0), - "NED": Quaterniond(0, 0, 0), - "NWU": Quaterniond(0, 0, 0), - "GRAV_UP": Quaterniond(0, 0, 0), - "GRAV_DOWN": Quaterniond(0, 0, 0) - }, + {"ENU", ignition::math::Quaterniond(0, 0, 0)}, + {"NED", ignition::math::Quaterniond(0, 0, 0)}, + {"NWU", ignition::math::Quaterniond(0, 0, 0)}, + {"GRAV_UP", ignition::math::Quaterniond(0, 0, 0)}, + {"GRAV_DOWN", ignition::math::Quaterniond(0, 0, 0)} + } + } }; if (transformTable.count(localization) != 0) @@ -354,7 +359,7 @@ math::Pose3d ImuSensor::WorldPose() const } ////////////////////////////////////////////////// -void ImuSensor::SetWorldFrameOrientation(const Quaterniond &_rot, std::string _relativeTo) const +void ImuSensor::SetWorldFrameOrientation(const math::Quaterniond &_rot, std::string _relativeTo) { this->dataPtr->headingOffset = _rot; this->dataPtr->worldFrameType = _relativeTo; From c4798879dcbbea49c0477edf2d79aed495f90e47 Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 11 Jan 2022 09:29:52 -0800 Subject: [PATCH 03/22] Added transformation quaternions to convert frames Signed-off-by: Aditya --- src/ImuSensor.cc | 36 ++++++------------------------------ 1 file changed, 6 insertions(+), 30 deletions(-) diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 6a0d6db6..03b7dcfb 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -147,46 +147,22 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) {"ENU", { {"ENU", ignition::math::Quaterniond(0, 0, 0)}, - {"NED", ignition::math::Quaterniond(0, 0, 0)}, - {"NWU", ignition::math::Quaterniond(0, 0, 0)}, - {"GRAV_UP", ignition::math::Quaterniond(0, 0, 0)}, - {"GRAV_DOWN", ignition::math::Quaterniond(0, 0, 0)} + {"NED", ignition::math::Quaterniond(1/std::sqrt(2), 1/std::sqrt(2), 0, 0)}, + {"NWU", ignition::math::Quaterniond(0, 0, IGN_PI/2)}, } }, {"NED", { - {"ENU", ignition::math::Quaterniond(0, 0, 0)}, + {"ENU", ignition::math::Quaterniond(1/std::sqrt(2), 1/std::sqrt(2), 0, 0).Inverse()}, {"NED", ignition::math::Quaterniond(0, 0, 0)}, - {"NWU", ignition::math::Quaterniond(0, 0, 0)}, - {"GRAV_UP", ignition::math::Quaterniond(0, 0, 0)}, - {"GRAV_DOWN", ignition::math::Quaterniond(0, 0, 0)} + {"NWU", ignition::math::Quaterniond(IGN_PI, 0, 0)}, } }, {"NWU", { - {"ENU", ignition::math::Quaterniond(0, 0, 0)}, - {"NED", ignition::math::Quaterniond(0, 0, 0)}, - {"NWU", ignition::math::Quaterniond(0, 0, 0)}, - {"GRAV_UP", ignition::math::Quaterniond(0, 0, 0)}, - {"GRAV_DOWN", ignition::math::Quaterniond(0, 0, 0)} - } - }, - {"GRAV_UP", - { - {"ENU", ignition::math::Quaterniond(0, 0, 0)}, - {"NED", ignition::math::Quaterniond(0, 0, 0)}, - {"NWU", ignition::math::Quaterniond(0, 0, 0)}, - {"GRAV_UP", ignition::math::Quaterniond(0, 0, 0)}, - {"GRAV_DOWN", ignition::math::Quaterniond(0, 0, 0)} - } - }, - {"GRAV_DOWN", - { - {"ENU", ignition::math::Quaterniond(0, 0, 0)}, - {"NED", ignition::math::Quaterniond(0, 0, 0)}, + {"ENU", ignition::math::Quaterniond(0, 0, -IGN_PI/2)}, + {"NED", ignition::math::Quaterniond(-IGN_PI, 0, 0)}, {"NWU", ignition::math::Quaterniond(0, 0, 0)}, - {"GRAV_UP", ignition::math::Quaterniond(0, 0, 0)}, - {"GRAV_DOWN", ignition::math::Quaterniond(0, 0, 0)} } } }; From 1fdc240ebeb05dd819da7093de202a9bfb1c3652 Mon Sep 17 00:00:00 2001 From: Aditya Date: Thu, 13 Jan 2022 06:34:21 -0800 Subject: [PATCH 04/22] Added enum type, review changes Signed-off-by: Aditya --- include/ignition/sensors/ImuSensor.hh | 19 ++++++++--- src/ImuSensor.cc | 48 +++++++++++++++++---------- 2 files changed, 46 insertions(+), 21 deletions(-) diff --git a/include/ignition/sensors/ImuSensor.hh b/include/ignition/sensors/ImuSensor.hh index 75ac58d8..4f1932b2 100644 --- a/include/ignition/sensors/ImuSensor.hh +++ b/include/ignition/sensors/ImuSensor.hh @@ -35,6 +35,16 @@ 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 + }; + // /// \brief forward declarations class ImuSensorPrivate; @@ -136,11 +146,12 @@ namespace ignition /// \return Gravity vectory in meters per second squared. public: math::Vector3d Gravity() const; - /// \brief Convey worl frame's orientation and heading offset - /// \param[in] _rot heading offset - /// \param[in] _relativeTo world frame orintation, "ENU" by default + /// \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, std::string _relativeTo); + const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo); IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 03b7dcfb..af24fbf7 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -75,7 +75,7 @@ class ignition::sensors::ImuSensorPrivate public: ignition::math::Quaterniond headingOffset; /// \brief Store world orientation - public: std::string worldFrameType = "ENU"; + public: WorldFrameEnumType worldFrameType = WorldFrameEnumType::ENU; /// \brief Previous update time step. public: std::chrono::steady_clock::duration prevStep @@ -142,35 +142,49 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) } // Table to hold frame transformations - std::map> transformTable = + std::map> transformTable = { - {"ENU", + {WorldFrameEnumType::ENU, { - {"ENU", ignition::math::Quaterniond(0, 0, 0)}, - {"NED", ignition::math::Quaterniond(1/std::sqrt(2), 1/std::sqrt(2), 0, 0)}, - {"NWU", ignition::math::Quaterniond(0, 0, IGN_PI/2)}, + {WorldFrameEnumType::ENU, ignition::math::Quaterniond(0, 0, 0)}, + {WorldFrameEnumType::NED, ignition::math::Quaterniond(1/std::sqrt(2), 1/std::sqrt(2), 0, 0)}, + {WorldFrameEnumType::NWU, ignition::math::Quaterniond(0, 0, IGN_PI/2)}, } }, - {"NED", + {WorldFrameEnumType::NED, { - {"ENU", ignition::math::Quaterniond(1/std::sqrt(2), 1/std::sqrt(2), 0, 0).Inverse()}, - {"NED", ignition::math::Quaterniond(0, 0, 0)}, - {"NWU", ignition::math::Quaterniond(IGN_PI, 0, 0)}, + {WorldFrameEnumType::ENU, ignition::math::Quaterniond(1/std::sqrt(2), 1/std::sqrt(2), 0, 0).Inverse()}, + {WorldFrameEnumType::NED, ignition::math::Quaterniond(0, 0, 0)}, + {WorldFrameEnumType::NWU, ignition::math::Quaterniond(IGN_PI, 0, 0)}, } }, - {"NWU", + {WorldFrameEnumType::NWU, { - {"ENU", ignition::math::Quaterniond(0, 0, -IGN_PI/2)}, - {"NED", ignition::math::Quaterniond(-IGN_PI, 0, 0)}, - {"NWU", ignition::math::Quaterniond(0, 0, 0)}, + {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 (transformTable.count(localization) != 0) + WorldFrameEnumType localizationEnum; + + if (localization == "ENU") + { + localizationEnum = WorldFrameEnumType::ENU; + } else if (localization == "NED") { + localizationEnum = WorldFrameEnumType::NED; + } else if (localization == "NWU") { + localizationEnum = WorldFrameEnumType::NWU; + } else { + localizationEnum = WorldFrameEnumType::NONE; + } + + if (localizationEnum != WorldFrameEnumType::NONE) { // A valid localization tag is supplied in the sdf - auto tranformation = transformTable[this->dataPtr->worldFrameType][localization]; + auto tranformation = transformTable[this->dataPtr->worldFrameType][localizationEnum]; this->SetOrientationReference(this->dataPtr->headingOffset * tranformation); } else { ignwarn << "This localization tag is not supported: " << localization << std::endl; @@ -335,7 +349,7 @@ math::Pose3d ImuSensor::WorldPose() const } ////////////////////////////////////////////////// -void ImuSensor::SetWorldFrameOrientation(const math::Quaterniond &_rot, std::string _relativeTo) +void ImuSensor::SetWorldFrameOrientation(const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo) { this->dataPtr->headingOffset = _rot; this->dataPtr->worldFrameType = _relativeTo; From eaa94cd4ea8c92d27d76ddfe40f7ea9f6c8a35c1 Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 14 Jan 2022 01:07:34 -0800 Subject: [PATCH 05/22] codecheck fixes Signed-off-by: Aditya --- include/ignition/sensors/ImuSensor.hh | 2 +- src/ImuSensor.cc | 41 ++++++++++++++++----------- 2 files changed, 26 insertions(+), 17 deletions(-) diff --git a/include/ignition/sensors/ImuSensor.hh b/include/ignition/sensors/ImuSensor.hh index 4f1932b2..7bec3c75 100644 --- a/include/ignition/sensors/ImuSensor.hh +++ b/include/ignition/sensors/ImuSensor.hh @@ -145,7 +145,7 @@ namespace ignition /// \brief Get the gravity vector /// \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 diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index af24fbf7..5bedbe66 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -70,7 +70,7 @@ 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; @@ -143,28 +143,34 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) // Table to hold frame transformations std::map> transformTable = + std::map> + transformTable = { {WorldFrameEnumType::ENU, { - {WorldFrameEnumType::ENU, ignition::math::Quaterniond(0, 0, 0)}, - {WorldFrameEnumType::NED, ignition::math::Quaterniond(1/std::sqrt(2), 1/std::sqrt(2), 0, 0)}, - {WorldFrameEnumType::NWU, ignition::math::Quaterniond(0, 0, IGN_PI/2)}, - } + {WorldFrameEnumType::ENU, ignition::math::Quaterniond(0, 0, 0)}, + {WorldFrameEnumType::NED, ignition::math::Quaterniond( + 1/std::sqrt(2), 1/std::sqrt(2), 0, 0)}, + {WorldFrameEnumType::NWU, ignition::math::Quaterniond( + 0, 0, IGN_PI/2)}, + } }, {WorldFrameEnumType::NED, { - {WorldFrameEnumType::ENU, ignition::math::Quaterniond(1/std::sqrt(2), 1/std::sqrt(2), 0, 0).Inverse()}, - {WorldFrameEnumType::NED, ignition::math::Quaterniond(0, 0, 0)}, - {WorldFrameEnumType::NWU, ignition::math::Quaterniond(IGN_PI, 0, 0)}, - } + {WorldFrameEnumType::ENU, ignition::math::Quaterniond( + 1/std::sqrt(2), 1/std::sqrt(2), 0, 0).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::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)}, - } + } } }; @@ -184,10 +190,12 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) if (localizationEnum != WorldFrameEnumType::NONE) { // A valid localization tag is supplied in the sdf - auto tranformation = transformTable[this->dataPtr->worldFrameType][localizationEnum]; + auto tranformation = + transformTable[this->dataPtr->worldFrameType][localizationEnum]; this->SetOrientationReference(this->dataPtr->headingOffset * tranformation); } else { - ignwarn << "This localization tag is not supported: " << localization << std::endl; + ignwarn << "This localization tag is not supported: " << localization + << std::endl; } if (this->Topic().empty()) @@ -349,7 +357,8 @@ math::Pose3d ImuSensor::WorldPose() const } ////////////////////////////////////////////////// -void ImuSensor::SetWorldFrameOrientation(const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo) +void ImuSensor::SetWorldFrameOrientation( + const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo) { this->dataPtr->headingOffset = _rot; this->dataPtr->worldFrameType = _relativeTo; From 59d0b082a90f56364542530edc2b7bee7bf04610 Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 14 Jan 2022 03:44:24 -0800 Subject: [PATCH 06/22] Added testcases Signed-off-by: Aditya --- include/ignition/sensors/ImuSensor.hh | 3 +- src/ImuSensor.cc | 118 +++++++++---------- src/ImuSensor_TEST.cc | 156 ++++++++++++++++++++++++++ 3 files changed, 218 insertions(+), 59 deletions(-) diff --git a/include/ignition/sensors/ImuSensor.hh b/include/ignition/sensors/ImuSensor.hh index 7bec3c75..23376101 100644 --- a/include/ignition/sensors/ImuSensor.hh +++ b/include/ignition/sensors/ImuSensor.hh @@ -42,7 +42,8 @@ namespace ignition NONE = 0, ENU = 1, NED = 2, - NWU = 3 + NWU = 3, + CUSTOM = 4 }; // diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 5bedbe66..97d31c8d 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -76,6 +76,9 @@ class ignition::sensors::ImuSensorPrivate /// \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 @@ -126,7 +129,21 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) // TODO(adityapande-1995) : Add support for named frames like // ENU using ign-gazebo std::string localization = _sdf.ImuSensor()->Localization(); - if (localization == "CUSTOM") + + 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; + } + + if (this->dataPtr->localizationEnum == WorldFrameEnumType::CUSTOM) { if (_sdf.ImuSensor()->CustomRpyParentFrame() == "") { @@ -141,63 +158,6 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) } } - // Table to hold frame transformations - std::map> - transformTable = - { - {WorldFrameEnumType::ENU, - { - {WorldFrameEnumType::ENU, ignition::math::Quaterniond(0, 0, 0)}, - {WorldFrameEnumType::NED, ignition::math::Quaterniond( - 1/std::sqrt(2), 1/std::sqrt(2), 0, 0)}, - {WorldFrameEnumType::NWU, ignition::math::Quaterniond( - 0, 0, IGN_PI/2)}, - } - }, - {WorldFrameEnumType::NED, - { - {WorldFrameEnumType::ENU, ignition::math::Quaterniond( - 1/std::sqrt(2), 1/std::sqrt(2), 0, 0).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)}, - } - } - }; - - WorldFrameEnumType localizationEnum; - - if (localization == "ENU") - { - localizationEnum = WorldFrameEnumType::ENU; - } else if (localization == "NED") { - localizationEnum = WorldFrameEnumType::NED; - } else if (localization == "NWU") { - localizationEnum = WorldFrameEnumType::NWU; - } else { - localizationEnum = WorldFrameEnumType::NONE; - } - - if (localizationEnum != WorldFrameEnumType::NONE) - { - // A valid localization tag is supplied in the sdf - auto tranformation = - transformTable[this->dataPtr->worldFrameType][localizationEnum]; - this->SetOrientationReference(this->dataPtr->headingOffset * tranformation); - } else { - ignwarn << "This localization tag is not supported: " << localization - << std::endl; - } - if (this->Topic().empty()) this->SetTopic("/imu"); @@ -362,6 +322,48 @@ void ImuSensor::SetWorldFrameOrientation( { this->dataPtr->headingOffset = _rot; this->dataPtr->worldFrameType = _relativeTo; + + // Table to hold frame transformations + 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->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); + } } ////////////////////////////////////////////////// diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index 0f4aa16d..f89fc6bf 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -511,6 +511,162 @@ TEST(ImuSensor_TEST, CustomRpyParentFrame) EXPECT_EQ(defultOrientValue, sensor->Orientation()); } +sdf::ElementPtr generateSensor(std::string namedFrame) +{ + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " imu_test" + << " 1" + << " " + << " " + << " "+namedFrame+"" + << " " + << " " + << " 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( + 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 -- TODO + // 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( + 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( + 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 -- TODO + // 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) { From d09f5086af982c6296f7ff82e2dfb4e265e68b68 Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 14 Jan 2022 07:17:55 -0800 Subject: [PATCH 07/22] Cleanup for codecheck Signed-off-by: Aditya --- src/ImuSensor.cc | 7 +++--- src/ImuSensor_TEST.cc | 54 ++++++++++++++++++++++++++----------------- 2 files changed, 37 insertions(+), 24 deletions(-) diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 97d31c8d..61dfc7c7 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -76,7 +76,7 @@ class ignition::sensors::ImuSensorPrivate /// \brief Store world orientation public: WorldFrameEnumType worldFrameType = WorldFrameEnumType::ENU; - + /// \brief Store localization orientation public: WorldFrameEnumType localizationEnum = WorldFrameEnumType::NONE; @@ -322,7 +322,7 @@ void ImuSensor::SetWorldFrameOrientation( { this->dataPtr->headingOffset = _rot; this->dataPtr->worldFrameType = _relativeTo; - + // Table to hold frame transformations std::map> @@ -361,7 +361,8 @@ void ImuSensor::SetWorldFrameOrientation( { // A valid named localization tag is supplied in the sdf auto tranformation = - transformTable[this->dataPtr->worldFrameType][this->dataPtr->localizationEnum]; + transformTable[this->dataPtr->worldFrameType][ + this->dataPtr->localizationEnum]; this->SetOrientationReference(this->dataPtr->headingOffset * tranformation); } } diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index f89fc6bf..1b1e5e7c 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -561,16 +561,18 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) generateSensor("ENU")); ASSERT_NE(nullptr, sensorENU); - // Case A.1 : Localization ref: ENU, World : 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->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->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)); @@ -578,20 +580,22 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) // 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->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 -- TODO + + // 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->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( @@ -600,14 +604,16 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) // 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->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->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)); @@ -615,20 +621,22 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) // 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->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->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( @@ -637,14 +645,16 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) // 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->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->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)); @@ -652,15 +662,17 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) // 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->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 -- TODO + // 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->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)); From 7bdfb0b640a45fed7de927dd15e4eb069358e412 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 19 Jan 2022 08:52:54 -0800 Subject: [PATCH 08/22] Localization tag parsing moved to a separate method Signed-off-by: Aditya --- include/ignition/sensors/ImuSensor.hh | 8 +++ src/ImuSensor.cc | 76 ++++++++++++++++----------- src/ImuSensor_TEST.cc | 7 +++ 3 files changed, 59 insertions(+), 32 deletions(-) diff --git a/include/ignition/sensors/ImuSensor.hh b/include/ignition/sensors/ImuSensor.hh index 23376101..5dbca925 100644 --- a/include/ignition/sensors/ImuSensor.hh +++ b/include/ignition/sensors/ImuSensor.hh @@ -154,6 +154,14 @@ namespace ignition 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 diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 61dfc7c7..7a701946 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -125,38 +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 - 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; - } - - 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; - } - } if (this->Topic().empty()) this->SetTopic("/imu"); @@ -316,6 +284,50 @@ 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) diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index 1b1e5e7c..d7de7bdc 100644 --- a/src/ImuSensor_TEST.cc +++ b/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))); @@ -560,6 +564,7 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) auto sensorENU = mgr.CreateSensor( 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 @@ -601,6 +606,7 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) auto sensorNWU = mgr.CreateSensor( 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 @@ -642,6 +648,7 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) auto sensorNED = mgr.CreateSensor( 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 From e16ab676c4bb0899664cb8416736afecc2d14f9b Mon Sep 17 00:00:00 2001 From: Aditya Date: Thu, 20 Jan 2022 06:09:05 -0800 Subject: [PATCH 09/22] Changed variable names and docstrings Signed-off-by: Aditya --- src/ImuSensor.cc | 42 ++++++++++++++++++++++-------------------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 7a701946..77d02361 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -71,14 +71,16 @@ 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 Orientation of world frame relative to a specified frame + public: ignition::math::Quaterniond worldRelativeOrientation; - /// \brief Store world orientation - public: WorldFrameEnumType worldFrameType = WorldFrameEnumType::ENU; + /// \brief Frame relative-to which the worldRelativeOrientation + // is defined + public: WorldFrameEnumType worldFrameRelativeTo = WorldFrameEnumType::ENU; - /// \brief Store localization orientation - public: WorldFrameEnumType localizationEnum = WorldFrameEnumType::NONE; + /// \brief Frame relative-to which the sensor orientation is reported + public: WorldFrameEnumType sensorOrientationRelativeTo + = WorldFrameEnumType::NONE; /// \brief Previous update time step. public: std::chrono::steady_clock::duration prevStep @@ -125,7 +127,6 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) return false; } - if (this->Topic().empty()) this->SetTopic("/imu"); @@ -291,19 +292,19 @@ void ImuSensor::ReadLocalizationTag(const sdf::Sensor &_sdf) if (localization == "ENU") { - this->dataPtr->localizationEnum = WorldFrameEnumType::ENU; + this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::ENU; } else if (localization == "NED") { - this->dataPtr->localizationEnum = WorldFrameEnumType::NED; + this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NED; } else if (localization == "NWU") { - this->dataPtr->localizationEnum = WorldFrameEnumType::NWU; + this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NWU; } else if (localization == "CUSTOM") { - this->dataPtr->localizationEnum = WorldFrameEnumType::CUSTOM; + this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::CUSTOM; } else { - this->dataPtr->localizationEnum = WorldFrameEnumType::NONE; + this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NONE; } // Set orientation reference frame if custom_rpy was supplied - if (this->dataPtr->localizationEnum == WorldFrameEnumType::CUSTOM) + if (this->dataPtr->sensorOrientationRelativeTo == WorldFrameEnumType::CUSTOM) { if (_sdf.ImuSensor()->CustomRpyParentFrame() == "") { @@ -332,8 +333,8 @@ void ImuSensor::ReadLocalizationTag(sdf::ElementPtr _sdf) void ImuSensor::SetWorldFrameOrientation( const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo) { - this->dataPtr->headingOffset = _rot; - this->dataPtr->worldFrameType = _relativeTo; + this->dataPtr->worldRelativeOrientation = _rot; + this->dataPtr->worldFrameRelativeTo = _relativeTo; // Table to hold frame transformations std::mapdataPtr->localizationEnum != WorldFrameEnumType::NONE && - this->dataPtr->localizationEnum != WorldFrameEnumType::CUSTOM) + if (this->dataPtr->sensorOrientationRelativeTo != WorldFrameEnumType::NONE && + this->dataPtr->sensorOrientationRelativeTo != 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); + transformTable[this->dataPtr->worldFrameRelativeTo][ + this->dataPtr->sensorOrientationRelativeTo]; + this->SetOrientationReference(this->dataPtr->worldRelativeOrientation * + tranformation); } } From 2e5e9106a25fd01e911abb8f3dcd4ce1d2c62ec8 Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 24 Jan 2022 09:14:19 -0800 Subject: [PATCH 10/22] static const transformTable added Signed-off-by: Aditya --- src/ImuSensor.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 77d02361..9c507f99 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -337,7 +337,7 @@ void ImuSensor::SetWorldFrameOrientation( this->dataPtr->worldFrameRelativeTo = _relativeTo; // Table to hold frame transformations - std::map> transformTable = { @@ -374,8 +374,8 @@ void ImuSensor::SetWorldFrameOrientation( { // A valid named localization tag is supplied in the sdf auto tranformation = - transformTable[this->dataPtr->worldFrameRelativeTo][ - this->dataPtr->sensorOrientationRelativeTo]; + transformTable.at(this->dataPtr->worldFrameRelativeTo).at + (this->dataPtr->sensorOrientationRelativeTo); this->SetOrientationReference(this->dataPtr->worldRelativeOrientation * tranformation); } From df783de971324b9a9c428ab5769eb804f0230442 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 26 Jan 2022 08:42:33 -0800 Subject: [PATCH 11/22] Added documentation for WorlfFrameEnum Signed-off-by: Aditya --- include/ignition/sensors/ImuSensor.hh | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/include/ignition/sensors/ImuSensor.hh b/include/ignition/sensors/ImuSensor.hh index 5dbca925..6c829b1c 100644 --- a/include/ignition/sensors/ImuSensor.hh +++ b/include/ignition/sensors/ImuSensor.hh @@ -35,8 +35,14 @@ namespace ignition { // Inline bracket to help doxygen filtering. inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // + // \brief Reference frames enum + // NONE : To be used only when + // reference orintation tag is empty. + // ENU (East-North-Up): x - East, y - North, z - Up. + // NED (North-East-Down): x - North, y - East, z - Down. + // NWU (North-West-Up): x - North, y - West, z - Up. + // CUSTOM : The frame is described using custom_rpy tag. enum class IGNITION_SENSORS_VISIBLE WorldFrameEnumType { NONE = 0, From 3ead9f23eee326060c85b43b1438ae1e0d43c636 Mon Sep 17 00:00:00 2001 From: Aditya Date: Thu, 27 Jan 2022 15:07:48 -0800 Subject: [PATCH 12/22] Address not mapped to object error Signed-off-by: Aditya --- include/ignition/sensors/ImuSensor.hh | 8 --- src/ImuSensor.cc | 71 +++++++++++++-------------- src/ImuSensor_TEST.cc | 12 ++--- 3 files changed, 40 insertions(+), 51 deletions(-) diff --git a/include/ignition/sensors/ImuSensor.hh b/include/ignition/sensors/ImuSensor.hh index 6c829b1c..c3e43aac 100644 --- a/include/ignition/sensors/ImuSensor.hh +++ b/include/ignition/sensors/ImuSensor.hh @@ -160,14 +160,6 @@ namespace ignition 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 diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 9c507f99..6a5235ee 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -79,8 +79,13 @@ class ignition::sensors::ImuSensorPrivate public: WorldFrameEnumType worldFrameRelativeTo = WorldFrameEnumType::ENU; /// \brief Frame relative-to which the sensor orientation is reported - public: WorldFrameEnumType sensorOrientationRelativeTo - = WorldFrameEnumType::NONE; + public: WorldFrameEnumType sensorOrientationRelativeTo; + + /// \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 @@ -159,6 +164,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; + std::cout << "add: " << &(this->dataPtr->sensorOrientationRelativeTo) << std::endl; + } 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; } @@ -286,30 +311,17 @@ math::Pose3d ImuSensor::WorldPose() const } ////////////////////////////////////////////////// -void ImuSensor::ReadLocalizationTag(const sdf::Sensor &_sdf) +void ImuSensor::SetWorldFrameOrientation( + const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo) { - 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; - } - + std::cout << "Debug 1" << std::endl; + std::cout << "add: " << &(this->dataPtr->sensorOrientationRelativeTo) << std::endl; // Set orientation reference frame if custom_rpy was supplied if (this->dataPtr->sensorOrientationRelativeTo == WorldFrameEnumType::CUSTOM) { - if (_sdf.ImuSensor()->CustomRpyParentFrame() == "") + if (this->dataPtr->CustomRpyParentFrame == "") { - this->SetOrientationReference(ignition::math::Quaterniond( - _sdf.ImuSensor()->CustomRpy())); + this->SetOrientationReference(this->dataPtr->CustomRpyQuaternion); } else { @@ -317,22 +329,9 @@ void ImuSensor::ReadLocalizationTag(const sdf::Sensor &_sdf) "string. Setting it to any other frame is not " "supported yet." << std::endl; } + return; } -} - - -////////////////////////////////////////////////// -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->worldRelativeOrientation = _rot; this->dataPtr->worldFrameRelativeTo = _relativeTo; diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index d7de7bdc..525bd593 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -436,12 +436,12 @@ 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); - sensor->ReadLocalizationTag(imuSDF); - sensor->Update(std::chrono::steady_clock::duration( std::chrono::nanoseconds(10000000))); @@ -497,12 +497,13 @@ TEST(ImuSensor_TEST, CustomRpyParentFrame) // 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->ReadLocalizationTag(imuSDF); - sensor->Update(std::chrono::steady_clock::duration( std::chrono::nanoseconds(10000000))); @@ -564,7 +565,6 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) auto sensorENU = mgr.CreateSensor( 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 @@ -606,7 +606,6 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) auto sensorNWU = mgr.CreateSensor( 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 @@ -648,7 +647,6 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) auto sensorNED = mgr.CreateSensor( 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 From 0f99a512e8a0c846f91b89f43e7ea79709a2ecc0 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 2 Feb 2022 14:09:55 -0800 Subject: [PATCH 13/22] Removed debug prints Signed-off-by: Aditya --- src/ImuSensor.cc | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 6a5235ee..12e2d61c 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -171,7 +171,6 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::ENU; } else if (localization == "NED") { this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NED; - std::cout << "add: " << &(this->dataPtr->sensorOrientationRelativeTo) << std::endl; } else if (localization == "NWU") { this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NWU; } else if (localization == "CUSTOM") { @@ -314,8 +313,6 @@ math::Pose3d ImuSensor::WorldPose() const void ImuSensor::SetWorldFrameOrientation( const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo) { - std::cout << "Debug 1" << std::endl; - std::cout << "add: " << &(this->dataPtr->sensorOrientationRelativeTo) << std::endl; // Set orientation reference frame if custom_rpy was supplied if (this->dataPtr->sensorOrientationRelativeTo == WorldFrameEnumType::CUSTOM) { From 368a24fc24062184067d327a4534ab85f0f9ad77 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 2 Feb 2022 14:15:49 -0800 Subject: [PATCH 14/22] Intialize sensorOrientationRelativeTo Signed-off-by: Aditya --- src/ImuSensor.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 12e2d61c..42db8d90 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -79,7 +79,8 @@ class ignition::sensors::ImuSensorPrivate public: WorldFrameEnumType worldFrameRelativeTo = WorldFrameEnumType::ENU; /// \brief Frame relative-to which the sensor orientation is reported - public: WorldFrameEnumType sensorOrientationRelativeTo; + public: WorldFrameEnumType sensorOrientationRelativeTo + = WorldFrameEnumType::NONE; /// \brief Frame realtive to which custom_rpy is specified public: std::string CustomRpyParentFrame; From 00e9c7c2466ae647d97f8bc46480bf5d4918e3fe Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 2 Feb 2022 20:34:38 -0800 Subject: [PATCH 15/22] Placed docstring with enum values Signed-off-by: Aditya --- include/ignition/sensors/ImuSensor.hh | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/include/ignition/sensors/ImuSensor.hh b/include/ignition/sensors/ImuSensor.hh index c3e43aac..3189ce9f 100644 --- a/include/ignition/sensors/ImuSensor.hh +++ b/include/ignition/sensors/ImuSensor.hh @@ -37,18 +37,22 @@ namespace ignition inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { // \brief Reference frames enum - // NONE : To be used only when - // reference orintation tag is empty. - // ENU (East-North-Up): x - East, y - North, z - Up. - // NED (North-East-Down): x - North, y - East, z - Down. - // NWU (North-West-Up): x - North, y - West, z - Up. - // CUSTOM : The frame is described using custom_rpy tag. enum class IGNITION_SENSORS_VISIBLE WorldFrameEnumType { + // \brief NONE : To be used only when + // 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 }; From 18b3645d3375ef0408d6fb6612e960a73ad354dc Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 2 Feb 2022 20:55:55 -0800 Subject: [PATCH 16/22] Linter fix Signed-off-by: Aditya --- src/ImuSensor.cc | 9 +++++---- src/ImuSensor_TEST.cc | 2 +- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 42db8d90..30a29cf2 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -80,7 +80,7 @@ class ignition::sensors::ImuSensorPrivate /// \brief Frame relative-to which the sensor orientation is reported public: WorldFrameEnumType sensorOrientationRelativeTo - = WorldFrameEnumType::NONE; + = WorldFrameEnumType::NONE; /// \brief Frame realtive to which custom_rpy is specified public: std::string CustomRpyParentFrame; @@ -180,9 +180,10 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NONE; } - this->dataPtr->CustomRpyParentFrame = _sdf.ImuSensor()->CustomRpyParentFrame(); + this->dataPtr->CustomRpyParentFrame = + _sdf.ImuSensor()->CustomRpyParentFrame(); this->dataPtr->CustomRpyQuaternion = ignition::math::Quaterniond( - _sdf.ImuSensor()->CustomRpy()); + _sdf.ImuSensor()->CustomRpy()); this->dataPtr->initialized = true; return true; @@ -329,7 +330,7 @@ void ImuSensor::SetWorldFrameOrientation( } return; } - + this->dataPtr->worldRelativeOrientation = _rot; this->dataPtr->worldFrameRelativeTo = _relativeTo; diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index 525bd593..6a39b4de 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -497,7 +497,7 @@ TEST(ImuSensor_TEST, CustomRpyParentFrame) // Create an ImuSensor auto sensor = mgr.CreateSensor( imuSDF); - + sensor->SetWorldFrameOrientation(math::Quaterniond(0, 0, 0), sensors::WorldFrameEnumType::ENU); From 33a79c6524e69dbf4006815458cddef1fec3f785 Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 4 Feb 2022 15:40:29 -0800 Subject: [PATCH 17/22] Account for heading with CUSTOM refernce frame Signed-off-by: Aditya --- src/ImuSensor.cc | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 30a29cf2..922e4ba4 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -315,12 +315,16 @@ math::Pose3d ImuSensor::WorldPose() const 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->CustomRpyQuaternion); + this->SetOrientationReference(this->dataPtr->worldRelativeOrientation * + this->dataPtr->CustomRpyQuaternion); } else { @@ -331,9 +335,6 @@ void ImuSensor::SetWorldFrameOrientation( return; } - this->dataPtr->worldRelativeOrientation = _rot; - this->dataPtr->worldFrameRelativeTo = _relativeTo; - // Table to hold frame transformations static const std::map> From 8f03ce188937359f20357d6f3be3e77aad219f1c Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 14 Feb 2022 10:42:16 -0800 Subject: [PATCH 18/22] Fixed typos, renamed helper function Signed-off-by: Aditya --- include/ignition/sensors/ImuSensor.hh | 22 +++++++++++----------- src/ImuSensor_TEST.cc | 11 ++++++----- 2 files changed, 17 insertions(+), 16 deletions(-) diff --git a/include/ignition/sensors/ImuSensor.hh b/include/ignition/sensors/ImuSensor.hh index 3189ce9f..3ab96ba1 100644 --- a/include/ignition/sensors/ImuSensor.hh +++ b/include/ignition/sensors/ImuSensor.hh @@ -33,30 +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 + /// \brief Reference frames enum enum class IGNITION_SENSORS_VISIBLE WorldFrameEnumType { - // \brief NONE : To be used only when - // reference orintation tag is empty. + /// \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. + /// \brief ENU (East-North-Up): x - East, y - North, z - Up. ENU = 1, - // \brief NED (North-East-Down): x - North, y - East, z - Down. + /// \brief NED (North-East-Down): x - North, y - East, z - Down. NED = 2, - // \brief NWU (North-West-Up): x - North, y - West, z - Up. + /// \brief NWU (North-West-Up): x - North, y - West, z - Up. NWU = 3, - // \brief CUSTOM : The frame is described using custom_rpy tag. + /// \brief CUSTOM : The frame is described using custom_rpy tag. CUSTOM = 4 }; - // + /// /// \brief forward declarations class ImuSensorPrivate; @@ -158,9 +158,9 @@ namespace ignition public: math::Vector3d Gravity() const; /// \brief Specify the rotation offset of the coordinates of the World - // frame relative to a geo-referenced frame + /// frame relative to a geo-referenced frame /// \param[in] _rot rotation offset - /// \param[in] _relativeTo world frame orintation, ENU by default + /// \param[in] _relativeTo world frame orientation, ENU by default public: void SetWorldFrameOrientation( const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo); diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index 6a39b4de..061f168f 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -516,7 +516,8 @@ TEST(ImuSensor_TEST, CustomRpyParentFrame) EXPECT_EQ(defultOrientValue, sensor->Orientation()); } -sdf::ElementPtr generateSensor(std::string namedFrame) +sdf::ElementPtr sensorWithLocalization( + std::string _orientationLocalization) { std::ostringstream stream; stream @@ -529,7 +530,7 @@ sdf::ElementPtr generateSensor(std::string namedFrame) << " 1" << " " << " " - << " "+namedFrame+"" + << " "+_orientationLocalization+"" << " " << " " << " 1" @@ -563,7 +564,7 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) // A. Localization tag is set to ENU // --------------------------------- auto sensorENU = mgr.CreateSensor( - generateSensor("ENU")); + sensorWithLocalization("ENU")); ASSERT_NE(nullptr, sensorENU); // Case A.1 : Localization ref: ENU, World : ENU @@ -604,7 +605,7 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) // B. Localization tag is set to NWU // --------------------------------- auto sensorNWU = mgr.CreateSensor( - generateSensor("NWU")); + sensorWithLocalization("NWU")); ASSERT_NE(nullptr, sensorNWU); // Case B.1 : Localization ref: NWU, World : NWU @@ -645,7 +646,7 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) // C. Localization tag is set to NED // --------------------------------- auto sensorNED = mgr.CreateSensor( - generateSensor("NED")); + sensorWithLocalization("NED")); ASSERT_NE(nullptr, sensorNED); // Case C.1 : Localization ref: NED, World : NED From 654ceb8a02ecc9b37794a572b6c7437f52de01e9 Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 14 Feb 2022 12:18:02 -0800 Subject: [PATCH 19/22] Refactored tests Signed-off-by: Aditya --- src/ImuSensor_TEST.cc | 216 ++++++++++++++++++++++++++++-------------- 1 file changed, 144 insertions(+), 72 deletions(-) diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index 061f168f..30d552a6 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -569,38 +569,62 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) // 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()); + { + 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 - 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()); + { + 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 - 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()); + { + 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 - 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()); + { + 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 // --------------------------------- @@ -610,38 +634,62 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) // 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()); + { + 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 - 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()); + { + 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 - 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()); + { + 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 - 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()); + { + 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 // --------------------------------- @@ -651,38 +699,62 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) // 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()); + { + 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 - 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()); + { + 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 - 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()); + { + 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 - 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()); + { + 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()); + } } ////////////////////////////////////////////////// From 90f691735879fb91b60362a6eb6450219f0b933c Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 1 Mar 2022 12:20:36 -0800 Subject: [PATCH 20/22] Added test case: empty localization tag Signed-off-by: Aditya --- src/ImuSensor_TEST.cc | 57 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 57 insertions(+) diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index 30d552a6..35fe2208 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -757,6 +757,63 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) } } +////////////////////////////////////////////////// +TEST(ImuSensor_TEST, LocalizationTagEmpty) +{ + // Create a sensor manager + ignition::sensors::Manager mgr; + + sdf::ElementPtr imuSDF; + + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " imu_test" + << " 1" + << " " + << " " + << " " + << " " + << " 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) { From 3f42ba62f3cd0f566267ed6902882ea1ed9792bb Mon Sep 17 00:00:00 2001 From: Aditya Date: Thu, 3 Mar 2022 12:10:00 -0800 Subject: [PATCH 21/22] Added invalid localization tag test case Signed-off-by: Aditya --- src/ImuSensor_TEST.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index 35fe2208..c035a376 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -758,7 +758,7 @@ TEST(ImuSensor_TEST, NamedFrameOrientationReference) } ////////////////////////////////////////////////// -TEST(ImuSensor_TEST, LocalizationTagEmpty) +TEST(ImuSensor_TEST, LocalizationTagInvalid) { // Create a sensor manager ignition::sensors::Manager mgr; @@ -776,6 +776,7 @@ TEST(ImuSensor_TEST, LocalizationTagEmpty) << " 1" << " " << " " + << " UNRECOGNIZED" << " " << " " << " 1" @@ -811,7 +812,6 @@ TEST(ImuSensor_TEST, LocalizationTagEmpty) math::Quaterniond orientValue(math::Vector3d(0, 0, 0)); EXPECT_EQ(orientValue, sensor->Orientation()); - } ////////////////////////////////////////////////// From 735a61fb74722ef81e9ae6d33566b4cf93627b8f Mon Sep 17 00:00:00 2001 From: Aditya Date: Thu, 3 Mar 2022 15:36:39 -0800 Subject: [PATCH 22/22] changed variable names to lowerCamelCase Signed-off-by: Aditya --- src/ImuSensor.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 922e4ba4..7474759f 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -83,10 +83,10 @@ class ignition::sensors::ImuSensorPrivate = WorldFrameEnumType::NONE; /// \brief Frame realtive to which custom_rpy is specified - public: std::string CustomRpyParentFrame; + public: std::string customRpyParentFrame; /// \brief Quaternion for to store custom_rpy - public: ignition::math::Quaterniond CustomRpyQuaternion; + public: ignition::math::Quaterniond customRpyQuaternion; /// \brief Previous update time step. public: std::chrono::steady_clock::duration prevStep @@ -180,9 +180,9 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NONE; } - this->dataPtr->CustomRpyParentFrame = + this->dataPtr->customRpyParentFrame = _sdf.ImuSensor()->CustomRpyParentFrame(); - this->dataPtr->CustomRpyQuaternion = ignition::math::Quaterniond( + this->dataPtr->customRpyQuaternion = ignition::math::Quaterniond( _sdf.ImuSensor()->CustomRpy()); this->dataPtr->initialized = true; @@ -321,10 +321,10 @@ void ImuSensor::SetWorldFrameOrientation( // Set orientation reference frame if custom_rpy was supplied if (this->dataPtr->sensorOrientationRelativeTo == WorldFrameEnumType::CUSTOM) { - if (this->dataPtr->CustomRpyParentFrame == "") + if (this->dataPtr->customRpyParentFrame == "") { this->SetOrientationReference(this->dataPtr->worldRelativeOrientation * - this->dataPtr->CustomRpyQuaternion); + this->dataPtr->customRpyQuaternion); } else {