diff --git a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp index 5e9a494dc..7757e252e 100644 --- a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp +++ b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp @@ -69,6 +69,26 @@ void GazeboRosImuSensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPt return; } + bool initial_orientation_as_reference = false; + if (!_sdf->HasElement("initial_orientation_as_reference")) { + RCLCPP_INFO_STREAM( + impl_->ros_node_->get_logger(), + " is unset, using default value of false " + "to comply with REP 145 (world as orientation reference)"); + } else { + initial_orientation_as_reference = _sdf->Get("initial_orientation_as_reference"); + } + + if (initial_orientation_as_reference) { + RCLCPP_WARN_STREAM( + impl_->ros_node_->get_logger(), + " set to true, this behavior is deprecated " + "as it does not comply with REP 145."); + } else { + // This complies with REP 145 + impl_->sensor_->SetWorldToReferenceOrientation(ignition::math::Quaterniond::Identity); + } + impl_->pub_ = impl_->ros_node_->create_publisher( "~/out", qos.get_publisher_qos("~/out", rclcpp::SensorDataQoS()));