C++ SubscriberFilter::subscribe方法代码示例

  StereoVslamNode(const std::string& vocab_tree_file, const std::string& vocab_weights_file,
                  const std::string& calonder_trees_file)
    : it_(nh_), sync_(3),
      vslam_system_(vocab_tree_file, vocab_weights_file),
      detector_(new vslam_system::AnyDetector)
  {
    // Use calonder descriptor
    typedef cv::CalonderDescriptorExtractor<float> Calonder;
    vslam_system_.frame_processor_.setFrameDescriptor(new Calonder(calonder_trees_file));
    
    // Advertise outputs
    cam_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("/vslam/cameras", 1);
    point_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("/vslam/points", 1);
    vo_tracks_pub_ = it_.advertiseCamera("/vslam/vo_tracks/image", 1);
    odom_pub_ = nh_.advertise<nav_msgs::Odometry>("/vo", 1);
    pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/vslam/pointcloud", 1);

    // Synchronize inputs
    l_image_sub_.subscribe(it_, "left/image_rect", 1);
    l_info_sub_ .subscribe(nh_, "left/camera_info", 1);
    r_image_sub_.subscribe(it_, "right/image_rect", 1);
    r_info_sub_ .subscribe(nh_, "right/camera_info", 1);
    sync_.connectInput(l_image_sub_, l_info_sub_, r_image_sub_, r_info_sub_);
    sync_.registerCallback( boost::bind(&StereoVslamNode::imageCb, this, _1, _2, _3, _4) );

    // Dynamic reconfigure for parameters
    ReconfigureServer::CallbackType f = boost::bind(&StereoVslamNode::configCb, this, _1, _2);
    reconfigure_server_.setCallback(f);

  }

  /**
   * Constructor, subscribes to input topics using image transport and registers
   * callbacks.
   * \param transport The image transport to use
   */
  StereoProcessor(const std::string& transport) :
    left_received_(0), right_received_(0), left_info_received_(0), right_info_received_(0), all_received_(0)
  {
    // Read local parameters
    ros::NodeHandle local_nh("~");

    // Resolve topic names
    ros::NodeHandle nh;
    std::string stereo_ns = nh.resolveName("stereo");
    std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image"));
    std::string right_topic = ros::names::clean(stereo_ns + "/right/" + nh.resolveName("image"));

    std::string left_info_topic = stereo_ns + "/left/camera_info";
    std::string right_info_topic = stereo_ns + "/right/camera_info";

    // Subscribe to four input topics.
    ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s\n\t* %s", 
        left_topic.c_str(), right_topic.c_str(),
        left_info_topic.c_str(), right_info_topic.c_str());

    image_transport::ImageTransport it(nh);
    left_sub_.subscribe(it, left_topic, 1, transport);
    right_sub_.subscribe(it, right_topic, 1, transport);
    left_info_sub_.subscribe(nh, left_info_topic, 1);
    right_info_sub_.subscribe(nh, right_info_topic, 1);

    // Complain every 15s if the topics appear unsynchronized
    left_sub_.registerCallback(boost::bind(StereoProcessor::increment, &left_received_));
    right_sub_.registerCallback(boost::bind(StereoProcessor::increment, &right_received_));
    left_info_sub_.registerCallback(boost::bind(StereoProcessor::increment, &left_info_received_));
    right_info_sub_.registerCallback(boost::bind(StereoProcessor::increment, &right_info_received_));
    check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0),
                                             boost::bind(&StereoProcessor::checkInputsSynchronized, this));

    // Synchronize input topics. Optionally do approximate synchronization.
    local_nh.param("queue_size", queue_size_, 5);
    bool approx;
    local_nh.param("approximate_sync", approx, false);
    if (approx)
    {
      approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(queue_size_),
                                                  left_sub_, right_sub_, left_info_sub_, right_info_sub_) );
      approximate_sync_->registerCallback(boost::bind(&StereoProcessor::dataCb, this, _1, _2, _3, _4));
    }
    else
    {
      exact_sync_.reset(new ExactSync(ExactPolicy(queue_size_),
                                      left_sub_, right_sub_, left_info_sub_, right_info_sub_) );
      exact_sync_->registerCallback(boost::bind(&StereoProcessor::dataCb, this, _1, _2, _3, _4));
    }
  }

  /**
   * Constructor, subscribes to input topics using image transport and registers
   * callbacks.
   * \param transport The image transport to use
   */
  MonoDepthProcessor(const std::string& transport) :
    image_received_(0), depth_received_(0), image_info_received_(0), depth_info_received_(0), all_received_(0)
  {
    // Read local parameters
    ros::NodeHandle local_nh("~");

    // Resolve topic names
    ros::NodeHandle nh;
    std::string camera_ns = nh.resolveName("camera");
    std::string image_topic = ros::names::clean(camera_ns + "/rgb/image_rect");
    std::string depth_topic = ros::names::clean(camera_ns + "/depth_registered/image_rect");

    std::string image_info_topic = camera_ns + "/rgb/camera_info";
    std::string depth_info_topic = camera_ns + "/depth_registered/camera_info";

    // Subscribe to four input topics.
    ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s\n\t* %s", 
        image_topic.c_str(), depth_topic.c_str(),
        image_info_topic.c_str(), depth_info_topic.c_str());

    image_transport::ImageTransport it(nh);
    image_sub_.subscribe(it, image_topic, 1, transport);
    depth_sub_.subscribe(it, depth_topic, 1, transport);
    image_info_sub_.subscribe(nh, image_info_topic, 1);
    depth_info_sub_.subscribe(nh, depth_info_topic, 1);

    // Complain every 15s if the topics appear unsynchronized
    image_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &image_received_));
    depth_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &depth_received_));
    image_info_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &image_info_received_));
    depth_info_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &depth_info_received_));
    check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0),
                                             boost::bind(&MonoDepthProcessor::checkInputsSynchronized, this));

    // Synchronize input topics. Optionally do approximate synchronization.
    local_nh.param("queue_size", queue_size_, 5);
    bool approx;
    local_nh.param("approximate_sync", approx, true);
    if (approx)
    {
      approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(queue_size_),
                                                  image_sub_, depth_sub_, image_info_sub_, depth_info_sub_) );
      approximate_sync_->registerCallback(boost::bind(&MonoDepthProcessor::dataCb, this, _1, _2, _3, _4));
    }
    else
    {
      exact_sync_.reset(new ExactSync(ExactPolicy(queue_size_),
                                      image_sub_, depth_sub_, image_info_sub_, depth_info_sub_) );
      exact_sync_->registerCallback(boost::bind(&MonoDepthProcessor::dataCb, this, _1, _2, _3, _4));
    }
  }

        // Constructor
        PcPublisher()
        	: image_transport_(n_),
        	tof_sync_(SyncPolicy(3)),
        	c_xyz_image_32F3_(0),
           	c_grey_image_32F1_(0)
           	
        {
			topicPub_pointCloud_ = n_.advertise<sensor_msgs::PointCloud>("point_cloud", 1);
			xyz_image_subscriber_.subscribe(image_transport_, "image_xyz", 1);
			grey_image_subscriber_.subscribe(image_transport_,"image_grey", 1);

			tof_sync_.connectInput(xyz_image_subscriber_, grey_image_subscriber_);
			tof_sync_.registerCallback(boost::bind(&PcPublisher::syncCallback, this, _1, _2));

           // topicSub_xyzImage_ = n_.subscribe("xyz_image", 1, &PcPublisher::topicCallback_xyzImage, this);
        }

    /////////////////////////////////////////////////////////////////
    // Constructor
    PedestrianDetectorHOG(ros::NodeHandle nh): 
      nh_(nh), 
      local_nh_("~"), 
      it_(nh_), 
      stereo_sync_(4),
      //      cam_model_(NULL), 
      counter(0) 
    {
      
      hog_.setSVMDetector(cv::HOGDescriptor::getDefaultPeopleDetector());
      
      // Get parameters from the server
      local_nh_.param("hit_threshold",hit_threshold_,0.0);
      local_nh_.param("group_threshold",group_threshold_,2);
      local_nh_.param("use_depth", use_depth_, true);
      local_nh_.param("use_height",use_height_,true);
      local_nh_.param("max_height_m",max_height_m_,2.2);
      local_nh_.param("ground_frame",ground_frame_,std::string("base_link"));
      local_nh_.param("do_display", do_display_, true);
       
      // Advertise a 3d position measurement for each head.
      people_pub_ = nh_.advertise<people_msgs::PositionMeasurement>("people_tracker_measurements",1);

      // Subscribe to tf & the images  
      if (use_depth_) {  

	tf_.setExtrapolationLimit(ros::Duration().fromSec(0.01));

	std::string left_topic = nh_.resolveName("stereo") + "/left/image";
	std::string disp_topic = nh_.resolveName("stereo") + "/disparity";
	std::string left_info_topic = nh_.resolveName("stereo") + "/left/camera_info";
	std::string right_info_topic = nh_.resolveName("stereo") + "/right/camera_info";
	left_sub_.subscribe(it_, left_topic, 10);
	disp_sub_.subscribe(it_, disp_topic, 10);
	left_info_sub_.subscribe(nh_, left_info_topic, 10);
	right_info_sub_.subscribe(nh_, right_info_topic, 10);
	stereo_sync_.connectInput(left_sub_, left_info_sub_, disp_sub_, right_info_sub_);
	stereo_sync_.registerCallback(boost::bind(&PedestrianDetectorHOG::imageCBAll, this, _1, _2, _3, _4));
      }
      else {
	std::string topic = nh_.resolveName("image");
	image_sub_ = it_.subscribe("image",10,&PedestrianDetectorHOG::imageCB,this);
      }

    }

 void connectCb()
 {
   num_subs++;
   if(num_subs > 0)
   {
     color_image_sub_.subscribe(image_transport_,"image_color", 1);
     pc_sub_.subscribe(n_, "point_cloud2", 1);
   }
 }

