///Connect Signals and Slots only relevant for the graphical interface
void gui_connections(Graphical_UI* gui, GraphManager* graph_mgr, OpenNIListener* listener)
{
QObject::connect(listener, SIGNAL(newVisualImage(QImage)), gui, SLOT(setVisualImage(QImage)));
QObject::connect(listener, SIGNAL(newFeatureFlowImage(QImage)), gui, SLOT(setFeatureFlowImage(QImage)));
QObject::connect(listener, SIGNAL(newDepthImage(QImage)), gui, SLOT(setDepthImage(QImage)));
QObject::connect(graph_mgr, SIGNAL(sendFinished()), gui, SLOT(sendFinished()));
QObject::connect(graph_mgr, SIGNAL(iamBusy(int, const char*, int)), gui, SLOT(showBusy(int, const char*, int)));
QObject::connect(graph_mgr, SIGNAL(progress(int, const char*, int)), gui, SLOT(setBusy(int, const char*, int)));
QObject::connect(graph_mgr, SIGNAL(setGUIInfo(QString)), gui, SLOT(setInfo(QString)));
QObject::connect(graph_mgr, SIGNAL(setGUIStatus(QString)), gui, SLOT(setStatus(QString)));
QObject::connect(gui, SIGNAL(printEdgeErrors(QString)), graph_mgr, SLOT(printEdgeErrors(QString)));
QObject::connect(gui, SIGNAL(pruneEdgesWithErrorAbove(float)), graph_mgr, SLOT(pruneEdgesWithErrorAbove(float)));
QObject::connect(gui, SIGNAL(clearClouds()), graph_mgr, SLOT(clearPointClouds()));
if (ParameterServer::instance()->get<bool>("use_glwidget") && gui->getGLViewer() != NULL) {
GLViewer* glv = gui->getGLViewer();
QObject::connect(graph_mgr, SIGNAL(setPointCloud(pointcloud_type *, QMatrix4x4)), glv, SLOT(addPointCloud(pointcloud_type *, QMatrix4x4)), Qt::BlockingQueuedConnection ); //Needs to block, otherwise the opengl list compilation makes the app unresponsive. This effectively throttles the processing rate though
typedef const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >* cnst_ft_vectors;
QObject::connect(graph_mgr, SIGNAL(setFeatures(const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >*)), glv, SLOT(addFeatures(const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >*))); //, Qt::DirectConnection);
QObject::connect(graph_mgr, SIGNAL(setGraphEdges(const QList<QPair<int, int> >*)), glv, SLOT(setEdges(const QList<QPair<int, int> >*)));
QObject::connect(graph_mgr, SIGNAL(updateTransforms(QList<QMatrix4x4>*)), glv, SLOT(updateTransforms(QList<QMatrix4x4>*)));
QObject::connect(graph_mgr, SIGNAL(deleteLastNode()), glv, SLOT(deleteLastNode()));
QObject::connect(graph_mgr, SIGNAL(resetGLViewer()), glv, SLOT(reset()));
if(!ParameterServer::instance()->get<bool>("store_pointclouds")) {
QObject::connect(glv, SIGNAL(cloudRendered(pointcloud_type const *)), graph_mgr, SLOT(clearPointCloud(pointcloud_type const *))); //
} else if(ParameterServer::instance()->get<double>("voxelfilter_size") > 0.0) {
QObject::connect(glv, SIGNAL(cloudRendered(pointcloud_type const *)), graph_mgr, SLOT(reducePointCloud(pointcloud_type const *))); //
}
}
QObject::connect(listener, SIGNAL(setGUIInfo(QString)), gui, SLOT(setInfo(QString)));
QObject::connect(listener, SIGNAL(setGUIStatus(QString)), gui, SLOT(setStatus(QString)));
QObject::connect(graph_mgr, SIGNAL(setGUIInfo2(QString)), gui, SLOT(setInfo2(QString)));
}
void GraphManager::sendAllCloudsImpl()
{
ScopedTimer s(__FUNCTION__);
if (batch_cloud_pub_.getNumSubscribers() == 0){
ROS_WARN("No Subscribers: Sending of clouds cancelled");
return;
}
ROS_INFO("Sending out all clouds");
batch_processing_runs_ = true;
ros::Rate r(ParameterServer::instance()->get<double>("send_clouds_rate")); //slow down a bit, to allow for transmitting to and processing in other nodes
for (graph_it it = graph_.begin(); it != graph_.end(); ++it)
{
Node* node = it->second;
if(!node->valid_tf_estimate_) {
ROS_INFO("Skipping node %i: No valid estimate", node->id_);
continue;
}
tf::StampedTransform base_to_fixed = this->computeFixedToBaseTransform(node, true);
br_.sendTransform(base_to_fixed);
publishCloud(node, base_to_fixed.stamp_, batch_cloud_pub_);
QString message;
Q_EMIT setGUIInfo(message.sprintf("Sending pointcloud and map transform (%i/%i) on topics %s and /tf", it->first, (int)graph_.size(), ParameterServer::instance()->get<std::string>("individual_cloud_out_topic").c_str()) );
r.sleep();
}
batch_processing_runs_ = false;
Q_EMIT sendFinished();
}
void GraphManager::deleteLastFrame(){
if(graph_.size() <= 1) {
ROS_INFO("Resetting, as the only node is to be deleted");
reset_request_ = true;
Q_EMIT deleteLastNode();
return;
}
deleteCameraFrame(graph_.size()-1);
Q_EMIT deleteLastNode();
optimizeGraph();//s.t. the effect of the removed edge transforms are removed to
ROS_INFO("Removed most recent node");
Q_EMIT setGUIInfo("Removed most recent node");
//Q_EMIT setGraphEdges(getGraphEdges());
//updateTransforms needs to be last, as it triggers a redraw
//Q_EMIT updateTransforms(getAllPosesAsMatrixList());
}
/**This function reads the sensor input from a bagfile specified in the parameter bagfile_name.
* It is meant for offline processing of each frame */
void BagLoader::loadBag(std::string filename)
{
ScopedTimer s(__FUNCTION__);
ros::NodeHandle nh;
ParameterServer* ps = ParameterServer::instance();
std::string points_tpc = ps->get<std::string>("individual_cloud_out_topic").c_str();//ps->get<std::string>("topic_points");
ROS_INFO_STREAM("Listening to " << points_tpc);
std::string tf_tpc = std::string("/tf");
tf_pub_ = nh.advertise<tf::tfMessage>(tf_tpc, 10);
ROS_INFO("Loading Bagfile %s", filename.c_str());
Q_EMIT setGUIStatus(QString("Loading ")+filename.c_str());
{ //bag will be destructed after this block (hopefully frees memory for the optimizer)
rosbag::Bag bag;
try{
bag.open(filename, rosbag::bagmode::Read);
} catch(rosbag::BagIOException ex) {
ROS_FATAL("Opening Bagfile %s failed: %s Quitting!", filename.c_str(), ex.what());
ros::shutdown();
return;
}
ROS_INFO("Opened Bagfile %s", filename.c_str());
Q_EMIT setGUIStatus(QString("Opened ")+filename.c_str());
// Image topics to load for bagfiles
std::vector<std::string> topics;
topics.push_back(points_tpc);
topics.push_back(tf_tpc);
rosbag::View view(bag, rosbag::TopicQuery(topics));
// Simulate sending of the messages in the bagfile
std::deque<sensor_msgs::PointCloud2::ConstPtr> pointclouds;
ros::Time last_tf(0);
BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
if(!ros::ok()) return;
while(pause_) {
usleep(100);
ROS_INFO_THROTTLE(5.0,"Paused - press Space to unpause");
if(!ros::ok()) return;
}
ROS_INFO_STREAM("Processing message on topic " << m.getTopic());
if (m.getTopic() == points_tpc || ("/" + m.getTopic() == points_tpc))
{
sensor_msgs::PointCloud2::ConstPtr pointcloud = m.instantiate<sensor_msgs::PointCloud2>();
//if (cam_info) cam_info_sub_->newMessage(cam_info);
if (pointcloud) pointclouds.push_back(pointcloud);
ROS_INFO("Found Message of %s", points_tpc.c_str());
}
if (m.getTopic() == tf_tpc|| ("/" + m.getTopic() == tf_tpc)){
tf::tfMessage::ConstPtr tf_msg = m.instantiate<tf::tfMessage>();
if (tf_msg) {
//if(tf_msg->transforms[0].header.frame_id == "/kinect") continue;//avoid destroying tf tree if odom is used
//prevents missing callerid warning
boost::shared_ptr<std::map<std::string, std::string> > msg_header_map = tf_msg->__connection_header;
(*msg_header_map)["callerid"] = "rgbdslam";
tf_pub_.publish(tf_msg);
ROS_INFO("Found Message of %s", tf_tpc.c_str());
last_tf = tf_msg->transforms[0].header.stamp;
last_tf -= ros::Duration(0.3);
}
}
while(!pointclouds.empty() && pointclouds.front()->header.stamp < last_tf){
Q_EMIT setGUIInfo(QString("Processing frame ") + QString::number(data_id_));
callback(pointclouds.front());
pointclouds.pop_front();
}
}
bag.close();
}
ROS_INFO("Bagfile fully processed");
Q_EMIT setGUIStatus(QString("Done with ")+filename.c_str());
Q_EMIT bagFinished();
}
