Statistics
| Branch: | Revision:

root / rgbdslam / src / openni_listener.h @ 9240aaa3

History | View | Annotate | Download (7.53 KB)

1
/* This file is part of RGBDSLAM.
2
 * 
3
 * RGBDSLAM is free software: you can redistribute it and/or modify
4
 * it under the terms of the GNU General Public License as published by
5
 * the Free Software Foundation, either version 3 of the License, or
6
 * (at your option) any later version.
7
 * 
8
 * RGBDSLAM is distributed in the hope that it will be useful,
9
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11
 * GNU General Public License for more details.
12
 * 
13
 * You should have received a copy of the GNU General Public License
14
 * along with RGBDSLAM.  If not, see <http://www.gnu.org/licenses/>.
15
 */
16

    
17

    
18
#ifndef OPENNI_LISTENER_H
19
#define OPENNI_LISTENER_H
20
#include "ros/ros.h"
21
//#include <pcl_tf/transforms.h>
22
#include <message_filters/subscriber.h>
23
#include <message_filters/synchronizer.h>
24
#include <message_filters/sync_policies/approximate_time.h>
25
#include <sensor_msgs/Image.h>
26
#include <sensor_msgs/CameraInfo.h>
27
#include <sensor_msgs/PointCloud2.h>
28
#include <opencv2/core/core.hpp>
29
#include <opencv2/features2d/features2d.hpp>
30
#include "graph_manager.h"
31
#include <qtconcurrentrun.h>
32
#include <QImage> //for cvMat2QImage not listet here but defined in cpp file
33
#include <rosbag/bag.h>
34

    
35
//forward-declare to avoid including tf
36
namespace tf{
37
  class TransformListener;
38
}
39

    
40

    
41
//The policy merges kinect messages with approximately equal timestamp into one callback 
42
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, 
43
                                                        sensor_msgs::Image, 
44
                                                        sensor_msgs::PointCloud2> KinectSyncPolicy;
45

    
46
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
47
                                                        sensor_msgs::PointCloud2> StereoSyncPolicy;
48

    
49
//The policy merges kinect messages with approximately equal timestamp into one callback 
50
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, 
51
                                                        sensor_msgs::Image,
52
                                                        sensor_msgs::CameraInfo> NoCloudSyncPolicy;
53

    
54

    
55
/**
56
 * Inherits from message_filters::SimpleFilter<M>
57
 * to use protected signalMessage function 
58
 */
59
template <class M>
60
class BagSubscriber : public message_filters::SimpleFilter<M>
61
{
62
public:
63
  void newMessage(const boost::shared_ptr<M const> &msg) { 
64
    signalMessage(msg); 
65
  }
66
};
67

    
68
//!Handles most of the ROS-based communication
69

    
70
/** The purpose of this class is to listen to 
71
 * synchronized image pairs from the kinect
72
 * convert them to opencv, process them, convert
73
 * them to a qt image and send them to the mainwindow
74
 * defined in qtcv.h/.cpp
75
 */
