From b89574c6c562390bf8bbd6b8cc0639db36cb701c Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Tue, 19 Dec 2017 20:13:28 +0300
Subject: [PATCH] Fix yaw value in rewritten fcu_horiz

---
 clever/src/fcu_horiz.cpp |  9 +++++++++
 clever/src/util.h        | 17 +++++++++++++++++
 2 files changed, 26 insertions(+)
 create mode 100644 clever/src/util.h

diff --git a/clever/src/fcu_horiz.cpp b/clever/src/fcu_horiz.cpp
index 4dd4adc0..aa17c1e3 100644
--- a/clever/src/fcu_horiz.cpp
+++ b/clever/src/fcu_horiz.cpp
@@ -4,6 +4,8 @@
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/PoseStamped.h>
 
+#include "util.h"
+
 class FcuHoriz : public nodelet::Nodelet
 {
     geometry_msgs::TransformStamped t_;
@@ -11,11 +13,18 @@ class FcuHoriz : public nodelet::Nodelet
     void handlePose(const geometry_msgs::PoseStampedConstPtr& msg)
     {
         static tf2_ros::TransformBroadcaster br;
+        double roll, pitch, yaw;
+
         t_.header.stamp = msg->header.stamp;
         t_.header.frame_id = msg->header.frame_id;
         t_.transform.translation.x = msg->pose.position.x;
         t_.transform.translation.y = msg->pose.position.y;
         t_.transform.translation.z = msg->pose.position.z;
+
+        // Warning: this is not thead-safe
+        quaternionToEuler(msg->pose.orientation, roll, pitch, yaw);
+        eulerToQuaternion(t_.transform.rotation, 0, 0, yaw);
+
         br.sendTransform(t_);
     }
 
diff --git a/clever/src/util.h b/clever/src/util.h
new file mode 100644
index 00000000..0eb7f1e9
--- /dev/null
+++ b/clever/src/util.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <tf/transform_datatypes.h>
+#include <geometry_msgs/Quaternion.h>
+
+inline void quaternionToEuler(geometry_msgs::Quaternion q, double& roll, double& pitch, double& yaw)
+{
+    tf::Quaternion tfq(q.x, q.y, q.z, q.w);
+    tf::Matrix3x3 m(tfq);
+    m.getRPY(roll, pitch, yaw);
+}
+
+inline void eulerToQuaternion(geometry_msgs::Quaternion& q, double roll, double pitch, double yaw)
+{
+    tf::Quaternion tfq(roll, pitch, yaw);
+    quaternionTFToMsg(tfq, q);
+}