Quadrotor

#include <ros/ros.h>

#include <vision/TargetDescriptor.h>

#include <geometry_msgs/Point.h>

#include <tf/transform_listener.h>

tf::TransformListener listener;

ros::Publisher pub;

void target_cb(const vision::TargetDescriptor target) {

  geometry_msgs::Point point;

  tf::StampedTransform transform;

  try {

    listener.lookupTransform("/camera", "/kinect", ros::Time(0), transform);

  }

  catch (tf::TransformException ex) {

    ROS_ERROR("%s", ex.what());

  }

  // TODO fill point based on target and tf

  pub.publish(point);

}

int main(int argc, char **argv) {

  ros::init(argc, argv, "v2v3_converter");

  ros::NodeHandle n;

  ros::Subscriber sub = n.subscribe("target_2d", 1, target_cb);

  pub = n.advertise<geometry_msgs::Point>("target_3d", 1000);

  ros::spin();

}