///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::saveOctomapImpl(QString filename)
{
ScopedTimer s(__FUNCTION__);
batch_processing_runs_ = true;
Q_EMIT iamBusy(0, "Saving Octomap", 0);
std::list<Node*> nodes_for_octomapping;
unsigned int points_to_render = 0;
{ //Get the transformations from the optimizer and store them in the node's point cloud header
QMutexLocker locker(&optimizer_mutex_);
for (graph_it it = graph_.begin(); it != graph_.end(); ++it){
Node* node = it->second;
if(this->updateCloudOrigin(node)){
nodes_for_octomapping.push_back(node);
points_to_render += node->pc_col->size();
}
}
}
// Now (this takes long) render the clouds into the octomap
int counter = 0;
co_server_.reset();
Q_EMIT iamBusy(0, "Saving Octomap", nodes_for_octomapping.size());
unsigned int rendered_points = 0;
BOOST_FOREACH(Node* node, nodes_for_octomapping)
{
QString message;
Q_EMIT setGUIStatus(message.sprintf("Inserting Node %i/%i into octomap", ++counter, (int)nodes_for_octomapping.size()));
this->renderToOctomap(node);
rendered_points += node->pc_col->size();
ROS_INFO("Rendered %u points of %u", rendered_points, points_to_render);
Q_EMIT progress(0, "Saving Octomap", counter);
if(counter % ParameterServer::instance()->get<int>("octomap_autosave_step") == 0){
Q_EMIT setGUIStatus(QString("Autosaving preliminary octomap to ") + filename);
this->writeOctomap(filename);
}
}
/**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();
}