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

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.

Co-authored-by: Jacob Perron <jacob@openrobotics.org>

gazebo_plugins/src/gazebo_ros_imu_sensor.cpp

@@ -66,6 +66,16 @@ void GazeboRosImuSensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPt
     return;
   }
 
+  if (_sdf->HasElement("initial_orientation_as_reference")) {
+    bool initial_orientation_as_reference = _sdf->Get<bool>("initial_orientation_as_reference");
+    RCLCPP_INFO_STREAM(impl_->ros_node_->get_logger(),
+      "<initial_orientation_as_reference> set to: " << initial_orientation_as_reference);
+    if (!initial_orientation_as_reference) {
+      // This complies with REP 145
+      impl_->sensor_->SetWorldToReferenceOrientation(ignition::math::Quaterniond::Identity);
+    }
+  }
+
   impl_->pub_ =
     impl_->ros_node_->create_publisher<sensor_msgs::msg::Imu>("~/out", rclcpp::SensorDataQoS());