From bdfefcbe39127c09d1dbc2d815bffa3e79e2f4dd Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 26 Feb 2020 17:57:41 -0800 Subject: [PATCH] Measure IMU orientation with respect to world 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. Signed-off-by: Jacob Perron --- gazebo_plugins/src/gazebo_ros_imu_sensor.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) 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())