Beispiel #1
0
void OpenNIListener::setupSubscribers(){
  ParameterServer* ps = ParameterServer::instance();
  int q = ps->get<int>("subscriber_queue_size");
  ros::NodeHandle nh;
  tflistener_ = new tf::TransformListener(nh);
  std::string bagfile_name = ps->get<std::string>("bagfile_name");
  if(bagfile_name.empty()){
    std::string visua_tpc = ps->get<std::string>("topic_image_mono");
    std::string depth_tpc = ps->get<std::string>("topic_image_depth");
    std::string cinfo_tpc = ps->get<std::string>("camera_info_topic");
    std::string cloud_tpc = ps->get<std::string>("topic_points");
    std::string widev_tpc = ps->get<std::string>("wide_topic");
    std::string widec_tpc = ps->get<std::string>("wide_cloud_topic");

    //All information from Kinect
    if(!visua_tpc.empty() && !depth_tpc.empty() && !cloud_tpc.empty())
    {   
        visua_sub_ = new image_sub_type(nh, visua_tpc, q);
        depth_sub_ = new image_sub_type (nh, depth_tpc, q);
        cloud_sub_ = new pc_sub_type (nh, cloud_tpc, q);  
        kinect_sync_ = new message_filters::Synchronizer<KinectSyncPolicy>(KinectSyncPolicy(q),  *visua_sub_, *depth_sub_, *cloud_sub_),
        kinect_sync_->registerCallback(boost::bind(&OpenNIListener::kinectCallback, this, _1, _2, _3));
        ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to " << visua_tpc << ", " << depth_tpc << " and " << cloud_tpc);
    } 
    //No cloud, but visual image and depth
    else if(!visua_tpc.empty() && !depth_tpc.empty() && !cinfo_tpc.empty() && cloud_tpc.empty())
    {   
        visua_sub_ = new image_sub_type(nh, visua_tpc, q);
        depth_sub_ = new image_sub_type(nh, depth_tpc, q);
        cinfo_sub_ = new cinfo_sub_type(nh, cinfo_tpc, q);
        no_cloud_sync_ = new message_filters::Synchronizer<NoCloudSyncPolicy>(NoCloudSyncPolicy(q),  *visua_sub_, *depth_sub_, *cinfo_sub_);
        no_cloud_sync_->registerCallback(boost::bind(&OpenNIListener::noCloudCallback, this, _1, _2, _3));
        ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to " << visua_tpc << " and " << depth_tpc);
    } 

    //All information from stereo                                               
    if(!widec_tpc.empty() && !widev_tpc.empty())
    {   
      visua_sub_ = new image_sub_type(nh, widev_tpc, q);
      cloud_sub_ = new pc_sub_type(nh, widec_tpc, q);
      stereo_sync_ = new message_filters::Synchronizer<StereoSyncPolicy>(StereoSyncPolicy(q), *visua_sub_, *cloud_sub_);
      stereo_sync_->registerCallback(boost::bind(&OpenNIListener::stereoCallback, this, _1, _2));
      ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to " << widev_tpc << " and " << widec_tpc );
    } 
    if(ps->get<bool>("use_robot_odom")){
    	odom_sub_= new odom_sub_type(nh, ps->get<std::string>("odometry_tpc"), 3);
      ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to odometry on " << ps->get<std::string>("odometry_tpc"));
    	odom_sub_->registerCallback(boost::bind(&OpenNIListener::odomCallback,this,_1));
    }
  } 
  else {
    ROS_WARN("RGBDSLAM loads a bagfile - therefore doesn't subscribe to topics.");
  }
}
void visual_slam::OpenNIListener::setupsubscribers(){
  int q;
  _node.getParam("/queue_size", q);
  std::string bagfile_name;
  _node.getParam("/bagfile_name", bagfile_name);
  if(bagfile_name.empty()){

    std::string visua_tpc, depth_tpc, cinfo_tpc, cloud_tpc;
    _node.getParam("topic_image_mono", visua_tpc);
    _node.getParam("topic_image_depth", depth_tpc);
    _node.getParam("camera_info_topic", cinfo_tpc);
    _node.getParam("topic_points", cloud_tpc);

    //All information from Kinect
    if(!visua_tpc.empty() && !depth_tpc.empty() && !cloud_tpc.empty())
    {
      visua_sub_ = new image_sub_type(_node, visua_tpc, q);
      depth_sub_ = new image_sub_type (_node, depth_tpc, q);
      cloud_sub_ = new pc_sub_type (_node, cloud_tpc, q);
      kinect_sync_ = new message_filters::Synchronizer<KinectSyncPolicy>(KinectSyncPolicy(q),  *visua_sub_, *depth_sub_, *cloud_sub_),
      kinect_sync_->registerCallback(boost::bind(&visual_slam::OpenNIListener::kinectCallback, this, _1, _2, _3));
    }
    //No cloud, but visual image and depth
    else if(!visua_tpc.empty() && !depth_tpc.empty() && !cinfo_tpc.empty() && cloud_tpc.empty())
    {
      visua_sub_ = new image_sub_type(_node, visua_tpc, q);
      depth_sub_ = new image_sub_type(_node, depth_tpc, q);
      cinfo_sub_ = new cinfo_sub_type(_node, cinfo_tpc, q);
      no_cloud_sync_ = new message_filters::Synchronizer<NoCloudSyncPolicy>(NoCloudSyncPolicy(q),  *visua_sub_, *depth_sub_, *cinfo_sub_);
      no_cloud_sync_->registerCallback(boost::bind(&visual_slam::OpenNIListener::noCloudCallback, this, _1, _2, _3));
    }
    bool use_robot_odom;
    _node.getParam("use_robot_odom", use_robot_odom);
    if(use_robot_odom){
      std::string odometry_tpc;
      _node.getParam("odometry_tpc", odometry_tpc);
      odom_sub_= new odom_sub_type(_node,odometry_tpc , 3);
      odom_sub_->registerCallback(boost::bind(&OpenNIListener::odomCallback,this,_1));
    }
  }
  else {
    ROS_WARN("RGBDSLAM loads a bagfile - therefore doesn't subscribe to topics.");
  }
}
Beispiel #3
0
OpenNIListener::OpenNIListener(GraphManager* graph_mgr)
: graph_mgr_(graph_mgr),
  stereo_sync_(NULL), kinect_sync_(NULL), no_cloud_sync_(NULL),
  visua_sub_(NULL), depth_sub_(NULL), cloud_sub_(NULL),
  depth_mono8_img_(cv::Mat()),
  save_bag_file(false),
  pause_(ParameterServer::instance()->get<bool>("start_paused")),
  getOneFrame_(false),
  first_frame_(true),
  data_id_(0),
  num_processed_(0),
  image_encoding_("rgb8")
{
  ParameterServer* ps = ParameterServer::instance();
  int q = ps->get<int>("subscriber_queue_size");
  if(ps->get<bool>("encoding_bgr")){
    image_encoding_ = "bgr8";//used in conversion to qimage. exact value does not matter, just not rgb8
  }
  std::string bagfile_name = ps->get<std::string>("bagfile_name");
  std::string visua_tpc = ps->get<std::string>("topic_image_mono");
  std::string depth_tpc = ps->get<std::string>("topic_image_depth");
  std::string cinfo_tpc = ps->get<std::string>("camera_info_topic");
  ros::NodeHandle nh;
  tflistener_ = new tf::TransformListener(nh);
  if(bagfile_name.empty()){
    std::string cloud_tpc = ps->get<std::string>("topic_points");
    std::string widev_tpc = ps->get<std::string>("wide_topic");
    std::string widec_tpc = ps->get<std::string>("wide_cloud_topic");

    //All information from Kinect
    if(!visua_tpc.empty() && !depth_tpc.empty() && !cloud_tpc.empty())
    {   
        visua_sub_ = new image_sub_type(nh, visua_tpc, q);
        depth_sub_ = new image_sub_type (nh, depth_tpc, q);
        cloud_sub_ = new pc_sub_type (nh, cloud_tpc, q);  
        kinect_sync_ = new message_filters::Synchronizer<KinectSyncPolicy>(KinectSyncPolicy(q),  *visua_sub_, *depth_sub_, *cloud_sub_),
        kinect_sync_->registerCallback(boost::bind(&OpenNIListener::kinectCallback, this, _1, _2, _3));
        ROS_INFO_STREAM("Listening to " << visua_tpc << ", " << depth_tpc << " and " << cloud_tpc);
    } 
    //No cloud, but visual image and depth
    else if(!visua_tpc.empty() && !depth_tpc.empty() && !cinfo_tpc.empty() && cloud_tpc.empty())
    {   
        visua_sub_ = new image_sub_type(nh, visua_tpc, q);
        depth_sub_ = new image_sub_type(nh, depth_tpc, q);
        cinfo_sub_ = new cinfo_sub_type(nh, cinfo_tpc, q);
        no_cloud_sync_ = new message_filters::Synchronizer<NoCloudSyncPolicy>(NoCloudSyncPolicy(q),  *visua_sub_, *depth_sub_, *cinfo_sub_);
        no_cloud_sync_->registerCallback(boost::bind(&OpenNIListener::noCloudCallback, this, _1, _2, _3));
        ROS_INFO_STREAM("Listening to " << visua_tpc << " and " << depth_tpc);
    } 

    //All information from stereo                                               
    if(!widec_tpc.empty() && !widev_tpc.empty())
    {   
      visua_sub_ = new image_sub_type(nh, widev_tpc, q);
      cloud_sub_ = new pc_sub_type(nh, widec_tpc, q);
      stereo_sync_ = new message_filters::Synchronizer<StereoSyncPolicy>(StereoSyncPolicy(q), *visua_sub_, *cloud_sub_);
      stereo_sync_->registerCallback(boost::bind(&OpenNIListener::stereoCallback, this, _1, _2));
      ROS_INFO_STREAM("Listening to " << widev_tpc << " and " << widec_tpc );
    } 
  } 
  detector_ = cv::Ptr<cv::FeatureDetector>( createDetector(ps->get<std::string>("feature_detector_type")) );
  extractor_ = cv::Ptr<cv::DescriptorExtractor>( createDescriptorExtractor(ps->get<std::string>("feature_extractor_type")) );
}
Beispiel #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 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();
  }