/*
 * orientation2tf.cpp
 *
 *  Created on: 29/09/2014
 *      Author: rescue
 */

#include <ros/ros.h>

#include <geometry_msgs/QuaternionStamped.h>
#include <tf/transform_broadcaster.h>

class Q2TF {
public:
	ros::Subscriber subQuat;
	tf::TransformBroadcaster* tfCaster;

	std::string base_frame, stabilized_frame, odom_frame;
	Q2TF() {
	    ros::NodeHandle nh, nhPriv("~");
	    nhPriv.param< std::string >("base_frame", base_frame, "/camera_link");
	    nhPriv.param< std::string >("stabilized_frame", stabilized_frame, "/camera_stabilized");
	    nhPriv.param< std::string >("odom_frame", odom_frame, "/odom");

	    subQuat = nh.subscribe("orientation", 1, &Q2TF::callback, this);

    	tfCaster = new tf::TransformBroadcaster();
	}

	void callback(const geometry_msgs::QuaternionStampedConstPtr& quat) {
		tf::Quaternion orientation =
				tf::Quaternion(quat->quaternion.x, quat->quaternion.y, quat->quaternion.z, quat->quaternion.w),
				compass;

		double r,p,y;
		tf::Matrix3x3 mat(orientation);
		mat.getEulerYPR(y, p, r);

		orientation.setRPY(0, p, r);
		compass.setRPY(y, 0, 0);

		if (tfCaster) {
			std::vector< tf::StampedTransform > transforms;

			tf::StampedTransform tf;
			tf.frame_id_ = stabilized_frame;
			tf.child_frame_id_ = base_frame;
			tf.stamp_ = quat->header.stamp;
			tf.setOrigin(tf::Vector3(0,0,0));
			tf.setRotation(orientation);
			transforms.push_back(tf);

			if (odom_frame != "") {
				tf.frame_id_ = odom_frame;
				tf.child_frame_id_ = stabilized_frame;
				tf.setRotation(compass);
				transforms.push_back(tf);
			}

			tfCaster->sendTransform(transforms);
		}
	}
};


int main(int argc, char**argv) {
	ros::init(argc, argv, "q2tf");

	Q2TF q2tf;

	while (ros::ok()) {
		ros::spin();
	}
	return 0;
}
