diff --git a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp index 1732da712..29c266b41 100644 --- a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp +++ b/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("initialOrientationAsReference"); + ROS_INFO_STREAM(" 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())