76
class OpenNIListener : public QObject {
77
  ///QT Stuff, to communicate with the gui
78
  Q_OBJECT
79
  Q_SIGNALS:
80
    ///Connect to this signal to get up-to-date optical images from the listener
81
    void newVisualImage(QImage);
82
    ///Connect to this signal to get up-to-date featureFlow visualizations from the listener
83
    void newFeatureFlowImage(QImage);
84
    ///Connect to this signal to get up-to-date depth images from the listener
85
    void newDepthImage(QImage);
86
    ///Connect to this signal to get the transformation matrix from the last frame as QString
87
    void newTransformationMatrix(QString);
88
    //void pauseStatus(bool is_paused);
89
    ///Set the info label on the right side in the statusbar of the GUI
90
    void setGUIInfo(QString message);
91
    ///Set the temporary status-message in the GUI
92
    void setGUIStatus(QString message);
93
    void bagFinished();
94
  public Q_SLOTS:
95
    ///Switch between processing or ignoring new incoming data
96
    void togglePause();
97
    void toggleBagRecording();
98
    ///Process a single incomming frame. Useful in pause-mode for getting one snapshot at a time
99
    void getOneFrame();
100

    
101
  public:
102
    //!Ctor: setup synced listening to ros topics (kinect/stereo data) and prepare the feature handling
103
    /*!Constructor: The listener needs to know the node handle and the GraphManager instance.
104
     * Which topics to listen to and which feature detector/extractor to use is queried from the parameter 
105
     * server.
106
     */
107
    OpenNIListener(ros::NodeHandle nh, GraphManager* g_mgr);
108

    
109
    //!Delete tflistener, shutdown ros publishers
110
    ~OpenNIListener();
111
    //! Listen to kinect data, construct nodes and feed the graph manager with it.
112
    /*! For each dataset from the kinect, do some data conversion,
113
     *  construct a node, hand it to the graph manager and
114
     *  do some visualization of the result in the GUI and RVIZ.
115
     */
116
    void kinectCallback (const sensor_msgs::ImageConstPtr& visual_img,
117
                         const sensor_msgs::ImageConstPtr& depth_img, 
118
                         const sensor_msgs::PointCloud2ConstPtr& point_cloud);
119
    void noCloudCallback (const sensor_msgs::ImageConstPtr& visual_img_msg,
120
                          const sensor_msgs::ImageConstPtr& depth_img_msg,
121
                          const sensor_msgs::CameraInfoConstPtr& cam_info_msg) ;
122
    void stereoCallback(const sensor_msgs::ImageConstPtr& visual_img_msg, const sensor_msgs::PointCloud2ConstPtr& point_cloud);
123
    void cameraCallback(cv::Mat visual_img, pointcloud_type::Ptr point_cloud, cv::Mat depth_mono8_img);
124
    void loadBag(const std::string &filename);
125
  protected:
126
    /// Create a QImage from image. The QImage stores its data in the rgba_buffers_ indexed by idx (reused/overwritten each call)
127
    QImage cvMat2QImage(const cv::Mat& image, unsigned int idx); 
128
    QImage cvMat2QImage(const cv::Mat& channel1, const cv::Mat& channel2, const cv::Mat& channel3, unsigned int idx);
129

    
130
    //!processNode is called by cameraCallback in a separate thread and after finishing visualizes the results
131
    void processNode(cv::Mat& visual_img,  
132
                     pointcloud_type::Ptr point_cloud,
133
                     Node* new_node);
134

    
135
    ///The GraphManager uses the Node objects to do the actual SLAM
136
    ///Public, s.t. the qt signals can be connected to by the holder of the OpenNIListener
137
    GraphManager* graph_mgr_;
138
    
139
    //Variables
140
    cv::Ptr<cv::FeatureDetector> detector_;
141
    cv::Ptr<cv::DescriptorExtractor> extractor_;
142

    
143
    message_filters::Synchronizer<StereoSyncPolicy>* stereo_sync_;
144
    message_filters::Synchronizer<KinectSyncPolicy>* kinect_sync_;
145
    message_filters::Synchronizer<NoCloudSyncPolicy>* no_cloud_sync_;
146
    message_filters::Subscriber<sensor_msgs::Image> *visua_sub_;      
147
    message_filters::Subscriber<sensor_msgs::Image> *depth_sub_;      
148
    message_filters::Subscriber<sensor_msgs::CameraInfo> *cinfo_sub_;      
149
    message_filters::Subscriber<sensor_msgs::PointCloud2> *cloud_sub_;
150

    
151
    BagSubscriber<sensor_msgs::Image>* rgb_img_sub_;
152
    BagSubscriber<sensor_msgs::Image>* depth_img_sub_;
153
    BagSubscriber<sensor_msgs::CameraInfo>* cam_info_sub_;
154

    
155
    cv::Mat depth_mono8_img_;
156
    std::vector<cv::Mat> rgba_buffers_;
157
    
158
    rosbag::Bag bag;
159
    bool save_bag_file;
160
    
161
    //ros::Publisher pc_pub; 
162
    /*unsigned int callback_counter_;*/
163
    bool pause_;
164
    bool getOneFrame_;
165
    bool first_frame_;
166
    QFuture<void> future_;
167
    QMutex bagfile_mutex;
168
    tf::TransformListener* tflistener_; //!this being a pointer saves the include (using the above forward declaration)
169
    tf::TransformBroadcaster tf_br_;
170
    ros::Publisher tf_pub_;
171
    int data_id_;
172
};
173

    
174
//!Creates a pointcloud from rgb8 or mono8 coded images + float depth
175
pointcloud_type* createXYZRGBPointCloud (const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::ImageConstPtr& rgb_msg, const sensor_msgs::CameraInfoConstPtr& cam_info); 
176
#endif