From 6e5c91e00647913b2c099d4ef2158e253e4661bd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Fri, 4 Nov 2022 23:05:48 +0100
Subject: [PATCH] Reference initial orientation to world frame in IMU plugin

---
 CHANGELOG.md                  | 3 +++
 plugins/imu/src/IMU.cc        | 3 +++
 plugins/imu/src/IMUDriver.cpp | 6 ++++++
 3 files changed, 12 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 0fd1c3424..c9e439e7c 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -5,6 +5,9 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ## [Unreleased]
 
+### Added
+- Added a new `useWorldReferenceOrientation` config option in the `gazebo_imu` plugin to take account for any initial non-zero orientation of the sensor as measured via `yarp::dev::IOrientationSensors` (https://github.com/robotology/gazebo-yarp-plugins/pull/639).
+
 ## [4.5.2] - 2022-11-17
 
 ### Fixed
diff --git a/plugins/imu/src/IMU.cc b/plugins/imu/src/IMU.cc
index 90ee91256..52f18c402 100644
--- a/plugins/imu/src/IMU.cc
+++ b/plugins/imu/src/IMU.cc
@@ -152,6 +152,9 @@ void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
                                           {"sensor_name", ::yarp::os::Value{_sensor->Name()}}
                                         };
 
+    if (m_parameters.check("useWorldReferenceOrientation")) {
+        imu_properties.put("useWorldReferenceOrientation", yarp::os::Value::getNullValue());
+    }
 
     //Open the driver
     if (!m_imuDriver.open(imu_properties)) {
diff --git a/plugins/imu/src/IMUDriver.cpp b/plugins/imu/src/IMUDriver.cpp
index 8c0e29b4e..cbdc868d0 100644
--- a/plugins/imu/src/IMUDriver.cpp
+++ b/plugins/imu/src/IMUDriver.cpp
@@ -14,6 +14,8 @@
 #include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
 
+#include <ignition/math/Quaternion.hh>
+
 using namespace boost::placeholders;
 
 using namespace yarp::dev;
@@ -88,6 +90,10 @@ bool GazeboYarpIMUDriver::open(yarp::os::Searchable& config)
         return false;
     }
 
+    if (config.check("useWorldReferenceOrientation")) {
+        m_parentSensor->SetWorldToReferenceOrientation(ignition::math::Quaterniond::Identity);
+    }
+
     //Connect the driver to the gazebo simulation
     using namespace boost::placeholders;
     this->m_updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboYarpIMUDriver::onUpdate, this, _1));