From 7262283b8d48e2fbfcab6298fa56db6667deda68 Mon Sep 17 00:00:00 2001
From: Philipp Caspers <117186241+philipp-caspers@users.noreply.github.com>
Date: Mon, 7 Oct 2024 14:03:15 +0000
Subject: [PATCH] fix: Use correct quaternion assigment

as voraus robot control publishes the `w` quaternion at the fist index.
---
 src/ros_message_creation.rs | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/ros_message_creation.rs b/src/ros_message_creation.rs
index ea2312e..1401d54 100644
--- a/src/ros_message_creation.rs
+++ b/src/ros_message_creation.rs
@@ -51,10 +51,10 @@ pub fn create_pose_stamped_msg(
                 z: pose[2],
             },
             orientation: geometry_msgs::msg::Quaternion {
-                x: quaternion[0],
-                y: quaternion[1],
-                z: quaternion[2],
-                w: quaternion[3],
+                x: quaternion[1],
+                y: quaternion[2],
+                z: quaternion[3],
+                w: quaternion[0],
             },
         },
     }
@@ -133,10 +133,10 @@ mod tests {
                     z: 3.0,
                 },
                 orientation: Quaternion {
-                    x: 4.0,
-                    y: 5.0,
-                    z: 6.0,
-                    w: 7.0,
+                    x: 5.0,
+                    y: 6.0,
+                    z: 7.0,
+                    w: 4.0,
                 },
             },
         };