C++ Publisher::getNumSubscribers方法代码示例

int getNumDepthSubscribers()
{
    int n = realsense_points_pub.getNumSubscribers() + realsense_reg_points_pub.getNumSubscribers() + realsense_depth_image_pub.getNumSubscribers();
#ifdef V4L2_PIX_FMT_INZI
    n += realsense_infrared_image_pub.getNumSubscribers();
#endif
    return n;
}

void SBANode::publishTopics(/*const ros::TimerEvent& event*/)
{
  // Visualization
  if (cam_marker_pub.getNumSubscribers() > 0 || point_marker_pub.getNumSubscribers() > 0)
  { 
     drawGraph(sba, cam_marker_pub, point_marker_pub);
  }
}

void publishState(void)
{
	uint8_t buf[10];
	const int ret = libusb_control_transfer(dev, 0xC0, 0x32, 0x0, 0x0, buf, 10, 0);
	if (ret != 10)
	{
		ROS_ERROR_STREAM("Error in accelerometer reading, libusb_control_transfer returned " << ret);
		ros::shutdown();
	}
	
	const uint16_t ux = ((uint16_t)buf[2] << 8) | buf[3];
	const uint16_t uy = ((uint16_t)buf[4] << 8) | buf[5];
	const uint16_t uz = ((uint16_t)buf[6] << 8) | buf[7];
	
	const int16_t accelerometer_x = (int16_t)ux;
	const int16_t accelerometer_y = (int16_t)uy;
	const int16_t accelerometer_z = (int16_t)uz;
	const int8_t tilt_angle = (int8_t)buf[8];
	const uint8_t tilt_status = buf[9];
	
	// publish IMU
	sensor_msgs::Imu imu_msg;
	if (pub_imu.getNumSubscribers() > 0)
	{
		imu_msg.header.stamp = ros::Time::now();
		imu_msg.linear_acceleration.x = (double(accelerometer_x)/FREENECT_COUNTS_PER_G)*GRAVITY;
		imu_msg.linear_acceleration.y = (double(accelerometer_y)/FREENECT_COUNTS_PER_G)*GRAVITY;
		imu_msg.linear_acceleration.z = (double(accelerometer_z)/FREENECT_COUNTS_PER_G)*GRAVITY;
		imu_msg.linear_acceleration_covariance[0] = imu_msg.linear_acceleration_covariance[4]
			= imu_msg.linear_acceleration_covariance[8] = 0.01; // @todo - what should these be?
		imu_msg.angular_velocity_covariance[0] = -1; // indicates angular velocity not provided
		imu_msg.orientation_covariance[0] = -1; // indicates orientation not provided
		pub_imu.publish(imu_msg);
	}
	
	// publish tilt angle and status
	if (pub_tilt_angle.getNumSubscribers() > 0)
	{
		std_msgs::Float64 tilt_angle_msg;
		tilt_angle_msg.data = double(tilt_angle) / 2.;
		pub_tilt_angle.publish(tilt_angle_msg);
	}
	if (pub_tilt_status.getNumSubscribers() > 0)
	{
		std_msgs::UInt8 tilt_status_msg;
		tilt_status_msg.data = tilt_status;
		pub_tilt_status.publish(tilt_status_msg);
	}
}

  void updateCallback(const ros::TimerEvent& e)
  {
    static bool got_first = false;
    latest_dt = (e.current_real - e.last_real).toSec();
    latest_jitter = (e.current_real - e.current_expected).toSec();
    max_abs_jitter = max(max_abs_jitter, fabs(latest_jitter));
    Result::Enum the_result;
    bool was_new_frame = true;

    if ((not segment_data_enabled) //
        and (pose_pub.getNumSubscribers() > 0 or markers_pub.getNumSubscribers() > 0))
    {
      the_result = MyClient.EnableSegmentData().Result;
      if (the_result != Result::Success)
      {
        ROS_ERROR("Enable segment data call failed");
      }
      else
      {
        ROS_ASSERT(MyClient.IsSegmentDataEnabled().Enabled);
        ROS_INFO("Segment data enabled");
        segment_data_enabled = true;
      }
    }

    if (segment_data_enabled)
    {
      while (was_new_frame and (the_result = MyClient.GetFrame().Result) == Result::Success)
        ;
      {
        now_time = ros::Time::now(); // try to grab as close to getting message as possible
        was_new_frame = process_frame();
        got_first = true;
      }

      if (the_result != Result::NoFrame)
      {
        ROS_ERROR_STREAM("GetFrame() returned " << (int)the_result);
      }

      if (got_first)
      {
        max_period_between_updates = max(max_period_between_updates, latest_dt);
        last_callback_duration = e.profile.last_duration.toSec();
      }
    }
    diag_updater.update();
  }

 void publishObstacleMarker(const pcl::PointXYZ& obstacle_location)
 {
   if (marker_pub.getNumSubscribers())
   {
     visualization_msgs::Marker obstacle;
     obstacle.header.frame_id = nh.getNamespace() + "/kinect_depth";
     obstacle.header.stamp = ros::Time::now();
     obstacle.ns = "obstacle";
     obstacle.action = visualization_msgs::Marker::ADD;
     obstacle.pose.position.x = obstacle_location.x;
     obstacle.pose.position.y = obstacle_location.y;
     obstacle.pose.position.z = obstacle_location.z;
     obstacle.pose.orientation.w = 1.0;
     obstacle.id = 0;
     obstacle.type = visualization_msgs::Marker::SPHERE;
     obstacle.scale.x = 0.1;
     obstacle.scale.y = 0.1;
     obstacle.scale.z = 0.1;
     obstacle.color.r = 1.0;
     obstacle.color.g = 0.0;
     obstacle.color.b = 0.0;
     obstacle.color.a = 0.8;
     obstacle.lifetime = ros::Duration(1.0);
     marker_pub.publish(obstacle);
   }
 }

