Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef OPENNI_LISTENER_H
00019 #define OPENNI_LISTENER_H
00020 #include "ros/ros.h"
00021
00022 #include <message_filters/subscriber.h>
00023 #include <message_filters/synchronizer.h>
00024 #include <message_filters/sync_policies/approximate_time.h>
00025 #include <sensor_msgs/Image.h>
00026 #include <sensor_msgs/CameraInfo.h>
00027 #include <sensor_msgs/PointCloud2.h>
00028 #include <opencv2/core/core.hpp>
00029 #include <opencv2/features2d/features2d.hpp>
00030 #include "graph_manager.h"
00031 #include <qtconcurrentrun.h>
00032 #include <QImage>
00033 #include <rosbag/bag.h>
00034
00035
00037 namespace tf{
00038 class TransformListener;
00039 }
00041
00042
00043 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
00044 sensor_msgs::Image,
00045 sensor_msgs::PointCloud2> KinectSyncPolicy;
00046
00047 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
00048 sensor_msgs::PointCloud2> StereoSyncPolicy;
00049
00050
00051 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
00052 sensor_msgs::Image,
00053 sensor_msgs::CameraInfo> NoCloudSyncPolicy;
00054
00055
00060 template <class M>
00061 class BagSubscriber : public message_filters::SimpleFilter<M>
00062 {
00063 public:
00064 void newMessage(const boost::shared_ptr<M const> &msg) {
00065 signalMessage(msg);
00066 }
00067 };
00068
00070
00077 class OpenNIListener : public QObject {
00079 Q_OBJECT
00080 Q_SIGNALS:
00082 void newVisualImage(QImage);
00084 void newFeatureFlowImage(QImage);
00086 void newDepthImage(QImage);
00088 void newTransformationMatrix(QString);
00089
00091 void setGUIInfo(QString message);
00093 void setGUIStatus(QString message);
00094 void bagFinished();
00095
00096 public Q_SLOTS:
00098 void togglePause();
00099 void toggleBagRecording();
00101 void getOneFrame();
00102
00103 public:
00105
00109 OpenNIListener(ros::NodeHandle nh, GraphManager* g_mgr);
00110
00112 ~OpenNIListener();
00114
00118 void kinectCallback (const sensor_msgs::ImageConstPtr& visual_img,
00119 const sensor_msgs::ImageConstPtr& depth_img,
00120 const sensor_msgs::PointCloud2ConstPtr& point_cloud);
00122 void noCloudCallback (const sensor_msgs::ImageConstPtr& visual_img_msg,
00123 const sensor_msgs::ImageConstPtr& depth_img_msg,
00124 const sensor_msgs::CameraInfoConstPtr& cam_info_msg) ;
00126 void stereoCallback(const sensor_msgs::ImageConstPtr& visual_img_msg, const sensor_msgs::PointCloud2ConstPtr& point_cloud);
00127
00128 void loadBag(const std::string &filename);
00129 protected:
00132 QImage cvMat2QImage(const cv::Mat& image, unsigned int idx);
00135 QImage cvMat2QImage(const cv::Mat& channel1, const cv::Mat& channel2, const cv::Mat& channel3, unsigned int idx);
00137 void retrieveTransformations(std_msgs::Header depth_header, Node* node_ptr);
00138
00140 void callProcessing(cv::Mat gray_img, Node* node_ptr);
00142 void processNode(Node* new_node);
00143
00145 void cameraCallback(cv::Mat visual_img,
00146 pointcloud_type::Ptr point_cloud,
00147 cv::Mat depth_mono8_img);
00149 void noCloudCameraCallback(cv::Mat visual_img,
00150 cv::Mat depth,
00151 cv::Mat depth_mono8_img,
00152 std_msgs::Header depth_header,
00153 const sensor_msgs::CameraInfoConstPtr& cam_info);
00156 GraphManager* graph_mgr_;
00157
00158
00159 cv::Ptr<cv::FeatureDetector> detector_;
00160 cv::Ptr<cv::DescriptorExtractor> extractor_;
00161
00162 message_filters::Synchronizer<StereoSyncPolicy>* stereo_sync_;
00163 message_filters::Synchronizer<KinectSyncPolicy>* kinect_sync_;
00164 message_filters::Synchronizer<NoCloudSyncPolicy>* no_cloud_sync_;
00165 message_filters::Subscriber<sensor_msgs::Image> *visua_sub_;
00166 message_filters::Subscriber<sensor_msgs::Image> *depth_sub_;
00167 message_filters::Subscriber<sensor_msgs::CameraInfo> *cinfo_sub_;
00168 message_filters::Subscriber<sensor_msgs::PointCloud2> *cloud_sub_;
00169
00170 BagSubscriber<sensor_msgs::Image>* rgb_img_sub_;
00171 BagSubscriber<sensor_msgs::Image>* depth_img_sub_;
00172 BagSubscriber<sensor_msgs::CameraInfo>* cam_info_sub_;
00173
00174 cv::Mat depth_mono8_img_;
00176 cv::Mat visualization_depth_mono8_img_;
00178 cv::Mat visualization_img_;
00179 std::vector<cv::Mat> rgba_buffers_;
00180
00181 rosbag::Bag bag;
00182 bool save_bag_file;
00183
00184
00185
00186 bool pause_;
00187 bool getOneFrame_;
00188 bool first_frame_;
00189 QFuture<void> future_;
00190 QMutex bagfile_mutex;
00191 tf::TransformListener* tflistener_;
00192 tf::TransformBroadcaster tf_br_;
00193 ros::Publisher tf_pub_;
00194 int data_id_;
00195 std::string image_encoding_;
00196 };
00197
00198 #endif