/* This file is part of RGBDSLAM.
*
* RGBDSLAM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RGBDSLAM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RGBDSLAM. If not, see .
*/
#ifndef OPENNI_LISTENER_H
#define OPENNI_LISTENER_H
#include "ros/ros.h"
//#include
#include
#include
#include
#include
#include
#include
#include
#include
#include "graph_manager.h"
#include
#include //for cvMat2QImage not listet here but defined in cpp file
#include
//forward-declare to avoid including tf
namespace tf{
class TransformListener;
}
//The policy merges kinect messages with approximately equal timestamp into one callback
typedef message_filters::sync_policies::ApproximateTime KinectSyncPolicy;
typedef message_filters::sync_policies::ApproximateTime StereoSyncPolicy;
//The policy merges kinect messages with approximately equal timestamp into one callback
typedef message_filters::sync_policies::ApproximateTime NoCloudSyncPolicy;
/**
* Inherits from message_filters::SimpleFilter
* to use protected signalMessage function
*/
template
class BagSubscriber : public message_filters::SimpleFilter
{
public:
void newMessage(const boost::shared_ptr &msg) {
signalMessage(msg);
}
};
//!Handles most of the ROS-based communication
/** The purpose of this class is to listen to
* synchronized image pairs from the kinect
* convert them to opencv, process them, convert
* them to a qt image and send them to the mainwindow
* defined in qtcv.h/.cpp
*/
class OpenNIListener : public QObject {
///QT Stuff, to communicate with the gui
Q_OBJECT
Q_SIGNALS:
///Connect to this signal to get up-to-date optical images from the listener
void newVisualImage(QImage);
///Connect to this signal to get up-to-date featureFlow visualizations from the listener
void newFeatureFlowImage(QImage);
///Connect to this signal to get up-to-date depth images from the listener
void newDepthImage(QImage);
///Connect to this signal to get the transformation matrix from the last frame as QString
void newTransformationMatrix(QString);
//void pauseStatus(bool is_paused);
///Set the info label on the right side in the statusbar of the GUI
void setGUIInfo(QString message);
///Set the temporary status-message in the GUI
void setGUIStatus(QString message);
void bagFinished();
public Q_SLOTS:
///Switch between processing or ignoring new incoming data
void togglePause();
void toggleBagRecording();
///Process a single incomming frame. Useful in pause-mode for getting one snapshot at a time
void getOneFrame();
public:
//!Ctor: setup synced listening to ros topics (kinect/stereo data) and prepare the feature handling
/*!Constructor: The listener needs to know the node handle and the GraphManager instance.
* Which topics to listen to and which feature detector/extractor to use is queried from the parameter
* server.
*/
OpenNIListener(ros::NodeHandle nh, GraphManager* g_mgr);
//!Delete tflistener, shutdown ros publishers
~OpenNIListener();
//! Listen to kinect data, construct nodes and feed the graph manager with it.
/*! For each dataset from the kinect, do some data conversion,
* construct a node, hand it to the graph manager and
* do some visualization of the result in the GUI and RVIZ.
*/
void kinectCallback (const sensor_msgs::ImageConstPtr& visual_img,
const sensor_msgs::ImageConstPtr& depth_img,
const sensor_msgs::PointCloud2ConstPtr& point_cloud);
void noCloudCallback (const sensor_msgs::ImageConstPtr& visual_img_msg,
const sensor_msgs::ImageConstPtr& depth_img_msg,
const sensor_msgs::CameraInfoConstPtr& cam_info_msg) ;
void stereoCallback(const sensor_msgs::ImageConstPtr& visual_img_msg, const sensor_msgs::PointCloud2ConstPtr& point_cloud);
void cameraCallback(cv::Mat visual_img, pointcloud_type::Ptr point_cloud, cv::Mat depth_mono8_img);
void loadBag(const std::string &filename);
protected:
/// Create a QImage from image. The QImage stores its data in the rgba_buffers_ indexed by idx (reused/overwritten each call)
QImage cvMat2QImage(const cv::Mat& image, unsigned int idx);
QImage cvMat2QImage(const cv::Mat& channel1, const cv::Mat& channel2, const cv::Mat& channel3, unsigned int idx);
//!processNode is called by cameraCallback in a separate thread and after finishing visualizes the results
void processNode(cv::Mat& visual_img,
pointcloud_type::Ptr point_cloud,
Node* new_node);
///The GraphManager uses the Node objects to do the actual SLAM
///Public, s.t. the qt signals can be connected to by the holder of the OpenNIListener
GraphManager* graph_mgr_;
//Variables
cv::Ptr detector_;
cv::Ptr extractor_;
message_filters::Synchronizer* stereo_sync_;
message_filters::Synchronizer* kinect_sync_;
message_filters::Synchronizer* no_cloud_sync_;
message_filters::Subscriber *visua_sub_;
message_filters::Subscriber *depth_sub_;
message_filters::Subscriber *cinfo_sub_;
message_filters::Subscriber *cloud_sub_;
BagSubscriber* rgb_img_sub_;
BagSubscriber* depth_img_sub_;
BagSubscriber* cam_info_sub_;
cv::Mat depth_mono8_img_;
std::vector rgba_buffers_;
rosbag::Bag bag;
bool save_bag_file;
//ros::Publisher pc_pub;
/*unsigned int callback_counter_;*/
bool pause_;
bool getOneFrame_;
bool first_frame_;
QFuture future_;
QMutex bagfile_mutex;
tf::TransformListener* tflistener_; //!this being a pointer saves the include (using the above forward declaration)
tf::TransformBroadcaster tf_br_;
ros::Publisher tf_pub_;
int data_id_;
};
//!Creates a pointcloud from rgb8 or mono8 coded images + float depth
pointcloud_type* createXYZRGBPointCloud (const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::ImageConstPtr& rgb_msg, const sensor_msgs::CameraInfoConstPtr& cam_info);
#endif