C++ NodeHandle::setCallbackQueue方法代码示例

  GripperRosControl(const std::vector<std::string>& actr_names, const std::vector<std::string>& jnt_names,
                    const std::vector<std::string>& controller_names, const std::vector<double>& reducers)
    : actr_names_(actr_names)
    , jnt_names_(jnt_names)
    , controller_names_(controller_names)
  {
    for (int i = 0; i < jnt_names_.size(); i++)
    {
      std::string actr = actr_names_[i];
      std::string jnt = jnt_names_[i];

      // Get transmission
      boost::shared_ptr<transmission_interface::SimpleTransmission> t_ptr(
          new transmission_interface::SimpleTransmission(reducers[i]));
      trans_.push_back(t_ptr);

      // Initialize and wrap raw current data
      actr_curr_data_[actr].position.push_back(&actr_curr_pos_[actr]);
      actr_curr_data_[actr].velocity.push_back(&actr_curr_vel_[actr]);
      actr_curr_data_[actr].effort.push_back(&actr_curr_eff_[actr]);
      jnt_curr_data_[jnt].position.push_back(&jnt_curr_pos_[jnt]);
      jnt_curr_data_[jnt].velocity.push_back(&jnt_curr_vel_[jnt]);
      jnt_curr_data_[jnt].effort.push_back(&jnt_curr_eff_[jnt]);

      // Initialize and wrap raw command data
      actr_cmd_data_[actr].position.push_back(&actr_cmd_pos_[actr]);
      jnt_cmd_data_[jnt].position.push_back(&jnt_cmd_pos_[jnt]);

      // Register transmissions to each interface
      actr_to_jnt_state_.registerHandle(transmission_interface::ActuatorToJointStateHandle(
          actr + "_trans", trans_[i].get(), actr_curr_data_[actr], jnt_curr_data_[jnt]));
      jnt_to_actr_pos_.registerHandle(transmission_interface::JointToActuatorPositionHandle(
          jnt + "_trans", trans_[i].get(), actr_cmd_data_[actr], jnt_cmd_data_[jnt]));

      // Connect and register the joint state handle
      hardware_interface::JointStateHandle state_handle(jnt, &jnt_curr_pos_[jnt], &jnt_curr_vel_[jnt],
                                                        &jnt_curr_eff_[jnt]);
      jnt_state_interface_.registerHandle(state_handle);

      // Connect and register the joint position handle
      hardware_interface::JointHandle pos_handle(jnt_state_interface_.getHandle(jnt), &jnt_cmd_pos_[jnt]);
      pos_jnt_interface_.registerHandle(pos_handle);

      // ROS publishers and subscribers
      actr_cmd_pub_[actr] = nh_.advertise<std_msgs::Float64>("dxl/" + controller_names_[i] + "/command", 5);
      actr_state_sub_[actr] =
          nh_.subscribe("dxl/" + controller_names_[i] + "/state", 1, &GripperRosControl::actrStateCallback, this);
    }

    // Register interfaces
    registerInterface(&jnt_state_interface_);
    registerInterface(&pos_jnt_interface_);

    // Start spinning
    nh_.setCallbackQueue(&subscriber_queue_);
    subscriber_spinner_.reset(new ros::AsyncSpinner(1, &subscriber_queue_));
    subscriber_spinner_->start();
  }

    ImageConverter()
        : it_(nh_),
          ch_(nh_, "trigger_node")
    {
        ch_.setCallbackQueue(&trigger_queue_);
        // Subscribe to input video feed and publish output video feed
        image_sub_ = it_.subscribe("/camera/image_raw", 1,
                                   &ImageConverter::imageCb, this);
        image_pub_ = it_.advertise("/image_converter/output_video", 1);

        cv::namedWindow(OPENCV_WINDOW);
        advertisetriggerservice("no_name");
    }

/**
 * Initialize module - called in server constructor
 */
