normalize quaternions to be in half-space w >= 0

... as in tf1
ros · Oct 19, 2018 · b1f5287 · b1f5287
1 parent 70484eb
commit b1f5287
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h
@@ -314,6 +314,12 @@ geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) {
   msg.orientation.y = q.y();
   msg.orientation.z = q.z();
   msg.orientation.w = q.w();
+  if (msg.orientation.w < 0) {
+    msg.orientation.x *= -1;
+    msg.orientation.y *= -1;
+    msg.orientation.z *= -1;
+    msg.orientation.w *= -1;
+  }
   return msg;
 }