// Connection callback that unsubscribes from the tracker if no one is subscribed.
void connectCallback(message_filters::Subscriber<CameraInfo> &sub_cam,
                     message_filters::Subscriber<GroundPlane> &sub_gp,
                     image_transport::SubscriberFilter &sub_col,
                     image_transport::SubscriberFilter &sub_dep,
                     image_transport::ImageTransport &it) {
    if(!pub_message.getNumSubscribers() && !pub_result_image.getNumSubscribers() && !pub_centres.getNumSubscribers() && !pub_detected_persons.getNumSubscribers()) {
        ROS_DEBUG("Upper Body Detector: No subscribers. Unsubscribing.");
        sub_cam.unsubscribe();
        sub_gp.unsubscribe();
        sub_col.unsubscribe();
        sub_dep.unsubscribe();
    } else {
        ROS_DEBUG("Upper Body Detector: New subscribers. Subscribing.");
        sub_cam.subscribe();
        sub_gp.subscribe();
        sub_col.subscribe(it,sub_col.getTopic().c_str(),1);
        sub_dep.subscribe(it,sub_dep.getTopic().c_str(),1);
    }
}

  void initNode()
  {
    pc_sync_ = boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> >(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(3)));
    pc_pub_ = n_.advertise<sensor_msgs::PointCloud2>("colored_point_cloud2", 1);
    color_image_sub_.subscribe(image_transport_,"image_color", 1);
    pc_sub_.subscribe(n_, "point_cloud2", 1);

    pc_sync_->connectInput(color_image_sub_, pc_sub_);
    pc_sync_->registerCallback(boost::bind(&CreateColoredPointCloud::syncCallback, this, _1, _2));
  }

 unsigned long init()
 {
   color_camera_image_sub_.subscribe(*it_, "colorimage", 1);
   point_cloud_sub_.subscribe(node_handle_, "pointcloud", 1);
   
   sync_pointcloud_->connectInput(point_cloud_sub_, color_camera_image_sub_);
   sync_pointcloud_callback_connection_ = sync_pointcloud_->registerCallback(boost::bind(&CobKinectImageFlipNodelet::inputCallback, this, _1, _2));
   
   return ipa_Utils::RET_OK;
 }

  StereoSynchronizer () : nh_(), it_(nh_), sync_(15) {

    std::string left_raw = nh_.resolveName("left_raw");
    std::string right_raw = nh_.resolveName("right_raw");
    image_sub_l_.subscribe(it_, left_raw  + "/image_raw", 4);
    info_sub_l_ .subscribe(nh_, left_raw  + "/camera_info", 4);
    image_sub_r_.subscribe(it_, right_raw + "/image_raw", 4);
    info_sub_r_ .subscribe(nh_, right_raw + "/camera_info", 4);

    left_ns_ = nh_.resolveName("left");
    right_ns_ = nh_.resolveName("right");
    ros::NodeHandle cam_l_nh(left_ns_);
    ros::NodeHandle cam_r_nh(right_ns_);
    it_l_ = new image_transport::ImageTransport(cam_l_nh);
    it_r_ = new image_transport::ImageTransport(cam_r_nh);
    pub_left_ = it_l_->advertiseCamera("image_raw", 1);
    pub_right_ = it_r_->advertiseCamera("image_raw", 1);

    sync_.connectInput(image_sub_l_, info_sub_l_, image_sub_r_, info_sub_r_);
    sync_.registerCallback(boost::bind(&StereoSynchronizer::imageCB, this, _1, _2, _3, _4));
  }

