예제 #1
0
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);
      }
  }
예제 #2
0
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();
  }
예제 #3
0
/**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();
  }