/** On program startup:
* Create
* - a Qt Application
* - an Object representing the ROS Node and its callback loop,
* - an OpenNIListener, setting up subscribers and callbacks for various formats of RGBD data
* - a GraphManager, getting Nodes constructed from the RGBD data
* - A Class providing a service call interface for ROS
* - If applicable also a GUI
* - let the above communicate internally via QT Signals, where communcication needs to be across threads or if the communication is conditional on the ROS node's parameterization.
*/
int main(int argc, char** argv)
{
setlocale(LC_NUMERIC,"C");//Avoid expecting german decimal separators in launch files
//create thread object, to run the ros event processing loop in parallel to the qt loop
QtROS qtRos(argc, argv, "rgbdslam"); //ros node name & namespace
//Depending an use_gui on the Parameter Server, a gui- or a headless application is used
QApplication app(argc, argv, ParameterServer::instance()->get<bool>("use_gui"));
GraphManager graph_mgr;
//Instantiate the kinect image listener
OpenNIListener listener(&graph_mgr);
std::string bagfile_name = ParameterServer::instance()->get<std::string>("bagfile_name");
if(!bagfile_name.empty())
{
QObject::connect(&listener, SIGNAL(bagFinished()), &app, SLOT(quit()));
QObject::connect(&listener, SIGNAL(bagFinished()), &qtRos, SLOT(quitNow()));
QtConcurrent::run(&listener, &OpenNIListener::loadBag, bagfile_name);
}
Graphical_UI* gui = NULL;
if (app.type() == QApplication::GuiClient){
gui = new Graphical_UI();
gui->show();
gui_connections(gui, &graph_mgr, &listener);
ui_connections(gui, &graph_mgr, &listener);//common connections for the user interfaces
} else {
ROS_WARN("Running without graphical user interface! See README or wiki page for how to interact with RGBDSLAM.");
}
//Create Ros service interface with or without gui
RosUi ui("rgbdslam"); //ui namespace for service calls
ui_connections(&ui, &graph_mgr, &listener);//common connections for the user interfaces
//If one thread receives a exit signal from the user, signal the other thread to quit too
QObject::connect(&app, SIGNAL(aboutToQuit()), &qtRos, SLOT(quitNow()));
QObject::connect(&qtRos, SIGNAL(rosQuits()), &app, SLOT(quit()));
qtRos.start();// Run main loop.
app.exec();
//if(ros::ok()) ros::shutdown();//If not yet done through the qt connection
//ros::waitForShutdown(); //not sure if necessary.
}
/**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();
}
