From 51efe93febc99703bb3ea0c5770d5f0758c59a83 Mon Sep 17 00:00:00 2001
From: Raul Dominguez <Raul.Dominguez@dfki.de>
Date: Fri, 26 Jan 2024 17:58:20 +0100
Subject: [PATCH] Adds yaw offset property, since the init pose property
 ignores the orientation

---
 odometry.orogen | 4 +++-
 tasks/Skid.cpp  | 6 ++++++
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/odometry.orogen b/odometry.orogen
index afb8700..9724515 100644
--- a/odometry.orogen
+++ b/odometry.orogen
@@ -30,6 +30,9 @@ task_context 'Generic' do
     property('start_pose', '/base/samples/RigidBodyState').
         doc "starting position for odometry"
 
+    property('yaw_offset', 'double', 0.0).
+        doc "adds this yaw to the body_to_IMUWorld"
+
     ##########################
     # i/o ports 
     ##########################
@@ -168,4 +171,3 @@ task_context 'ContactPointTask' do
 
     port_driven 'dynamic_transformations'
 end
-
diff --git a/tasks/Skid.cpp b/tasks/Skid.cpp
index 8565bcf..b1402de 100644
--- a/tasks/Skid.cpp
+++ b/tasks/Skid.cpp
@@ -97,6 +97,12 @@ void Skid::actuator_samplesTransformerCallback(const base::Time &ts, const base:
 
     // calculates the rotation from body to world base on the orientation measurment 
     Eigen::Quaterniond R_body2World(body2IMUWorld.rotation()); 
+    double yaw_offset = _yaw_offset.get();
+    if( yaw_offset != 0 )
+    {
+        Eigen::AngleAxisd yaw(yaw_offset, Eigen::Vector3d::UnitZ());
+        R_body2World = yaw * R_body2World;
+    }
 
     if(!usePosition)
     {