Add enable_orientation SDF element to imu

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.

sdf/1.6/imu.sdf

@@ -118,4 +118,8 @@
     </element>
   </element>
 
+  <element name="enable_orientation" type="bool" default="true" required="0">
+    <description>Some IMU sensors rely on external filters to produce orientation estimates. True to generate and output orientation data, false to disable orientation data generation.</description>
+  </element>
+
 </element>

sdf/1.7/imu.sdf

@@ -114,4 +114,7 @@
     </element>
   </element>
 
+  <element name="enable_orientation" type="bool" default="true" required="0">
+    <description>Some IMU sensors rely on external filters to produce orientation estimates. True to generate and output orientation data, false to disable orientation data generation.</description>
+  </element>
 </element>

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<bool>(
+        "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;
+}

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);

test/integration/link_dom.cc

@@ -447,21 +447,21 @@ 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,
       imuSensorObj->AngularVelocityZNoise().DynamicBiasStdDev());
   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");

test/sdf/sensors.sdf

@@ -246,6 +246,7 @@
             <custom_rpy parent_frame="linka">0 1 0</custom_rpy>
             <grav_dir_x parent_frame="linkb">0 0 1</grav_dir_x>
           </orientation_reference_frame>
+          <enable_orientation>false</enable_orientation>
         </imu>
       </sensor>