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(); }