From 3045cc22f58d47999175f5546aa1d4db37099ba8 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 2 Aug 2021 16:57:48 -0700 Subject: [PATCH 1/2] add enable_orientation element to imu Signed-off-by: Ian Chen --- include/sdf/Imu.hh | 9 +++++++++ sdf/1.6/imu.sdf | 4 ++++ sdf/1.7/imu.sdf | 3 +++ src/Imu.cc | 21 +++++++++++++++++++++ src/Imu_TEST.cc | 4 ++++ test/integration/link_dom.cc | 4 ++-- test/sdf/sensors.sdf | 1 + 7 files changed, 44 insertions(+), 2 deletions(-) diff --git a/include/sdf/Imu.hh b/include/sdf/Imu.hh index 1ff9c2f89..83b42a97f 100644 --- a/include/sdf/Imu.hh +++ b/include/sdf/Imu.hh @@ -255,6 +255,15 @@ namespace sdf /// \param[in] _frame The name of the parent frame. public: void SetCustomRpyParentFrame(const std::string &_frame) const; + /// \brief Get whether orientation data generation is enabled. + /// \return True if orientation data generation is enabled orientation data, + /// false otherwise. + public: bool OrientationEnabled() const; + + /// \brief Set whether to enable orientation data generation. + /// \param[in] _enabled True to enabled orientation data, false to disable it. + public: void SetOrientationEnabled(bool _enabled); + /// \brief Return true if both Imu objects contain the same values. /// \param[_in] _imu Imu value to compare. /// \returen True if 'this' == _imu. diff --git a/sdf/1.6/imu.sdf b/sdf/1.6/imu.sdf index 70f9988fc..f68fc9e22 100644 --- a/sdf/1.6/imu.sdf +++ b/sdf/1.6/imu.sdf @@ -118,4 +118,8 @@ + + Some IMU sensors rely on external filters to produce orientation estimates. True to generate and output orientation data, false to disable orientation data generation. + + diff --git a/sdf/1.7/imu.sdf b/sdf/1.7/imu.sdf index cd23f872c..25690c8f1 100644 --- a/sdf/1.7/imu.sdf +++ b/sdf/1.7/imu.sdf @@ -114,4 +114,7 @@ + + Some IMU sensors rely on external filters to produce orientation estimates. True to generate and output orientation data, false to disable orientation data generation. + diff --git a/src/Imu.cc b/src/Imu.cc index 70b33b9e7..6c14a1a7b 100644 --- a/src/Imu.cc +++ b/src/Imu.cc @@ -64,6 +64,9 @@ class sdf::ImuPrivate /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; + + /// \brief True to enable orientation + public: bool orientationEnabled = true; }; ////////////////////////////////////////////////// @@ -210,6 +213,12 @@ Errors Imu::Load(ElementPtr _sdf) } } + if (_sdf->HasElement("enable_orientation")) + { + this->dataPtr->orientationEnabled = _sdf->Get( + "enable_orientation", this->dataPtr->orientationEnabled).first; + } + return errors; } @@ -377,3 +386,15 @@ void Imu::SetCustomRpyParentFrame(const std::string &_frame) const { this->dataPtr->customRpyParentFrame = _frame; } + +////////////////////////////////////////////////// +void Imu::SetOrientationEnabled(bool _enabled) +{ + this->dataPtr->orientationEnabled = _enabled; +} + +////////////////////////////////////////////////// +bool Imu::OrientationEnabled() const +{ + return this->dataPtr->orientationEnabled; +} diff --git a/src/Imu_TEST.cc b/src/Imu_TEST.cc index 2d76c20ca..4deda538b 100644 --- a/src/Imu_TEST.cc +++ b/src/Imu_TEST.cc @@ -75,6 +75,10 @@ TEST(DOMImu, Construction) imu.SetLocalization("NED"); EXPECT_EQ("NED", imu.Localization()); + EXPECT_TRUE(imu.OrientationEnabled()); + imu.SetOrientationEnabled(false); + EXPECT_FALSE(imu.OrientationEnabled()); + // Copy Constructor sdf::Imu imu2(imu); EXPECT_EQ(imu, imu2); diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 2702f9731..688a2afdb 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -423,7 +423,6 @@ TEST(DOMLink, Sensors) EXPECT_DOUBLE_EQ(5.0, imuSensorObj->AngularVelocityYNoise().DynamicBiasCorrelationTime()); - EXPECT_DOUBLE_EQ(5.0, imuSensorObj->AngularVelocityZNoise().Mean()); EXPECT_DOUBLE_EQ(5.1, imuSensorObj->AngularVelocityZNoise().StdDev()); EXPECT_DOUBLE_EQ(6.2, @@ -431,13 +430,14 @@ TEST(DOMLink, Sensors) EXPECT_DOUBLE_EQ(6.0, imuSensorObj->AngularVelocityZNoise().DynamicBiasCorrelationTime()); - EXPECT_EQ("ENU", imuSensorObj->Localization()); EXPECT_EQ("linka", imuSensorObj->CustomRpyParentFrame()); EXPECT_EQ(ignition::math::Vector3d::UnitY, imuSensorObj->CustomRpy()); EXPECT_EQ("linkb", imuSensorObj->GravityDirXParentFrame()); EXPECT_EQ(ignition::math::Vector3d::UnitZ, imuSensorObj->GravityDirX()); + EXPECT_FALSE(imuSensorObj->OrientationEnabled()); + // Get the logical camera sensor const sdf::Sensor *logicalCameraSensor = link->SensorByName("logical_camera_sensor"); diff --git a/test/sdf/sensors.sdf b/test/sdf/sensors.sdf index b65f07d0d..c8f76aa62 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -213,6 +213,7 @@ 0 1 0 0 0 1 + false From 428c82a0d1e6536995faa2b4be7aee5183288504 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 2 Aug 2021 17:31:26 -0700 Subject: [PATCH 2/2] codecheck Signed-off-by: Ian Chen --- include/sdf/Imu.hh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/sdf/Imu.hh b/include/sdf/Imu.hh index 83b42a97f..e8b116602 100644 --- a/include/sdf/Imu.hh +++ b/include/sdf/Imu.hh @@ -261,7 +261,8 @@ namespace sdf public: bool OrientationEnabled() const; /// \brief Set whether to enable orientation data generation. - /// \param[in] _enabled True to enabled orientation data, false to disable it. + /// \param[in] _enabled True to enabled orientation data, false to disable + /// it. public: void SetOrientationEnabled(bool _enabled); /// \brief Return true if both Imu objects contain the same values.