Measure IMU orientation with respect to world (ros-simulation#1051)

Report the IMU orientation from the sensor plugin with respect to the world frame.
This complies with convention documented in REP 145: https://www.ros.org/reps/rep-0145.html

In order to not break existing behavior, users should opt-in by adding a new SDF tag.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

gazebo_plugins/src/gazebo_ros_imu_sensor.cpp

@@ -41,6 +41,16 @@ void gazebo::GazeboRosImuSensor::Load(gazebo::sensors::SensorPtr sensor_, sdf::E
     return;
   }
 
+  if (sdf->HasElement("initialOrientationAsReference"))
+  {
+    bool initial_orientation_as_reference = sdf->Get<bool>("initialOrientationAsReference");
+    ROS_INFO_STREAM("<initialOrientationAsReference> set to: "<< initial_orientation_as_reference);
+    if (!initial_orientation_as_reference) {
+      // This complies with REP 145
+      sensor->SetWorldToReferenceOrientation(ignition::math::Quaterniond::Identity);
+    }
+  }
+
   sensor->SetActive(true);
 
   if(!LoadParameters())