void srs_env_model::COcPatchMaker::init(ros::NodeHandle & node_handle)
{
	ROS_DEBUG("Initializing CCompressedPointCloudPlugin");

	if ( m_bSpinThread )
	{
		// if we're spinning our own thread, we'll also need our own callback queue
		node_handle.setCallbackQueue( &callback_queue_ );

		need_to_terminate_ = false;
		spin_thread_.reset( new boost::thread(boost::bind(&COcPatchMaker::spinThread, this)) );
		node_handle_ = node_handle;
	}

	// Read parameters
	{
		// Where to get camera position information
		node_handle.param("camera_info_topic_name", m_cameraInfoTopic, CPC_CAMERA_INFO_PUBLISHER_NAME );
	}


	// Subscribe to position topic
	// Create subscriber
	m_camPosSubscriber = node_handle.subscribe<sensor_msgs::CameraInfo>( m_cameraInfoTopic, 10, &srs_env_model::COcPatchMaker::onCameraChangedCB, this );

	if (!m_camPosSubscriber)
	{
		ROS_ERROR("Not subscribed...");
		ROS_ERROR( "Cannot subscribe to the camera position publisher..." );
	}

	// stereo cam params for sensor cone:
//	node_handle.param<int> ("compressed_pc_camera_stereo_offset_left", m_camera_stereo_offset_left, 0); // 128
//	node_handle.param<int> ("compressed_pc_camera_stereo_offset_right", m_camera_stereo_offset_right, 0);

	node_handle.param("registration_patch_view_fraction_x", m_fracX, m_fracX );
	node_handle.param("registration_patch_view_fraction_y", m_fracY, m_fracY );

	node_handle.param("registration_patch_publish_cloud", m_bPublishCloud, m_bPublishCloud );

	if( m_bPublishCloud )
		// Create publisher - simple point cloud
		m_pubConstrainedPC = node_handle.advertise<sensor_msgs::PointCloud2> (REGISTRATION_CONSTRAINED_CLOUD_PUBLISHER_NAME, 5, false);

}

void srs::CLimitedPointCloudPlugin::init(ros::NodeHandle & node_handle)
{
	if ( m_bSpinThread )
	{
		// if we're spinning our own thread, we'll also need our own callback queue
		node_handle.setCallbackQueue( &callback_queue_ );

		need_to_terminate_ = false;
		spin_thread_.reset( new boost::thread(boost::bind(&CLimitedPointCloudPlugin::spinThread, this)) );
		node_handle_ = node_handle;
	}

    // Read parameters

    // Camera position topic name
    std::string cameraPositionTopic;
    node_handle.param("rviz_camera_position_topic_name", cameraPositionTopic, CAMERA_POSITION_TOPIC_NAME );

    // Point cloud publishing topic name
    node_handle.param("pointcloud_centers_publisher", m_pcPublisherName, POINTCLOUD_CENTERS_PUBLISHER_NAME );

    // Create publisher
    m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 100, m_latchedTopics);

    // Subscribe to position topic
    // Create subscriber
    m_camPosSubscriber = node_handle.subscribe<srs_env_model_msgs::RVIZCameraPosition>( cameraPositionTopic, 10, &srs::CLimitedPointCloudPlugin::onCameraPositionChangedCB, this );
    // = new message_filters::Subscriber<srs_env_model_msgs::RVIZCameraPosition>(node_handle, cameraPositionTopic, 1);

    if (!m_camPosSubscriber)
    {
        ROS_ERROR("Not subscribed...");
        PERROR( "Cannot subscribe to the camera position publisher..." );
    }
/*
    // Create message filter
    m_tfCamPosSub = new tf::MessageFilter<srs_env_model_msgs::RVIZCameraPosition>( *m_camPosSubscriber, m_tfListener, "/map", 1);
    m_tfCamPosSub->registerCallback(boost::bind( &CLimitedPointCloudPlugin::onCameraPositionChangedCB, this, _1));
*/
    // Clear old pointcloud data
    m_data->clear();


}

  ShowTFTree() : root_nh_(""), priv_nh_("~")
  {
    tf_counter_ = 0;

    // subscribe to /tf
    string sub_topic;
    priv_nh_.param<string>("subscribe_topic", sub_topic, "/tf");

    sub_ = root_nh_.subscribe(sub_topic, 10, &ShowTFTree::callback, this);
    root_nh_.setCallbackQueue(&tf_message_callback_queue_);

    // create thread for processing callback queue
    boost::thread* dedicated_listener_thread_;
    dedicated_listener_thread_ = new boost::thread(boost::bind(&ShowTFTree::processCallbackQueueThread, this));

    // waiting callback
    cout <<"Collecting tf frames.. " ;
    wait_time_ = 3;
    priv_nh_.getParam("wait", wait_time_);
    sync_timer_ =  timer_nh_.createTimer(ros::Duration(1.0), boost::bind( &ShowTFTree::wait, this ) );
  }

