From 4cab4e752639ebbe081f970e490b5e9b84720ef5 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Thu, 21 Dec 2017 06:22:28 +0300
Subject: [PATCH] Add C++ version of aruco_vpe

---
 clever/src/aruco_vpe.cpp | 112 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 112 insertions(+)
 create mode 100644 clever/src/aruco_vpe.cpp

diff --git a/clever/src/aruco_vpe.cpp b/clever/src/aruco_vpe.cpp
new file mode 100644
index 00000000..e737970b
--- /dev/null
+++ b/clever/src/aruco_vpe.cpp
@@ -0,0 +1,112 @@
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <tf/transform_datatypes.h>
+#include <tf2/exceptions.h>
+#include <tf2/convert.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+
+#include "util.h"
+
+using namespace tf2_ros;
+using geometry_msgs::PoseStamped;
+using geometry_msgs::TransformStamped;
+
+class ArucoVPE : public nodelet::Nodelet
+{
+public:
+    ArucoVPE() : 
+        last_published_(0),
+        lookup_timeout_(0.05)
+    {}
+
+private:    
+    ros::Time last_published_;
+    ros::Duration lookup_timeout_;
+    ros::Publisher vision_position_pub_;
+    ros::Timer dummy_vision_timer_;
+
+    void onInit()
+    {
+        ros::NodeHandle& nh = getNodeHandle();
+        static ros::Subscriber pose_sub = nh.subscribe("aruco_pose/pose", 1, &ArucoVPE::handleArucoPose, this);
+        vision_position_pub_ = nh.advertise<PoseStamped>("mavros/vision_pose/pose", 1);
+
+        dummy_vision_timer_ = nh.createTimer(ros::Duration(0.5), &ArucoVPE::publishDummy, this);
+
+        ROS_INFO("Aruco VPE initialized");
+    }
+
+    void publishDummy(const ros::TimerEvent&)
+    {
+        // This is published to init FCU's position estimator
+        static PoseStamped ps;
+        ps.header.stamp = ros::Time::now();
+        ps.pose.orientation.w = 1;
+        vision_position_pub_.publish(ps);
+    }
+
+    void handleArucoPose(const geometry_msgs::PoseStampedConstPtr& pose)
+    {
+        static TransformBroadcaster br;
+        static Buffer tf_buffer;
+        static TransformListener tfListener(tf_buffer);
+        static StaticTransformBroadcaster static_br;
+        static PoseStamped ps, vpe_raw, vpe;
+        TransformStamped t;
+
+        ros::Time stamp = pose->header.stamp;
+        double roll, pitch, yaw;
+
+        try
+        {
+            // Refine aruco map pose
+            // Reference in local origin
+            t = tf_buffer.lookupTransform("local_origin", "aruco_map_reference", stamp, lookup_timeout_);
+            quaternionToEuler(t.transform.rotation, roll, pitch, yaw);
+            eulerToQuaternion(t.transform.rotation, 0, 0, yaw);
+            t.child_frame_id = "aruco_map_reference_horiz";
+            br.sendTransform(t);
+
+            // Aruco map in reference
+            t = tf_buffer.lookupTransform("aruco_map_reference", "aruco_map_raw", stamp, lookup_timeout_);
+            t.header.frame_id = "aruco_map_reference_horiz";
+            t.child_frame_id = "aruco_map_vision";
+            br.sendTransform(t);
+
+            // Reset VPE
+            if (!last_published_.isValid() or ros::Time::now() - last_published_ > ros::Duration(2))
+            {
+                ROS_INFO("Reset VPE");
+                t = tf_buffer.lookupTransform("local_origin", "aruco_map_vision", stamp, lookup_timeout_);
+                t.child_frame_id = "aruco_map";
+                static_br.sendTransform(t);
+            }
+
+            // Calculate VPE
+            ps.header.frame_id = "fcu_horiz";
+            ps.header.stamp = stamp;
+            ps.pose.orientation.w = 1;
+
+            tf_buffer.transform(ps, vpe_raw, "aruco_map_vision", lookup_timeout_);
+
+            vpe_raw.header.frame_id = "aruco_map";
+            tf_buffer.transform(vpe_raw, vpe, "local_origin", lookup_timeout_);
+
+            vision_position_pub_.publish(vpe);
+
+            last_published_ = stamp;
+            dummy_vision_timer_.stop();
+        }
+        catch (const tf2::TransformException& e)
+        {
+            ROS_WARN_THROTTLE(10, "Aruco VPE: failed to transform: %s", e.what());
+        }
+    }
+};
+
+PLUGINLIB_EXPORT_CLASS(ArucoVPE, nodelet::Nodelet)