diff --git a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp index 23bdc8baa..518dbebfe 100644 --- a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp +++ b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp @@ -41,6 +41,28 @@ void gazebo::GazeboRosImuSensor::Load(gazebo::sensors::SensorPtr sensor_, sdf::E return; } + bool initial_orientation_as_reference = false; + if (!sdf->HasElement("initialOrientationAsReference")) + { + ROS_INFO(" is unset, using default value of false " + "to comply with REP 145 (world as orientation reference)"); + } + else + { + initial_orientation_as_reference = sdf->Get("initialOrientationAsReference"); + } + + if (initial_orientation_as_reference) + { + ROS_WARN(" set to true, this behavior is deprecated " + "as it does not comply with REP 145."); + } + else + { + // This complies with REP 145 + sensor->SetWorldToReferenceOrientation(ignition::math::Quaterniond::Identity); + } + sensor->SetActive(true); if(!LoadParameters())