Go to the documentation of this file.00001
00018 #ifndef TFVISUALIZER_H
00019 #define TFVISUALIZER_H
00020 #include <QThread>
00021
00022 #ifndef Q_MOC_RUN
00023 #include <ros/ros.h>
00024 #include <ros/init.h>
00025 #include <boost/shared_ptr.hpp>
00026 #include <opencv/cv.h>
00027 #include <sensor_msgs/Image.h>
00028 #include <sensor_msgs/image_encodings.h>
00029 #include <sensor_msgs/CameraInfo.h>
00030 #include <tf/transform_broadcaster.h>
00031 #include <image_transport/image_transport.h>
00032 #include <image_geometry/pinhole_camera_model.h>
00033 #include <tf/transform_listener.h>
00034 #endif
00035
00036
00037 class TF_Visualizer : public QThread
00038 {
00039 Q_OBJECT
00040 public:
00041 explicit TF_Visualizer(const std::vector<std::string>& frame_ids, std::string camera_input_topic, std::string camera_output_topic);
00042 void imageCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg);
00043
00044 void stop();
00045 protected:
00046
00047 ros::NodeHandle nh_;
00048 image_transport::ImageTransport it_;
00049 image_transport::CameraSubscriber sub_;
00050 image_transport::Publisher pub_;
00051 tf::TransformListener tf_listener_;
00052 image_geometry::PinholeCameraModel cam_model_;
00053 std::vector<std::string> frame_ids_;
00054 std::string camera_input_topic_;
00055 std::string camera_output_topic_;
00056 CvFont font_;
00057
00058 void run();
00059 };
00060
00061
00062 #endif // TFVISUALIZER_H