diff --git a/include/sdf/Imu.hh b/include/sdf/Imu.hh index 1ff9c2f89..e8b116602 100644 --- a/include/sdf/Imu.hh +++ b/include/sdf/Imu.hh @@ -255,6 +255,16 @@ 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 506c868a6..4210f46e2 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -447,7 +447,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, @@ -455,13 +454,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 087dee6bd..00d05d684 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -246,6 +246,7 @@ 0 1 0 0 0 1 + false