void callback(const ros::MessageEvent<variant_topic_tools::Message>&
              messageEvent) {
    boost::shared_ptr<const variant_topic_tools::Message> message =
        messageEvent.getConstMessage();
    boost::shared_ptr<const ros::M_string> connectionHeader =
        messageEvent.getConnectionHeaderPtr();

    if (!publisher) {
        bool latch = false;

        if (connectionHeader) {
            ros::M_string::const_iterator it = connectionHeader->find("latching");
            if ((it != connectionHeader->end()) && (it->second == "1"))
                latch = true;
        }

        ros::AdvertiseOptions options(publisherTopic, publisherQueueSize,
                                      message->getType().getMD5Sum(), message->getType().getDataType(),
                                      message->getType().getDefinition(), connectCallback);
        options.latch = latch;

        publisher = nodeHandle->advertise<variant_msgs::Variant>(publisherTopic,
                    publisherQueueSize, connectCallback, ros::SubscriberStatusCallback(),
                    ros::VoidConstPtr(), latch);
    }

    if(!lazy || publisher.getNumSubscribers()) {
        boost::shared_ptr<const variant_msgs::Variant> variantMessage =
            message->toVariantMessage();
        publisher.publish(variantMessage);
    }
    else
        subscriber = ros::Subscriber();
}

	LocationFileWriter(string filename, bool append) {
		marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 1000);
		// Wait for subscribers to register before placing markers.
		int n;
		ros::Rate r(10);
		while (ros::ok() && n < 50) {
			// Note: do not run "rostopic echo visualization_marker" or this will not work!
			if (marker_pub.getNumSubscribers() > 0) {
				break;
			}
			n++;
			r.sleep();
		}
		next_id = 0;
		if (append) {
			auto locs = parse_locations(filename);
			// Place markers and set next_id to the max id of the parsed locations.
			for (auto it = locs.begin(); it != locs.end(); it++) {
				placeMarker(*it);
				if (it->id > next_id) {
					next_id = it->id;
				}
			}
		}
		next_id++;
		outfile.open(filename, append ? ofstream::app : ofstream::trunc);
		loc_sub = nh.subscribe("clicked_point", 1, &LocationFileWriter::locHandler, this);
	}

void timerCallback(const ros::TimerEvent& event) {
  if(!initalized){ return; }



  if(supplyVoltagePub.getNumSubscribers() > 0 /*|| batteryStatePub.getNumSubscribers() > 0*/){
    std_msgs::Float32 voltsMsg = publishSupplyVoltage(mcphid);
    /*sensor_msgs::BatteryState battMsg;

    battMsg.header.stamp = ros::Time::now();
    battMsg.voltage = voltsMsg.data;
    battMsg.current = nanf();
    battMsg.charge = nanf();
    battMsg.capacity = nanf();
    battMsg.design_capacity = nanf();
    battMsg.percentage = nanf();

    batteryStatePub.publish(battMsg);*/
  }

  /*if(motorCurrentPub.getNumSubscribers() > 0){
    publishMotorCurrents(mcphid);
  }

  if(motorBackEMFPub.getNumSubscribers() > 0){
    publishBackEMF(mcphid);
  }*/
}

/**
 * @brief It publishes the boundaries of the planes periodically.
 * 
 * @param s Segmenter instance already initiliazated.
 * @param marPub Publisher.
 * @param frame_id Reference frame.
 */
void publishBoundaries(const ros::TimerEvent&, const MultiplePlaneSegmentation &s, const ros::Publisher &marPub, const std::string &frame_id) {

	if (marPub.getNumSubscribers() > 0) {
		std::vector<std::vector<pcl::PointXYZRGBA>> boundaries;
		s.getBoundaries(boundaries);
		
		for(int i = 0; i < boundaries.size(); i++) {
			std::vector< std::vector<double> > positions = std::vector< std::vector<double> >(boundaries[i].size() + 1, std::vector<double>(3,0));
			int j;

			if (boundaries[i].size() == 0) continue;

			// Lines between consecutive points.
			for(j = 0; j < boundaries[i].size(); j++) {
				pcl::PointXYZRGBA p = boundaries[i][j];

				positions[j][0] = p.x;
				positions[j][1] = p.y;
				positions[j][2] = p.z;
			}
			// create a line between last and first point.
			positions[j] = positions[0];

			double width = 0.03;
			std::vector<double> color;
			computeColor(i, boundaries.size(), color);
			double colorArr[] = {color[0], color[1], color[2], 1.0};
			marPub.publish(buildLineMarker(frame_id, i, positions, width, colorArr));
	 	}
	}
}

