Add API for enabling / disabling IMU orientation (#142)

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

include/ignition/sensors/ImuSensor.hh

@@ -118,13 +118,27 @@ namespace ignition
       public: math::Quaterniond OrientationReference() const;
 
       /// \brief Get the orienation of the imu with respect to reference frame
-      /// \return Orientation in reference frame
+      /// \return Orientation in reference frame. If orientation is not
+      /// enabled, this will return the last computed orientation before
+      /// orientation is disabled or identity Quaternion if orientation has
+      /// never been enabled.
       public: math::Quaterniond Orientation() const;
 
       /// \brief Set the gravity vector
       /// \param[in] _gravity gravity vector in meters per second squared.
       public: void SetGravity(const math::Vector3d &_gravity);
 
+      /// \brief Set whether to output orientation. Not all imu's generate
+      /// orientation values as they use filters to produce orientation
+      /// estimates.
+      /// \param[in] _enabled True to publish orientation data, false to leave
+      /// the message field empty.
+      public: void SetOrientationEnabled(bool _enabled);
+
+      /// \brief Get whether or not orientation is enabled.
+      /// \return True if orientation is enabled, false otherwise.
+      public: bool OrientationEnabled() const;
+
       /// \brief Get the gravity vector
       /// \return Gravity vectory in meters per second squared.
       public: math::Vector3d Gravity() const;

src/ImuSensor.cc

@@ -59,6 +59,9 @@ class ignition::sensors::ImuSensorPrivate
   /// \brief transform to Imu frame from Imu reference frame.
   public: ignition::math::Quaterniond orientation;
 
+  /// \brief True to publish orientation data.
+  public: bool orientationEnabled = true;
+
   /// \brief store gravity vector to be added to the IMU output.
   public: ignition::math::Vector3d gravity;
 
@@ -211,20 +214,23 @@ bool ImuSensor::Update(const std::chrono::steady_clock::duration &_now)
   applyNoise(GYROSCOPE_Y_NOISE_RAD_S, this->dataPtr->angularVel.Y());
   applyNoise(GYROSCOPE_Z_NOISE_RAD_S, this->dataPtr->angularVel.Z());
 
-  // Set the IMU orientation
-  // imu orientation with respect to reference frame
-  this->dataPtr->orientation =
-      this->dataPtr->orientationReference.Inverse() *
-      this->dataPtr->worldPose.Rot();
-
   msgs::IMU msg;
   *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
   msg.set_entity_name(this->Name());
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
   frame->add_value(this->Name());
 
-  msgs::Set(msg.mutable_orientation(), this->dataPtr->orientation);
+  if (this->dataPtr->orientationEnabled)
+  {
+    // Set the IMU orientation
+    // imu orientation with respect to reference frame
+    this->dataPtr->orientation =
+        this->dataPtr->orientationReference.Inverse() *
+        this->dataPtr->worldPose.Rot();
+
+    msgs::Set(msg.mutable_orientation(), this->dataPtr->orientation);
+  }
   msgs::Set(msg.mutable_angular_velocity(), this->dataPtr->angularVel);
   msgs::Set(msg.mutable_linear_acceleration(), this->dataPtr->linearAcc);
 
@@ -284,6 +290,18 @@ math::Quaterniond ImuSensor::OrientationReference() const
   return this->dataPtr->orientationReference;
 }
 
+//////////////////////////////////////////////////
+void ImuSensor::SetOrientationEnabled(bool _enabled)
+{
+  this->dataPtr->orientationEnabled = _enabled;
+}
+
+//////////////////////////////////////////////////
+bool ImuSensor::OrientationEnabled() const
+{
+  return this->dataPtr->orientationEnabled;
+}
+
 //////////////////////////////////////////////////
 void ImuSensor::SetGravity(const math::Vector3d &_gravity)
 {

src/ImuSensor_TEST.cc

@@ -311,6 +311,84 @@ TEST(ImuSensor_TEST, ComputeNoise)
   EXPECT_GT(sensor->LinearAcceleration().SquaredLength(), 0.0);
 }
 
+//////////////////////////////////////////////////
+TEST(ImuSensor_TEST, Orientation)
+{
+  // Create a sensor manager
+  ignition::sensors::Manager mgr;
+
+  sdf::ElementPtr imuSDF;
+
+  {
+    const std::string name = "TestImu_Truth";
+    const std::string topic = "/ignition/sensors/test/imu_truth";
+    const double updateRate = 100;
+    const auto accelNoise = noNoiseParameters(updateRate, 0.0);
+    const auto gyroNoise = noNoiseParameters(updateRate, 0.0);
+    const bool always_on = 1;
+    const bool visualize = 1;
+
+    imuSDF = ImuSensorToSDF(name, updateRate, topic,
+      accelNoise, gyroNoise, always_on, visualize);
+  }
+
+  // Create an ImuSensor
+  auto sensor = mgr.CreateSensor<ignition::sensors::ImuSensor>(
+      imuSDF);
+
+  // Make sure the above dynamic cast worked.
+  ASSERT_NE(nullptr, sensor);
+
+  math::Quaterniond orientRef;
+  math::Quaterniond orientValue(math::Vector3d(IGN_PI/2.0, 0, IGN_PI));
+  math::Pose3d pose(math::Vector3d(0, 1, 2), orientValue);
+
+  sensor->SetOrientationReference(orientRef);
+  sensor->SetWorldPose(pose);
+
+  sensor->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(10000000)));
+
+  // Check orientation
+  EXPECT_TRUE(sensor->OrientationEnabled());
+  EXPECT_EQ(orientRef, sensor->OrientationReference());
+  EXPECT_EQ(orientValue, sensor->Orientation());
+
+  // update pose and check orientation
+  math::Quaterniond newOrientValue(math::Vector3d(IGN_PI, IGN_PI/2, IGN_PI));
+  math::Pose3d newPose(math::Vector3d(0, 1, 1), newOrientValue);
+  sensor->SetWorldPose(newPose);
+
+  sensor->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(20000000)));
+
+  EXPECT_TRUE(sensor->OrientationEnabled());
+  EXPECT_EQ(orientRef, sensor->OrientationReference());
+  EXPECT_EQ(newOrientValue, sensor->Orientation());
+
+  // disable orientation and check
+  sensor->SetOrientationEnabled(false);
+  EXPECT_FALSE(sensor->OrientationEnabled());
+  EXPECT_EQ(orientRef, sensor->OrientationReference());
+  // orientation remains the same after disabling orientation
+  EXPECT_EQ(newOrientValue, sensor->Orientation());
+
+  // update world pose with orientation disabled and verify that orientation
+  // does not change
+  math::Quaterniond newOrientValue2(math::Vector3d(IGN_PI/2, IGN_PI/2, IGN_PI));
+  math::Pose3d newPose2(math::Vector3d(1, 1, 0), newOrientValue2);
+  sensor->SetWorldPose(newPose2);
+  sensor->Update(std::chrono::steady_clock::duration(
+    std::chrono::nanoseconds(20000000)));
+
+  sensor->SetOrientationEnabled(false);
+  EXPECT_FALSE(sensor->OrientationEnabled());
+  EXPECT_EQ(orientRef, sensor->OrientationReference());
+  // orientation should still be the previous value because it is not being
+  // updated.
+  EXPECT_EQ(newOrientValue, sensor->Orientation());
+
+}
 
 //////////////////////////////////////////////////
 int main(int argc, char **argv)