C++ message_filters::Subscriber类代码示例

    DogPositionMeasurer() :
            pnh("~"),
            meanPositionDeviation(0),
            m2PositionDeviation(0),
            meanTimeDuration(0),
            m2TimeDuration(0),
            meanUnknownTimeDuration(0),
            m2UnknownTimeDuration(0),
            n(0),
            unknownN(0),
            startMeasuringSub(nh, "start_measuring", 1),
            stopMeasuringSub(nh, "stop_measuring", 1) {

        pnh.param<string>("model_name", modelName, "dog");

        // Setup the subscriber
        dogSub.reset(
                new message_filters::Subscriber<dogsim::DogPosition>(nh,
                        "/dog_position_detector/dog_position", 1));
        dogSub->unsubscribe();
        dogSub->registerCallback(boost::bind(&DogPositionMeasurer::callback, this, _1));

        startMeasuringSub.registerCallback(
                boost::bind(&DogPositionMeasurer::startMeasuring, this, _1));
        stopMeasuringSub.registerCallback(
                boost::bind(&DogPositionMeasurer::stopMeasuring, this, _1));
    }

  void onInit()
  {
    nh = getMTNodeHandle();
    nh_priv = getMTPrivateNodeHandle();

    // Parameters
    nh_priv.param("use_backup_estimator_alt", use_backup_estimator_alt, use_backup_estimator_alt);
    nh_priv.param("imu_to_kinect_offset", imu_to_kinect_offset, imu_to_kinect_offset);
    nh_priv.param("max_est_kinect_delta_alt", max_est_kinect_delta_alt, max_est_kinect_delta_alt);
    nh_priv.param("obstacle_height_threshold", obstacle_height_threshold, obstacle_height_threshold);
    nh_priv.param("debug_throttle_rate", debug_throttle_rate, debug_throttle_rate);
    nh_priv.param("z_vel_filt_a", z_vel_filt_a, z_vel_filt_a);
    nh_priv.param("z_vel_filt_b", z_vel_filt_b, z_vel_filt_b);
    // Publishers
    odom_pub = nh_priv.advertise<nav_msgs::Odometry> ("output", 10);
    obstacle_pub = nh_priv.advertise<starmac_kinect::Obstacle> ("obstacle", 10);
    debug_pub = nh_priv.advertise<starmac_kinect::Debug> ("debug", 10);
    marker_pub = nh_priv.advertise<visualization_msgs::Marker> ("marker", 10);
    mask_indices_pub = nh.advertise<pcl::PointIndices> ("mask_indices", 10);
    findplane_indices_pub = nh.advertise<pcl::PointIndices> ("findplane_indices", 10, true);
    // Subscribers
    sub_input_filter.subscribe(nh_priv, "input", max_queue_size);
    sub_indices_filter.subscribe(nh_priv, "indices", max_queue_size);
    sub_model_filter.subscribe(nh_priv, "model", max_queue_size);
    sync_input_indices_e
        = make_shared<Synchronizer<ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size);
    sync_input_indices_e->connectInput(sub_input_filter, sub_indices_filter, sub_model_filter);
    sync_input_indices_e->registerCallback(bind(&KinectEstimator::cloudIndicesModelCallback, this, _1, _2, _3));

    // Fill out most of the output Odometry message (we'll only be filling in the z pose value)
    odom_msg_output.child_frame_id = "imu";
    odom_msg_output.header.frame_id = "ned";
    odom_msg_output.pose.pose.orientation.x = 0;
    odom_msg_output.pose.pose.orientation.y = 0;
    odom_msg_output.pose.pose.orientation.z = 0;
    odom_msg_output.pose.pose.orientation.w = 1;
    odom_msg_output.pose.pose.position.x = 0;
    odom_msg_output.pose.pose.position.y = 0;

    if (use_backup_estimator_alt)
    {
      est_odom_sub = nh.subscribe("estimator/output", 1, &KinectEstimator::estOdomCallback, this,
                                  ros::TransportHints().tcpNoDelay());
    }

    updateMask(); // force once to start things

  }

 void checkInputsSynchronized()
 {
   int threshold = 3 * all_received_;
   if (left_received_ >= threshold || right_received_ >= threshold || 
       left_info_received_ >= threshold || right_info_received_ >= threshold) {
     ROS_WARN("[stereo_processor] Low number of synchronized left/right/left_info/right_info tuples received.\n"
              "Left images received:       %d (topic '%s')\n"
              "Right images received:      %d (topic '%s')\n"
              "Left camera info received:  %d (topic '%s')\n"
              "Right camera info received: %d (topic '%s')\n"
              "Synchronized tuples: %d\n"
              "Possible issues:\n"
              "\t* stereo_image_proc is not running.\n"
              "\t  Does `rosnode info %s` show any connections?\n"
              "\t* The cameras are not synchronized.\n"
              "\t  Try restarting the node with parameter _approximate_sync:=True\n"
              "\t* The network is too slow. One or more images are dropped from each tuple.\n"
              "\t  Try restarting the node, increasing parameter 'queue_size' (currently %d)",
              left_received_, left_sub_.getTopic().c_str(),
              right_received_, right_sub_.getTopic().c_str(),
              left_info_received_, left_info_sub_.getTopic().c_str(),
              right_info_received_, right_info_sub_.getTopic().c_str(),
              all_received_, ros::this_node::getName().c_str(), queue_size_);
   }
 }

	CaptureServer() :
			nh_private("~") {

		ROS_INFO("Creating localization");

		queue_size_ = 5;

		odom_pub = nh_.advertise<nav_msgs::Odometry>("vo", queue_size_);
		keyframe_pub = nh_.advertise<rm_localization::Keyframe>("keyframe",
				queue_size_);

		update_map_service = nh_.advertiseService("update_map",
				&CaptureServer::update_map, this);

		send_all_keyframes_service = nh_.advertiseService("send_all_keyframes",
				&CaptureServer::send_all_keyframes, this);

		clear_keyframes_service = nh_.advertiseService("clear_keyframes",
				&CaptureServer::clear_keyframes, this);

		rgb_sub.subscribe(nh_, "rgb/image_raw", queue_size_);
		depth_sub.subscribe(nh_, "depth/image_raw", queue_size_);
		info_sub.subscribe(nh_, "rgb/camera_info", queue_size_);

		// Synchronize inputs.
		sync.reset(
				new Synchronizer(SyncPolicy(queue_size_), rgb_sub, depth_sub,
						info_sub));

		sync->registerCallback(
				boost::bind(&CaptureServer::RGBDCallback, this, _1, _2, _3));

	}

    CaptureServer() :
        nh_private("~") {

        ROS_INFO("Creating localization");

        tf_prefix_ = tf::getPrefixParam(nh_private);
        odom_frame = tf::resolve(tf_prefix_, "odom_combined");
        map_frame = tf::resolve(tf_prefix_, "map");
        map_to_odom.setIdentity();
        queue_size_ = 1;

        rgb_sub.subscribe(nh_, "rgb/image_raw", queue_size_);
        depth_sub.subscribe(nh_, "depth/image_raw", queue_size_);
        info_sub.subscribe(nh_, "rgb/camera_info", queue_size_);

        // Synchronize inputs.
        sync.reset(
            new Synchronizer(SyncPolicy(queue_size_), rgb_sub, depth_sub,
                             info_sub));

        sync->registerCallback(
            boost::bind(&CaptureServer::RGBDCallback, this, _1, _2, _3));

        tracked_points.reset(new std::vector<cv::Vec2f>);

    }

 void subscribe()
 {
   
   if (use_indices_) {
     sub_input_.subscribe(*pnh_, "input", 1);
     sub_indices_.subscribe(*pnh_, "indices", 1);
     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(10);
     sync_->connectInput(sub_input_, sub_indices_);
     if (!not_use_rgb_) {
       sync_->registerCallback(boost::bind(&ResizePointsPublisher::filter<pcl::PointXYZRGB>, this, _1, _2));
     }
     else {
       sync_->registerCallback(boost::bind(&ResizePointsPublisher::filter<pcl::PointXYZ>, this, _1, _2));
     }
   }
   else {
     if (!not_use_rgb_) {
       sub_ = pnh_->subscribe(
         "input", 1,
         &ResizePointsPublisher::filter<pcl::PointXYZRGB>, this);
     }
     else {
       sub_ = pnh_->subscribe(
         "input", 1,
         &ResizePointsPublisher::filter<pcl::PointXYZ>, this);
     }
   }
 }

  explicit NegExampleNode(const ros::NodeHandle& nh): node_(nh)
  {
    disparity_scale_factor_ = 2.0; // hard coded to match roiPlayer
    string nn = ros::this_node::getName();
    node_.param(nn+"/imageFolderPath",imgFolderPath, std::string("."));
    int qs;
    if(!node_.getParam(nn + "/Q_Size",qs)){
      qs=3;
    }

    // Subscribe to Messages
    sub_image_.subscribe(node_,"Color_Image",qs);
    sub_disparity_.subscribe(node_, "Disparity_Image",qs);
    sub_rois_.subscribe(node_,"input_rois",qs);

    // Sync the Synchronizer
    approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(qs), 
						sub_image_, 
						sub_disparity_,
						sub_rois_));

    approximate_sync_->registerCallback(boost::bind(&NegExampleNode::imageCb,
						    this,
						    _1,
						    _2,
						    _3));
  }

 void unsubscribe()
 {
   if (use_indices_) {
     sub_input_.unsubscribe();
     sub_indices_.unsubscribe();
   }
   else {
     sub_.shutdown();
   }
 }

    ObjectTracker(ros::NodeHandle& nh)
    {
        camera_sub.subscribe(nh, "image", 1);
        saliency_sub.subscribe(nh, "saliency_img", 1);
        fg_objects_sub.subscribe(nh, "probability_image", 1);

        // ApproximateTime takes a queue size as its constructor argument, hence SyncPolicy(10)
        sync = new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), camera_sub, fg_objects_sub, saliency_sub);
        sync->registerCallback(boost::bind(&ObjectTracker::process_images, this, _1, _2, _3));
    }

 /**
 @brief Конструктор
 @details Подписывается на топики с камер, создает объект sync для синхронизации получаемых видео-потоков и регистрирует коллбэк,
 который будет вызыватся при получении новых данных с топиков.
 */
 ImageConverter()
   : it_(nh_)
 {
   ROS_INFO("ImageConverter constructor");
   image_sub_left.subscribe(nh_, "/wide_stereo/left/image_rect_color", 1);
   image_sub_right.subscribe(nh_, "/wide_stereo/right/image_rect_color", 1);
   message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image> sync(image_sub_left, image_sub_right, 1);
   sync.registerCallback(boost::bind(ImageConverter::imageCb, _1, _2));
   cv::namedWindow(OPENCV_WINDOW);
   ros::spin();
 }

  /**
   * 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, 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));
    }
  }

  explicit HaarAdaNode(const ros::NodeHandle& nh):
    node_(nh)
  {
    num_class1 = 0;
    num_class0 = 0;
    num_TP_class1 = 0;
    num_FP_class1 = 0;
    num_TP_class0 = 0;
    num_FP_class0 = 0;

    string nn = ros::this_node::getName();
    int qs;
    if(!node_.getParam(nn + "/Q_Size",qs)){
      qs=3;
    }

    int NS;
    if(!node_.getParam(nn + "/num_Training_Samples",NS)){
      NS = 350; // default sets asside very little for training
      node_.setParam(nn + "/num_Training_Samples",NS);
    }
    HAC_.setMaxSamples(NS);

    if(!node_.getParam(nn + "/RemoveOverlappingRois",remove_overlapping_rois_)){
      remove_overlapping_rois_ = false;
    }

    // Published Messages
    pub_rois_           = node_.advertise<Rois>("HaarAdaOutputRois",qs);
    pub_Color_Image_    = node_.advertise<Image>("HaarAdaColorImage",qs);
    pub_Disparity_Image_= node_.advertise<DisparityImage>("HaarAdaDisparityImage",qs);

	
    // Subscribe to Messages
    sub_image_.subscribe(node_,"Color_Image",qs);
    sub_disparity_.subscribe(node_, "Disparity_Image",qs);
    sub_rois_.subscribe(node_,"input_rois",qs);

    // Sync the Synchronizer
    approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(qs), 
						sub_image_, 
						sub_disparity_,
						sub_rois_));

    approximate_sync_->registerCallback(boost::bind(&HaarAdaNode::imageCb,
						    this,
						    _1,
						    _2,
						    _3));
  }

    explicit roiViewerNode(const ros::NodeHandle& nh):
        node_(nh)
    {

        label = 0;
        show_confidence = false;

        //Read mode from launch file
        std::string mode="";
        node_.param(ros::this_node::getName() + "/mode", mode, std::string("none"));
        ROS_INFO("Selected mode: %s",mode.c_str());

        if(mode.compare("roi_display")==0) {
            ROS_INFO("MODE: %s",mode.c_str());


            node_.param(ros::this_node::getName()+"/vertical",vertical,false);

            //Get the image width and height
            node_.param(ros::this_node::getName()+"/label",label,0);

            //Read parameter for showing roi confidence
            node_.param(ros::this_node::getName()+"/show_confidence",show_confidence,false);

            //Read parameter stating if the image is grayscale or with colors
            node_.param(ros::this_node::getName()+"/color_image", color_image, true);

            // Subscribe to Messages
            sub_image_.subscribe(node_,"input_image",20);
            sub_detections_.subscribe(node_,"input_detections",20);

            // Sync the Synchronizer
            approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(20),
                                    sub_image_,
                                    sub_detections_));

            approximate_sync_->registerCallback(boost::bind(&roiViewerNode::imageCb,
                                                this,
                                                _1,
                                                _2));
        } else {

            ROS_INFO("Unknown mode:%s  Please set to {roi_display} in roiViewer.launch",mode.c_str());
        }
        // Visualization
        cv::namedWindow("Detections", WINDOW_AUTOSIZE );
        cv::startWindowThread();

    }

  laserScanToSamgar(ros::NodeHandle n) : 
    n_(n),
    laser_sub_(n_, "base_scan", 10)
  {
    laser_sub_.registerCallback(boost::bind(&laserScanToSamgar::scanCallback, this, _1));
    //scan_pub_ = n_.advertise<sensor_msgs::PointCloud>("/my_cloud",1);

  }