diff --git a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp index c85f19058..38aff446c 100644 --- a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp +++ b/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("initial_orientation_as_reference"); + RCLCPP_INFO_STREAM(impl_->ros_node_->get_logger(), + " 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("~/out", rclcpp::SensorDataQoS());