C++ image_transport::CameraPublisher类代码示例

  void OpenNINode::publishRgbImage(const openni_wrapper::Image& rgb_img, image_transport::CameraPublisher img_pub) const {
      sensor_msgs::ImagePtr rgb_msg = boost::make_shared<sensor_msgs::Image>();
      ros::Time time = ros::Time::now();
      rgb_msg->header.stamp    = time;
      rgb_msg->encoding        = enc::RGB8;
      rgb_msg->width           = depth_width_;
      rgb_msg->height          = depth_height_;
      rgb_msg->step            = rgb_msg->width * 3;
      rgb_msg->data.resize(rgb_msg->height * rgb_msg->step);

      rgb_img.fillRGB(rgb_msg->width, rgb_msg->height, reinterpret_cast<unsigned char*>(&rgb_msg->data[0]), rgb_msg->step);

      img_pub.publish(rgb_msg, getDepthCameraInfo(rgb_msg->width, rgb_msg->height, time, 1));
    }

void publishframe(camera_info_manager::CameraInfoManager *mgr, const image_transport::CameraPublisher &campub, const sensor_msgs::ImagePtr img, const std::string& frame)
{
  ros::Time timestamp = ros::Time::now();
  sensor_msgs::CameraInfo::Ptr cinfo(new sensor_msgs::CameraInfo(mgr->getCameraInfo()));
  std_msgs::Header::Ptr imageheader(new std_msgs::Header());
  std_msgs::Header::Ptr cinfoheader(new std_msgs::Header());
  imageheader->frame_id = frame;
  cinfoheader->frame_id = frame;
  imageheader->stamp = timestamp;
  cinfoheader->stamp = timestamp;
  img->header = *imageheader;
  cinfo->header = *cinfoheader;
  campub.publish(img, cinfo);
}

  void OpenNINode::publishDepthImage(const openni_wrapper::DepthImage& depth, image_transport::CameraPublisher depth_pub) const {
      sensor_msgs::ImagePtr depth_msg = boost::make_shared<sensor_msgs::Image>();
      ros::Time time = ros::Time::now();
      depth_msg->header.stamp    = time;
      depth_msg->encoding        = sensor_msgs::image_encodings::TYPE_32FC1;
      depth_msg->width           = depth_width_;
      depth_msg->height          = depth_height_;
      depth_msg->step            = depth_msg->width * sizeof(float);
      depth_msg->data.resize(depth_msg->height * depth_msg->step);

      depth.fillDepthImage(depth_msg->width, depth_msg->height, reinterpret_cast<float*>(&depth_msg->data[0]), depth_msg->step);

      depth_pub.publish(depth_msg, getDepthCameraInfo(depth_msg->width, depth_msg->height, time, 1));
      //pub.publish(depth_msg);
    }

    	/// Continuously advertises xyz and grey images.
	bool spin()
	{
		boost::mutex::scoped_lock lock(service_mutex_);
		sensor_msgs::Image::Ptr xyz_image_msg_ptr;
		sensor_msgs::Image::Ptr grey_image_msg_ptr;
		sensor_msgs::CameraInfo tof_image_info;
	
		if(tof_camera_->AcquireImages(0, &grey_image_32F1_, &xyz_image_32F3_, false, false, ipa_CameraSensors::INTENSITY) & ipa_Utils::RET_FAILED)
		{
			ROS_ERROR("[tof_camera] Tof image acquisition failed");
			return false;
		}

		/// Filter images by amplitude and remove tear-off edges
		//if(filter_xyz_tearoff_edges_ || filter_xyz_by_amplitude_)
		//	ROS_ERROR("[tof_camera] FUNCTION UNCOMMENT BY JSF");
		if(filter_xyz_tearoff_edges_) ipa_Utils::FilterTearOffEdges(xyz_image_32F3_, 0, (float)tearoff_tear_half_fraction_);
		if(filter_xyz_by_amplitude_) ipa_Utils::FilterByAmplitude(xyz_image_32F3_, grey_image_32F1_, 0, 0, lower_amplitude_threshold_, upper_amplitude_threshold_);

		try
		{
			IplImage img = xyz_image_32F3_;
			xyz_image_msg_ptr = sensor_msgs::CvBridge::cvToImgMsg(&img, "passthrough");
		}
		catch (sensor_msgs::CvBridgeException error)
		{
			ROS_ERROR("[tof_camera] Could not convert 32bit xyz IplImage to ROS message");
			return false;
		}

		try
		{
			IplImage img = grey_image_32F1_;
			grey_image_msg_ptr = sensor_msgs::CvBridge::cvToImgMsg(&img, "passthrough");
		}
		catch (sensor_msgs::CvBridgeException error)
		{
			ROS_ERROR("[tof_camera] Could not convert 32bit grey IplImage to ROS message");
			return false;
		}

		/// Set time stamp
		ros::Time now = ros::Time::now();
		xyz_image_msg_ptr->header.stamp = now;
		grey_image_msg_ptr->header.stamp = now;

		tof_image_info = camera_info_msg_;
		tof_image_info.width = grey_image_32F1_.cols;
		tof_image_info.height = grey_image_32F1_.rows;
		tof_image_info.header.stamp = now;

		/// publish message
		xyz_image_publisher_.publish(*xyz_image_msg_ptr, tof_image_info);
		grey_image_publisher_.publish(*grey_image_msg_ptr, tof_image_info);

		return true;
	}

  void imageCB(const sensor_msgs::ImageConstPtr& image_l,
	       const sensor_msgs::CameraInfoConstPtr& info_l,
	       const sensor_msgs::ImageConstPtr& image_r,
	       const sensor_msgs::CameraInfoConstPtr& info_r)  {

    sensor_msgs::Image img = *image_r;
    sensor_msgs::CameraInfo info = *info_r;

    //img.header.stamp = image_l->header.stamp;
    //info.header.stamp = info_l->header.stamp;

    pub_left_.publish(image_l, info_l);
    pub_right_.publish(img, info, info_l->header.stamp);
    //pub_right_.publish(*image_r, *info_r, info_l->header.stamp);
  }

