예제 #1
0
파일: main.cpp 프로젝트: CURG/rgbdslam
///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)));
}
예제 #2
0
void GraphManager::sendAllCloudsImpl()
{
  ScopedTimer s(__FUNCTION__);

  if (batch_cloud_pub_.getNumSubscribers() == 0){
    ROS_WARN("No Subscribers: Sending of clouds cancelled");
    return;
  }

  ROS_INFO("Sending out all clouds");
  batch_processing_runs_ = true;
  ros::Rate r(ParameterServer::instance()->get<double>("send_clouds_rate")); //slow down a bit, to allow for transmitting to and processing in other nodes

  for (graph_it it = graph_.begin(); it != graph_.end(); ++it)
  {

    Node* node = it->second;
    if(!node->valid_tf_estimate_) {
      ROS_INFO("Skipping node %i: No valid estimate", node->id_);
      continue;
    }
    tf::StampedTransform base_to_fixed = this->computeFixedToBaseTransform(node, true);
    br_.sendTransform(base_to_fixed);
    publishCloud(node, base_to_fixed.stamp_, batch_cloud_pub_);

    QString message;
    Q_EMIT setGUIInfo(message.sprintf("Sending pointcloud and map transform (%i/%i) on topics %s and /tf", it->first, (int)graph_.size(), ParameterServer::instance()->get<std::string>("individual_cloud_out_topic").c_str()) );
    r.sleep();
  }

  batch_processing_runs_ = false;
  Q_EMIT sendFinished();
}
예제 #3
0
void GraphManager::deleteLastFrame(){
    if(graph_.size() <= 1) {
      ROS_INFO("Resetting, as the only node is to be deleted");
      reset_request_ = true;
      Q_EMIT deleteLastNode();
      return;
    }

    deleteCameraFrame(graph_.size()-1);

    Q_EMIT deleteLastNode();
    optimizeGraph();//s.t. the effect of the removed edge transforms are removed to
    ROS_INFO("Removed most recent node");
    Q_EMIT setGUIInfo("Removed most recent node");
    //Q_EMIT setGraphEdges(getGraphEdges());
    //updateTransforms needs to be last, as it triggers a redraw
    //Q_EMIT updateTransforms(getAllPosesAsMatrixList());
}
예제 #4
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 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();
}