root / rgbdslam / src / openni_listener.h @ 9240aaa3
History | View | Annotate | Download (7.53 KB)
1 | 9240aaa3 | Alex | /* 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 |