void
pubRealSenseInfraredImageMsg(cv::Mat& ir_mat)
{
	sensor_msgs::ImagePtr ir_img(new sensor_msgs::Image);;

	ir_img->header.seq = head_sequence_id;
	ir_img->header.stamp = head_time_stamp;
	ir_img->header.frame_id = depth_frame_id;


	ir_img->width = ir_mat.cols;
	ir_img->height = ir_mat.rows;

	ir_img->encoding = sensor_msgs::image_encodings::MONO8;
	ir_img->is_bigendian = 0;

	int step = sizeof(unsigned char) * ir_img->width;
	int size = step * ir_img->height;
	ir_img->step = step;
	ir_img->data.resize(size);
	memcpy(&ir_img->data[0], ir_mat.data, size);

	ir_camera_info->header.frame_id = depth_frame_id;
	ir_camera_info->header.stamp = head_time_stamp;
	ir_camera_info->header.seq = head_sequence_id;

	realsense_infrared_image_pub.publish(ir_img, ir_camera_info);
}

void
pubRealSenseDepthImageMsg32F(cv::Mat& depth_mat)
{
  sensor_msgs::ImagePtr depth_img(new sensor_msgs::Image);

  depth_img->header.seq = head_sequence_id;
  depth_img->header.stamp = head_time_stamp;
  depth_img->header.frame_id = depth_frame_id;

  depth_img->width = depth_mat.cols;
  depth_img->height = depth_mat.rows;

  depth_img->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
  depth_img->is_bigendian = 0;

  int step = sizeof(float) * depth_img->width;
  int size = step * depth_img->height;
  depth_img->step = step;
  depth_img->data.resize(size);
  memcpy(&depth_img->data[0], depth_mat.data, size);

  ir_camera_info->header.frame_id = depth_frame_id;
  ir_camera_info->header.stamp = head_time_stamp;
  ir_camera_info->header.seq = head_sequence_id;

/*double minVal, maxVal;
cv::minMaxLoc(depth_mat, &minVal, &maxVal);
std::cout << " ***** " << minVal << " " << maxVal << std::endl;*/

  realsense_depth_image_pub.publish(depth_img, ir_camera_info);
//pub_depth_info.publish(ir_camera_info);
}

// Handles (un)subscribing when clients (un)subscribe
void CropDecimateNodelet::connectCb()
{
  boost::lock_guard<boost::mutex> lock(connect_mutex_);
  if (pub_.getNumSubscribers() == 0)
    sub_.shutdown();
  else if (!sub_)
    sub_ = it_in_->subscribeCamera("image_raw", queue_size_, &CropDecimateNodelet::imageCb, this);
}

  virtual ~SensorStreamManager()
  {
    stream_.removeNewFrameListener(this);
    stream_.stop();
    stream_.destroy();

    publisher_.shutdown();
  }

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 publishImage (FlyCapture2::Image * frame)
  {
    //sensor_msgs::CameraInfoPtr cam_info(new sensor_msgs::CameraInfo(cam_info_->getCameraInfo()));
    // or with a member variable:
    cam_info_ptr_ = sensor_msgs::CameraInfoPtr(new sensor_msgs::CameraInfo(cam_info_mgr_->getCameraInfo()));

    if (processFrame (frame, img_, cam_info_ptr_))
      streaming_pub_.publish (img_, *cam_info_ptr_);
  }

  void stop ()
  {
    if (!running)
      return;

    cam_->stop ();              // Must stop camera before streaming_pub_.
    poll_srv_.shutdown ();
    streaming_pub_.shutdown ();

    running = false;
  }

// Handles (un)subscribing when clients (un)subscribe
void CropDecimateNodelet::connectCb()
{
  boost::lock_guard<boost::mutex> lock(connect_mutex_);
  if (pub_.getNumSubscribers() == 0)
    sub_.shutdown();
  else if (!sub_)
  {
    image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
    sub_ = it_in_->subscribeCamera("image_raw", queue_size_, &CropDecimateNodelet::imageCb, this, hints);
  }
}

  virtual void onSubscriptionChanged(const image_transport::SingleSubscriberPublisher& topic)
  {
    size_t disparity_clients = disparity_publisher_.getNumSubscribers() + disparity_registered_publisher_.getNumSubscribers();
    size_t depth_clients = publisher_.getNumSubscribers() + depth_registered_publisher_.getNumSubscribers();
    size_t all_clients = disparity_clients + depth_clients;

    if(!running_ && all_clients > 0)
    {
      running_ = (stream_.start() == STATUS_OK);
    }
    else if(running_ && all_clients == 0)
    {
      stream_.stop();
      running_ = false;
    }

    if(running_)
    {
      updateActivePublisher();
    }
  }

  bool take_and_send_image()
  {
    usb_cam_camera_grab_image(camera_image_);
    fillImage(img_, "rgb8", camera_image_->height, camera_image_->width, 3 * camera_image_->width, camera_image_->image);
    img_.header.stamp = ros::Time::now();

    sensor_msgs::CameraInfoPtr ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
    ci->header.frame_id = img_.header.frame_id;
    ci->header.stamp = img_.header.stamp;

    image_pub_.publish(img_, *ci);
    return true;
  }