#include <ros/ros.h>
#include <vector>
#include <iostream>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>
#include <aruco/aruco.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "aruco_effector");
    ros::NodeHandle node;

    tf::TransformListener tflisten;
    ros::Rate rate(1.0);
    while (node.ok()) {
        tf::StampedTransform l_transform, r_transform;
        try {
            tflisten.lookupTransform("Aruco_1_frame", "Aruco_0_frame",
                    ros::Time(0), l_transform);
            ROS_INFO("LEFT FRAME");
            std::cout << l_transform.getOrigin().x() << std::endl;
            std::cout << l_transform.getOrigin().y() << std::endl;
            std::cout << l_transform.getOrigin().z() << std::endl;
        }
        catch (tf::TransformException &ex) {
            // ROS_ERROR("%s", ex.what());
        }
        try {
            tflisten.lookupTransform("Aruco_2_frame", "Aruco_0_frame",
                    ros::Time(0), r_transform);
            ROS_INFO("RIGHT FRAME");
            std::cout << r_transform.getOrigin().x() << std::endl;
            std::cout << r_transform.getOrigin().y() << std::endl;
            std::cout << r_transform.getOrigin().z() << std::endl;
        }
        catch (tf::TransformException &ex) {
            // ROS_ERROR("%s", ex.what());
        }
        rate.sleep();
    }
    return 0;
}
