From 1f48adae8e837580ca55e6aee9a885dac93842f6 Mon Sep 17 00:00:00 2001 From: Johannes Mey <johannes.mey@tu-dresden.de> Date: Wed, 15 Jul 2020 16:23:19 +0200 Subject: [PATCH] normalize orientations received from trajectories via MQTT --- src/MqttToRosNode.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/MqttToRosNode.cpp b/src/MqttToRosNode.cpp index 0709efe..c40e1d4 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(); -- GitLab