// Handles (un)subscribing when clients (un)subscribe
void DisparityWideNodelet::connectCb()
{
  boost::lock_guard<boost::mutex> lock(connect_mutex_);
  if (pub_disparity_.getNumSubscribers() == 0)
  {
    sub_l_image_.unsubscribe();
    sub_l_info_ .unsubscribe();
    sub_r_image_.unsubscribe();
    sub_r_info_ .unsubscribe();
  }
  else if (!sub_l_image_.getSubscriber())
  {
    ros::NodeHandle &nh = getNodeHandle();
    // Queue size 1 should be OK; the one that matters is the synchronizer queue size.
    /// @todo Allow remapping left, right?
    image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
    sub_l_image_.subscribe(*it_, "left_wide/image_rect", 1, hints);
    sub_l_info_ .subscribe(nh,   "left_wide/camera_info", 1);
    sub_r_image_.subscribe(*it_, "right/image_rect", 1, hints);
    sub_r_info_ .subscribe(nh,   "right/camera_info", 1);
  }
}

	/// Subscribe to camera topics if not already done.
	void connectCallback()
	{
		if (sub_counter_ == 0) 
		{
			sub_counter_++;
			ROS_DEBUG("[all_camera_viewer] Subscribing to camera topics");

			if (use_right_color_camera_)
			{
				right_color_camera_image_sub_.subscribe(image_transport_, "right/image_color", 1);
				right_camera_info_sub_.subscribe(node_handle_, "right/camera_info", 1);
			}
			if (use_left_color_camera_)
			{
				left_color_camera_image_sub_.subscribe(image_transport_, "left/image_color", 1);
				left_camera_info_sub_.subscribe(node_handle_, "left/camera_info", 1);
			}
			if (use_tof_camera_)
			{
				tof_camera_grey_image_sub_.subscribe(image_transport_, "image_grey", 1);
			}
		}
	}

    /// Subscribe to camera topics if not already done.
    void connectCallback()
    {
        ROS_INFO("[fiducials] Subscribing to camera topics");

        color_camera_image_sub_.subscribe(*image_transport_0_, "image_color", 1);
        color_camera_info_sub_.subscribe(node_handle_, "camera_info", 1);

        color_image_sub_sync_ = boost::shared_ptr<message_filters::Synchronizer<ColorImageSyncPolicy> >(new message_filters::Synchronizer<ColorImageSyncPolicy>(ColorImageSyncPolicy(3)));
        color_image_sub_sync_->connectInput(color_camera_image_sub_, color_camera_info_sub_);
        color_image_sub_sync_->registerCallback(boost::bind(&CobFiducialsNode::colorImageCallback, this, _1, _2));

        sub_counter_++;
        ROS_INFO("[fiducials] %i subscribers on camera topics [OK]", sub_counter_);
    }

// Connection callback that unsubscribes from the tracker if no one is subscribed.
void connectCallback(ros::Subscriber &sub_msg,
                     ros::NodeHandle &n,
                     string gp_topic,
                     string img_topic,
                     Subscriber<GroundPlane> &sub_gp,
                     Subscriber<CameraInfo> &sub_cam,
                     image_transport::SubscriberFilter &sub_col,
                     image_transport::ImageTransport &it){
    if(!pub_message.getNumSubscribers() && !pub_result_image.getNumSubscribers() && !pub_detected_persons.getNumSubscribers()) {
        ROS_DEBUG("HOG: No subscribers. Unsubscribing.");
        sub_msg.shutdown();
        sub_gp.unsubscribe();
        sub_cam.unsubscribe();
        sub_col.unsubscribe();
    } else {
        ROS_DEBUG("HOG: New subscribers. Subscribing.");
        if(strcmp(gp_topic.c_str(), "") == 0) {
            sub_msg = n.subscribe(img_topic.c_str(), 1, &imageCallback);
        }
        sub_cam.subscribe();
        sub_gp.subscribe();
        sub_col.subscribe(it,sub_col.getTopic().c_str(),1);
    }
}