/**
 * Initialize plugin
 */
void srs_env_model::CCompressedPointCloudPlugin::init(ros::NodeHandle & node_handle)
{
	ROS_DEBUG("Initializing CCompressedPointCloudPlugin");

	// 2013/01/31 Majkl: I guess we should publish the map in the Octomap TF frame...
	node_handle.param("ocmap_frame_id", m_frameId, m_frameId);

	if ( m_bSpinThread )
	{
		// if we're spinning our own thread, we'll also need our own callback queue
		node_handle.setCallbackQueue( &callback_queue_ );

		need_to_terminate_ = false;
		spin_thread_.reset( new boost::thread(boost::bind(&CCompressedPointCloudPlugin::spinThread, this)) );
		node_handle_ = node_handle;
	}

    // Read parameters
	{
		// Where to get camera position information
		node_handle.param("compressed_pc_camera_info_topic_name", m_cameraInfoTopic, CPC_CAMERA_INFO_PUBLISHER_NAME );
		ROS_INFO("SRS_ENV_SERVER: parameter - compressed_pc_camera_info_topic_name: %s", m_cameraInfoTopic.c_str() );

		// Point cloud publishing topic name
		node_handle.param("compressed_pc_pointcloud_centers_publisher", m_pcPublisherName, CPC_SIMPLE_PC_PUBLISHING_TOPIC_NAME);
		ROS_INFO("SRS_ENV_SERVER: parameter - compressed_pc_pointcloud_centers_publisher: %s", m_pcPublisherName.c_str() );

		// Should simple pointcloud be published too?
		node_handle.param("publish_simple_cloud", m_bPublishSimpleCloud, false );

		// How many uncomplete frames should be published before complete one
		int uf;
		node_handle.param("compressed_pc_differential_frames_count", uf, CPC_NUM_DIFFERENTIAL_FRAMES );
		m_uncomplete_frames = uf;

		ROS_INFO("SRS_ENV_SERVER: parameter - compressed_pc_update_data_topic_name: %i", uf );

		// Complete data topic name
		node_handle.param( "compressed_pc_update_data_topic_name", m_ocUpdatePublisherName, CPC_COMPLETE_TOPIC_NAME );
		ROS_INFO("SRS_ENV_SERVER: parameter - compressed_pc_update_data_topic_name: %s", m_ocUpdatePublisherName.c_str() );
	}

	// Services
	{
		m_serviceSetNumIncomplete = node_handle.advertiseService( SetNumIncompleteFrames_SRV,
				&srs_env_model::CCompressedPointCloudPlugin::setNumIncompleteFramesCB, this );
	}

    // Create publisher - simple point cloud
	if( m_bPublishSimpleCloud)
		m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 5, m_latchedTopics);

    // Create publisher - packed info - cam position and update pointcloud
    m_ocUpdatePublisher = node_handle.advertise< srs_env_model::OctomapUpdates > ( m_ocUpdatePublisherName, 5, m_latchedTopics );

    // Subscribe to position topic
    // Create subscriber
    m_camPosSubscriber = node_handle.subscribe<sensor_msgs::CameraInfo>( m_cameraInfoTopic, 10, &srs_env_model::CCompressedPointCloudPlugin::onCameraChangedCB, this );
    // = new message_filters::Subscriber<srs_env_model_msgs::RVIZCameraPosition>(node_handle, cameraPositionTopic, 1);

    if (!m_camPosSubscriber)
    {
        ROS_ERROR("Not subscribed...");
        PERROR( "Cannot subscribe to the camera position publisher..." );
    }
/*
    // Create message filter
    m_tfCamPosSub = new tf::MessageFilter<srs_env_model_msgs::RVIZCameraPosition>( *m_camPosSubscriber, m_tfListener, "/map", 1);
    m_tfCamPosSub->registerCallback(boost::bind( &CCompressedPointCloudPlugin::onCameraPositionChangedCB, this, _1));
*/
    // Clear old pointcloud data
    m_data->clear();

    // stereo cam params for sensor cone:
	node_handle.param<int> ("compressed_pc_camera_stereo_offset_left", m_camera_stereo_offset_left, 0); // 128
	node_handle.param<int> ("compressed_pc_camera_stereo_offset_right", m_camera_stereo_offset_right, 0);

}