diff --git a/src/MqttToRosNode.cpp b/src/MqttToRosNode.cpp
index 0709efe25d72a402d386605f76353425b771fbb3..c40e1d45b8041c462dc007bc5a5e26ec8cd87656 100644
--- a/src/MqttToRosNode.cpp
+++ b/src/MqttToRosNode.cpp
@@ -29,10 +29,14 @@ void handleNewTrajectory(const plan::Trajectory &protoTrajectory, const ros::Pub
 
   for (const auto &protoPose : protoTrajectory.pose()) {
     panda_mqtt_connector::Waypoint waypoint;
-    waypoint.pose.orientation.w = protoPose.orientation().w();
-    waypoint.pose.orientation.x = protoPose.orientation().x();
-    waypoint.pose.orientation.y = protoPose.orientation().y();
-    waypoint.pose.orientation.z = protoPose.orientation().z();
+
+    tf2::Quaternion orientation{protoPose.orientation().x(), protoPose.orientation().y(), protoPose.orientation().z(), protoPose.orientation().w()};
+    orientation = orientation.normalize();
+    waypoint.pose.orientation.w = orientation.w();
+    waypoint.pose.orientation.x = orientation.x();
+    waypoint.pose.orientation.y = orientation.y();
+    waypoint.pose.orientation.z = orientation.z();
+
     waypoint.pose.position.x = protoPose.position().x();
     waypoint.pose.position.y = protoPose.position().y();
     waypoint.pose.position.z = protoPose.position().z();