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);
}
}
void OpenNIListener::processBagfile(std::string filename,
const std::string& visua_tpc,
const std::string& depth_tpc,
const std::string& points_tpc,
const std::string& cinfo_tpc,
const std::string& odom_tpc,
const std::string& tf_tpc)
{
ROS_INFO_NAMED("OpenNIListener", "Loading Bagfile %s", filename.c_str());
Q_EMIT iamBusy(4, "Loading Bagfile", 0);
{ //bag will be destructed after this block (hopefully frees memory for the optimizer)
rosbag::Bag input_bag;
try{
input_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_NAMED("OpenNIListener", "Opened Bagfile %s", filename.c_str());
std::vector<std::string> topics;
topics = createTopicsVector(visua_tpc, depth_tpc, points_tpc, cinfo_tpc, odom_tpc, tf_tpc);
rosbag::View view(input_bag, rosbag::TopicQuery(topics));
Q_EMIT iamBusy(4, "Processing Bagfile", view.size());
// Simulate sending of the messages in the bagfile
std::deque<sensor_msgs::Image::ConstPtr> vis_images;
std::deque<sensor_msgs::Image::ConstPtr> dep_images;
std::deque<sensor_msgs::CameraInfo::ConstPtr> cam_infos;
std::deque<sensor_msgs::PointCloud2::ConstPtr> pointclouds;
std::deque<nav_msgs::OdometryConstPtr> odometries;
//ros::Time last_tf=ros::Time(0);
ros::Time last_tf=ros::TIME_MIN;
bool tf_available = false;
int counter = 0;
BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
Q_EMIT progress(4, "Processing Bagfile", counter++);
do{
usleep(10);
if(!ros::ok()) return;
} while(pause_);
ROS_INFO_NAMED("OpenNIListener", "Processing %s of type %s with timestamp %f", m.getTopic().c_str(), m.getDataType().c_str(), m.getTime().toSec());
if (m.getTopic() == odom_tpc || ("/" + m.getTopic() == odom_tpc)) {
ROS_INFO_NAMED("OpenNIListener", "Processing %s of type %s with timestamp %f", m.getTopic().c_str(), m.getDataType().c_str(), m.getTime().toSec());
nav_msgs::OdometryConstPtr odommsg = m.instantiate<nav_msgs::Odometry>();
if (odommsg) odometries.push_back(odommsg);
}
else if (m.getTopic() == visua_tpc || ("/" + m.getTopic() == visua_tpc))
{
sensor_msgs::Image::ConstPtr rgb_img = m.instantiate<sensor_msgs::Image>();
if (rgb_img) vis_images.push_back(rgb_img);
ROS_DEBUG("Found Message of %s", visua_tpc.c_str());
}
else if (m.getTopic() == depth_tpc || ("/" + m.getTopic() == depth_tpc))
{
sensor_msgs::Image::ConstPtr depth_img = m.instantiate<sensor_msgs::Image>();
//if (depth_img) depth_img_sub_->newMessage(depth_img);
if (depth_img) dep_images.push_back(depth_img);
ROS_DEBUG("Found Message of %s", depth_tpc.c_str());
}
else 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_DEBUG("Found Message of %s", points_tpc.c_str());
}
else if (m.getTopic() == cinfo_tpc || ("/" + m.getTopic() == cinfo_tpc))
{
sensor_msgs::CameraInfo::ConstPtr cam_info = m.instantiate<sensor_msgs::CameraInfo>();
//if (cam_info) cam_info_sub_->newMessage(cam_info);
if (cam_info) cam_infos.push_back(cam_info);
ROS_DEBUG("Found Message of %s", cinfo_tpc.c_str());
}
else if (m.getTopic() == tf_tpc|| ("/" + m.getTopic() == tf_tpc)){
tf::tfMessage::ConstPtr tf_msg = m.instantiate<tf::tfMessage>();
if (tf_msg) {
tf_available = true;
addTFMessageDirectlyToTransformer(tf_msg, tflistener_);
last_tf = tf_msg->transforms[0].header.stamp;
last_tf -= ros::Duration(0.1);
}
}
if (last_tf == ros::TIME_MIN){//If not a valid time yet, set to something before first message's stamp
last_tf = m.getTime();
last_tf -= ros::Duration(0.1);
}
//last_tf = m.getTime();//FIXME: No TF -> no processing
while(!odometries.empty() && odometries.front()->header.stamp < last_tf){
ROS_INFO_NAMED("OpenNIListener", "Sending Odometry message");
odomCallback(odometries.front());
odometries.pop_front();
}
while(!vis_images.empty() && vis_images.front()->header.stamp < last_tf){
ROS_INFO_NAMED("OpenNIListener", "Forwarding buffered visual message from time %12f", vis_images.front()->header.stamp.toSec());
rgb_img_sub_->newMessage(vis_images.front());
vis_images.pop_front();
}
while(!dep_images.empty() && dep_images.front()->header.stamp < last_tf){
ROS_INFO_NAMED("OpenNIListener", "Forwarding buffered depth message from time %12f", dep_images.front()->header.stamp.toSec());
depth_img_sub_->newMessage(dep_images.front());
dep_images.pop_front();
}
while(!cam_infos.empty() && cam_infos.front()->header.stamp < last_tf){
ROS_INFO_NAMED("OpenNIListener", "Forwarding buffered cam info message from time %12f", cam_infos.front()->header.stamp.toSec());
cam_info_sub_->newMessage(cam_infos.front());
cam_infos.pop_front();
}
while(!pointclouds.empty() && pointclouds.front()->header.stamp < last_tf){
pc_sub_->newMessage(pointclouds.front());
pointclouds.pop_front();
}
}
ROS_WARN_NAMED("eval", "Finished processing of Bagfile");
input_bag.close();
}
/**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 OpenNIListener::loadBag(std::string filename)
{
ScopedTimer s(__FUNCTION__);
ros::NodeHandle nh;
ParameterServer* ps = ParameterServer::instance();
std::string visua_tpc = ps->get<std::string>("topic_image_mono");
std::string depth_tpc = ps->get<std::string>("topic_image_depth");
std::string points_tpc = ps->get<std::string>("topic_points");
std::string cinfo_tpc = ps->get<std::string>("camera_info_topic");
int q = ps->get<int>("subscriber_queue_size");
std::string tf_tpc = std::string("/tf");
tf_pub_ = nh.advertise<tf::tfMessage>(tf_tpc, 10);
//All information from Kinect
if(!visua_tpc.empty() && !depth_tpc.empty() && !cinfo_tpc.empty() && points_tpc.empty())
{
// Set up fake subscribers to capture images
depth_img_sub_ = new BagSubscriber<sensor_msgs::Image>();
rgb_img_sub_ = new BagSubscriber<sensor_msgs::Image>();
cam_info_sub_ = new BagSubscriber<sensor_msgs::CameraInfo>();
no_cloud_sync_ = new message_filters::Synchronizer<NoCloudSyncPolicy>(NoCloudSyncPolicy(q), *rgb_img_sub_, *depth_img_sub_, *cam_info_sub_);
no_cloud_sync_->registerCallback(boost::bind(&OpenNIListener::noCloudCallback, this, _1, _2, _3));
ROS_INFO_STREAM("Listening to " << visua_tpc << ", " << depth_tpc << " and " << cinfo_tpc);
}
else if(!visua_tpc.empty() && !depth_tpc.empty() && !points_tpc.empty())
{
// Set up fake subscribers to capture images
depth_img_sub_ = new BagSubscriber<sensor_msgs::Image>();
rgb_img_sub_ = new BagSubscriber<sensor_msgs::Image>();
pc_sub_ = new BagSubscriber<sensor_msgs::PointCloud2>();
kinect_sync_ = new message_filters::Synchronizer<KinectSyncPolicy>(KinectSyncPolicy(q), *rgb_img_sub_, *depth_img_sub_, *pc_sub_);
kinect_sync_->registerCallback(boost::bind(&OpenNIListener::kinectCallback, this, _1, _2, _3));
ROS_INFO_STREAM("Listening to " << visua_tpc << ", " << depth_tpc << " and " << points_tpc);
}
else {
ROS_ERROR("Missing required information: Topic names.");
ROS_ERROR_STREAM("Visual: " << visua_tpc);
ROS_ERROR_STREAM("Camera Info (Optional if Points given): " << cinfo_tpc);
ROS_ERROR_STREAM("Depth: " << depth_tpc);
ROS_ERROR_STREAM("Points (Optional if Cam Info given): " << points_tpc);
}
bool eval_landmarks = ps->get<bool>("optimize_landmarks");
ps->set<bool>("optimize_landmarks", false);
ROS_INFO("Loading Bagfile %s", filename.c_str());
Q_EMIT iamBusy(4, "Loading Bagfile", 0);
{ //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());
// Image topics to load for bagfiles
std::vector<std::string> topics;
topics.push_back(visua_tpc);
topics.push_back(depth_tpc);
if(points_tpc.empty()){
topics.push_back(cinfo_tpc);
} else {
topics.push_back(points_tpc);
}
topics.push_back(tf_tpc);
rosbag::View view(bag, rosbag::TopicQuery(topics));
Q_EMIT iamBusy(4, "Processing Bagfile", view.size());
// Simulate sending of the messages in the bagfile
std::deque<sensor_msgs::Image::ConstPtr> vis_images;
std::deque<sensor_msgs::Image::ConstPtr> dep_images;
std::deque<sensor_msgs::CameraInfo::ConstPtr> cam_infos;
std::deque<sensor_msgs::PointCloud2::ConstPtr> pointclouds;
//ros::Time last_tf=ros::Time(0);
ros::Time last_tf=ros::Time::now();
int counter = 0;
BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
Q_EMIT progress(4, "Processing Bagfile", counter++);
do{
usleep(150);
if(!ros::ok()) return;
} while(pause_);
if (m.getTopic() == visua_tpc || ("/" + m.getTopic() == visua_tpc))
{
sensor_msgs::Image::ConstPtr rgb_img = m.instantiate<sensor_msgs::Image>();
if (rgb_img) vis_images.push_back(rgb_img);
ROS_DEBUG("Found Message of %s", visua_tpc.c_str());
}
if (m.getTopic() == depth_tpc || ("/" + m.getTopic() == depth_tpc))
{
sensor_msgs::Image::ConstPtr depth_img = m.instantiate<sensor_msgs::Image>();
//if (depth_img) depth_img_sub_->newMessage(depth_img);
if (depth_img) dep_images.push_back(depth_img);
ROS_DEBUG("Found Message of %s", depth_tpc.c_str());
}
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_DEBUG("Found Message of %s", points_tpc.c_str());
}
if (m.getTopic() == cinfo_tpc || ("/" + m.getTopic() == cinfo_tpc))
{
sensor_msgs::CameraInfo::ConstPtr cam_info = m.instantiate<sensor_msgs::CameraInfo>();
//if (cam_info) cam_info_sub_->newMessage(cam_info);
if (cam_info) cam_infos.push_back(cam_info);
ROS_DEBUG("Found Message of %s", cinfo_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 ROS_VERSION_MINIMUM(1,11,0)
//use MessageEvent for indigo
#else
//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_DEBUG("Found Message of %s", tf_tpc.c_str());
last_tf = tf_msg->transforms[0].header.stamp;
last_tf -= ros::Duration(1.0);
#endif
}
}
while(!vis_images.empty() && vis_images.front()->header.stamp < last_tf){
rgb_img_sub_->newMessage(vis_images.front());
vis_images.pop_front();
}
while(!dep_images.empty() && dep_images.front()->header.stamp < last_tf){
depth_img_sub_->newMessage(dep_images.front());
dep_images.pop_front();
}
while(!cam_infos.empty() && cam_infos.front()->header.stamp < last_tf){
cam_info_sub_->newMessage(cam_infos.front());
cam_infos.pop_front();
}
while(!pointclouds.empty() && pointclouds.front()->header.stamp < last_tf){
pc_sub_->newMessage(pointclouds.front());
pointclouds.pop_front();
}
}
ROS_WARN_NAMED("eval", "Finished processing of Bagfile");
bag.close();
}