void in_cb(const boost::shared_ptr<ShapeShifter const>& msg)
{
  if (!g_advertised)
  {
    // If the input topic is latched, make the output topic latched, #3385.
    bool latch = false;
    ros::M_string::iterator it = msg->__connection_header->find("latching");
    if((it != msg->__connection_header->end()) && (it->second == "1"))
    {
      ROS_DEBUG("input topic is latched; latching output topic to match");
      latch = true;
    }
    g_pub = msg->advertise(*g_node, g_output_topic, 10, latch, conn_cb);
    g_advertised = true;
    ROS_INFO("advertised as %s\n", g_output_topic.c_str());
  }
  // If we're in lazy subscribe mode, and nobody's listening, 
  // then unsubscribe, #3389.
  if(g_lazy && !g_pub.getNumSubscribers())
  {
    ROS_DEBUG("lazy mode; unsubscribing");
    delete g_sub;
    g_sub = NULL;
  }
  else
    g_pub.publish(msg);
}

int main(int argc, char** argv) {

	ros::init(argc, argv, "add_noise_pcl");

	ROS_INFO("Starting kinect noise generator node...");

	ros::NodeHandle n;

	ros::param::param<bool>("~add_noise",add_noise,true);
	ros::param::param<double>("~noise_amount_coef",nac,1.0);

	ros::Subscriber sub;

	m_pub = n.advertise<sensor_msgs::PointCloud2> ("points_out", 1);

	ros::Rate r(1);

	ros::AsyncSpinner spinner(5);
	spinner.start();

	ROS_INFO("Spinning");

	while(ros::ok()) {

		if (m_pub.getNumSubscribers() != 0) sub = n.subscribe("points_in", 1, cloudCallback);
		else sub.shutdown();

		r.sleep();

	}

	spinner.stop();

}

	void handle_flow(const mavlink_message_t *msg) {
		if (flow_pub.getNumSubscribers() == 0)
			return;

		mavlink_optical_flow_t flow;
		mavlink_msg_optical_flow_decode(msg, &flow);

		mavros_extras::OpticalFlowPtr flow_msg =
			boost::make_shared<mavros_extras::OpticalFlow>();

		// Note: for ENU->NED conversion i swap x & y.
		flow_msg->header.stamp = ros::Time::now();
		flow_msg->flow_x = flow.flow_y;
		flow_msg->flow_y = flow.flow_x;
		flow_msg->flow_comp_m_x	 = flow.flow_comp_m_y;
		flow_msg->flow_comp_m_y	 = flow.flow_comp_m_x;
		flow_msg->quality = flow.quality;
		flow_msg->ground_distance = flow.ground_distance;

		flow_pub.publish(flow_msg);

		/* Optional TODO: send ground_distance in sensor_msgs/Range
		 *                with data filled by spec on used sonar.
		 */
	}

bool RosAriaNode::hasSonarSubscribers()
{
    int count = combined_range_pub.getNumSubscribers();
    for (int i = sonars__crossed_the_streams ? 8 : 0; i < 16; i++)
        count += range_pub[i].getNumSubscribers();
    return count > 0;
}

	void placeMarker(snacbot::Location l) {
		visualization_msgs::Marker m;
		m.header.frame_id = "map";
		m.header.stamp = ros::Time::now();
		m.ns = "fast_smart_good";
		m.id = l.id;
		m.frame_locked = true;
		m.type = visualization_msgs::Marker::SPHERE;
		m.action = visualization_msgs::Marker::ADD;
		m.pose.position.x = l.x;
		m.pose.position.y = l.y;
		m.pose.position.z = 1;
		m.pose.orientation.x = 0.0;
		m.pose.orientation.y = 0.0;
		m.pose.orientation.z = 0.0;
		m.pose.orientation.w = 1.0;
		m.scale.x = 0.2;
		m.scale.y = 0.2;
		m.scale.z = 0.2;
		m.color.a = 1.0;
		m.color.r = 0.8;
		m.color.g = 0.2;
		m.color.b = 0.1;
		m.text = to_string(l.id);
		marker_pub.publish(m);
		ROS_INFO("published loc %ld at (%.2f, %.2f)", l.id, l.x, l.y);
		ROS_INFO("marker info %d", marker_pub.getNumSubscribers());
	}

/// Called when another node subscribes or unsubscribes from sonar topic.
void RosAriaNode::sonarConnectCb()
{
  publish_sonar = (sonar_pub.getNumSubscribers() > 0);
  publish_sonar_pointcloud2 = (sonar_pointcloud2_pub.getNumSubscribers() > 0);
  robot->lock();
  if (publish_sonar || publish_sonar_pointcloud2)
  {
    robot->enableSonar();
    sonar_enabled = false;
  }
  else if(!publish_sonar && !publish_sonar_pointcloud2)
  {
    robot->disableSonar();
    sonar_enabled = true;
  }
  robot->unlock();
}