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

void callback(const sensor_msgs::ImageConstPtr &imgPtr){
    cvImageFromROS =cv_bridge::toCvCopy(imgPtr);
    diff =cv_bridge::toCvCopy(imgPtr);

    cvImageFromROS->image.copyTo(image);
    diff->image.copyTo(DiffImage);
    out_msg.header   = imgPtr->header;
    out_msg.encoding = "mono16";


    int cols=image.cols;
    int rows=image.rows;
    ushort v;
    cv::Point p;
    for(int i=0;i<cols;i++){
        for(int j=0;j<rows;j++){
            p.x=i;
            p.y=j;
            v=((float)image.at<ushort>(p));
            v*=multiplier->cell(p.y,p.x,v);
            image.at<ushort>(p)=(ushort)v;
        }
    }

    out_msg.image    =  image;
    pub.publish(out_msg.toImageMsg());
    out_msg.image    =  image-DiffImage;
    pub2.publish(out_msg.toImageMsg());
    //std::cout<<"global diff: "<< cv::sum(out_msg.image)/(out_msg.image.rows*out_msg.image.cols)<<std::endl;

}

      bool capture(int camNumber)
      {
         if (camNumber > numWebcams_ || camNumber < 0)
         {
         	cout << "invalid camera number requested for capture in webcams, camera number = " << camNumber << endl;
         	return false;
         }
         char filename[80];
         sprintf(filename, "webcam%d", camNumber);
         Mat image;
         if (camera_[camNumber].capture(&image))
         {
            std::string camType = "webcam";
            std_msgs::Header imgHeader;
            imgHeader.seq = webcamTilt_;
            imgHeader.frame_id = camType;
            imgHeader.stamp = ros::Time::now();
            sensor_msgs::ImagePtr msg = cv_bridge::CvImage(imgHeader, "bgr8", image).toImageMsg();
            //sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
            if (cap_home_)
		   	{
		   		home_image_pub_.publish(msg);  // home target image
		   		ROS_INFO("webcams published a home image");
		   		return true;
		   	}
		   	else 
		   	{
		   		image_pub_.publish(msg);   // publish the image
		   		ROS_INFO("webcams published a target image");
      			return true;
      		}
      	}
      	return false;
      }

/// \brief Main callback which takes care of republishing.
///
/// This callback is called by the Synchronizer filter when
/// messages for both left/right images and cameras information
/// have been collected.
///
/// The callback the left image timestamps in all the other messages.
///
/// \param pub_l left image publisher (binded)
/// \param pub_cam_l left camera information publisher (binded)
/// \param pub_r right image publisher (binded)
/// \param pub_cam_r right camera information publisher (binded)
/// \param left left image
/// \param left_camera left camera information
/// \param right right image
/// \param right_camera right camera information
void imageCallback(image_transport::Publisher& pub_l,
                   ros::Publisher& pub_cam_l,
                   image_transport::Publisher& pub_r,
                   ros::Publisher& pub_cam_r,
                   const sensor_msgs::ImageConstPtr& left,
                   const sensor_msgs::CameraInfoConstPtr& left_camera,
                   const sensor_msgs::ImageConstPtr& right,
                   const sensor_msgs::CameraInfoConstPtr& right_camera
                  )
{
    sensor_msgs::Image left_(*left);
    sensor_msgs::Image right_(*right);
    sensor_msgs::CameraInfo left_camera_(*left_camera);
    sensor_msgs::CameraInfo right_camera_(*right_camera);

    // Fix timestamps.
    // FIXME: a warning should be issued if the differences between
    // timestamps are too important.
    left_camera_.header.stamp = left_.header.stamp;
    right_camera_.header.stamp = left_.header.stamp;
    right_.header.stamp = left_.header.stamp;

    pub_l.publish(left_);
    pub_cam_l.publish(left_camera_);
    pub_r.publish(right_);
    pub_cam_r.publish(right_camera_);
}

        void callback(const sensor_msgs::ImageConstPtr& msg) {
            // First extract the image to a cv:Mat structure, from opencv2
            cv::Mat img(cv_bridge::toCvShare(msg,"mono8")->image);
            try{
                // Then receive the transformation between the robot body and
                // the world
                tf::StampedTransform transform;
                // Use the listener to find where we are. Check the
                // tutorials... (note: the arguments of the 2 functions below
                // need to be completed
                listener_.waitForTransform("","",ros::Time::now(),ros::Duration(1.0));
                listener_.lookupTransform("","", ros::Time::now(), transform);
                

                double proj_x = transform.getOrigin().x();
                double proj_y = transform.getOrigin().y();
                double proj_theta = -tf::getYaw(transform.getRotation());
                printf("We were at %.2f %.2f theta %.2f\n",proj_x,proj_y,proj_theta*180./M_PI);

                // Once the transformation is know, you can use it to find the
                // affine transform mapping the local floor to the global floor
                // and use cv::warpAffine to fill p_floor
                cv::Mat_<uint8_t> p_floor(floor_size_pix,floor_size_pix,0xFF);
                cv::Mat affine = (cv::Mat_<float>(2,3) 
                        << 1, 0, 0,   
                           0, 1, 0);
                cv::warpAffine(img,p_floor,affine, p_floor.size(), 
                        cv::INTER_NEAREST,cv::BORDER_CONSTANT,0xFF);

                // This published the projected floor on the p_floor topic
                cv_bridge::CvImage pbr(msg->header,"mono8",p_floor);
                ptpub_.publish(pbr.toImageMsg());

                // Now that p_floor and floor have the same size, you can use
                // the following for loop to go through all the pixels and fuse
                // the current observation with the previous one.
                for (unsigned int j=0;j<(unsigned)floor_.rows;j++) {
                    for (unsigned int i=0;i<(unsigned)floor_.cols;i++) {
                        uint8_t p = p_floor.at<uint8_t>(i,j);
                        uint8_t f = floor_.at<uint8_t>(i,j);
                        
                        // Compute the new value of pixel i,j here

                        floor_.at<uint8_t>(i,j) = f;
                    }
                }
                // Finally publish the floor estimation.
                cv_bridge::CvImage br(msg->header,"mono8",floor_);
                tpub_.publish(br.toImageMsg());
            }
            catch (tf::TransformException ex){
                ROS_ERROR("%s",ex.what());
            }
        }

//callback method for operations that require a depth image
void depthCallback(const sensor_msgs::ImageConstPtr &msg)
{
  //Load image into OpenCV
  bridge_image_depth = cv_bridge::toCvCopy(msg, msg->encoding);
  std::cout << "bridge_image created" << std::endl;
  cvImage_depth = bridge_image_depth->image;
  std::cout << "cvImage created" << std::endl;
  
  //set cv_bridge image matrix to output and publish
  getDepthThresholdImage(cvImage_depth, depthThresholdOut);
  std::cout << "got depthThresholdImage" << std::endl;
  bridge_image_depth->image = depthThresholdOut;
  std::cout << "bridge_image->image = depthThresholdOut" << std::endl;
  depthThresholdPub.publish(bridge_image_depth->toImageMsg());
  //printImagePixels(depthThresholdOut, "output depth threshold");
  //std::cout << depthThresholdOut << "\n*****\n" << std::endl;
  
  //cv::Mat temp;
  //depthThresholdOut.copyTo(temp);
  
  //printImageData(depthThresholdOut, "depthThesholdOut");
  
  if (lastImage.rows > 0) {
    getImageDifference(cvImage_depth, lastImage);
    bridge_image_depth->image = lastImage;
    motionDetectPub.publish(bridge_image_depth->toImageMsg());
  } else {
    std::cout << "lastImage hasn't been created yet" << std::endl;
  }
  
  lastImage = cvImage_depth;
  
/*
  cv::Mat depthThresholdBWOut;
  //getDepthThresholdBWImage(depthThresholdOut, depthThresholdBWOut);
  //printImageData(depthThresholdBWOut,"depthBW");
  //depthThresholdBWOut = depthThresholdOut;
  bridge_image->image = depthThresholdBWOut;
  depthThresholdBWPub.publish(bridge_image->toImageMsg());
*/

  //delete &depthThresholdOut;
  //delete &cvImage;

/*cvReleaseMat(&cvImage);
cvReleaseMat(&depthThresholdOut); 
*/
}

void faceDetect(){
  //Detect faces
   cv::cvtColor( frame, frame_gray, CV_BGR2GRAY );
   cv::equalizeHist( frame_gray, frame_gray );
   face_cascade.detectMultiScale( frame_gray, faces, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) );
  
   //for each face draws an ellipse arround and look for the red color at a distance from 
   for( i = 0; i < faces.size(); i++ )
    {
      cv::Point center( faces[i].x + faces[i].width*0.5, faces[i].y + faces[i].height*0.5 );
      cv::ellipse( frame, center, cv::Size( faces[i].width*0.5, faces[i].height*0.5), 0, 0, 360, cv::Scalar( 0, 255, 0 ), 2, 8, 0 );
      faceX = (float) faces[i].x;
      faceY = (float) faces[i].y;
     
      if( ((faceX + faceColorThresh) > (posX ) | (faceX - faceColorThresh) < (posX )) ) {
	face = true; 
	//publishing camera image
	out_msg.image    = frame; //frame
	out_msg.encoding = sensor_msgs::image_encodings::BGR8;
	msg = out_msg.toImageMsg();
	pub.publish(msg);
	ROS_FATAL("PERSON DETECTED");
	break;
      }
    }
}

  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    //int c, key, total_max;
    //Creates matrices available for filling later
  
    cv::Mat image;

    //CALL FACE REC HERE, PASS FORWARD
    //ESSENTIALLY, THIS IS MAIN


    // Draw an example circle on the video stream
    if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
      cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));

    // Update GUI Window
    cv::imshow(OPENCV_WINDOW, cv_ptr->image);
    cv::waitKey(3);
    
    // Output modified video stream
    image_pub_.publish(cv_ptr->toImageMsg());
  }

void stitcherCallback(const sensor_msgs::ImageConstPtr& original_image) {
    cv_bridge::CvImagePtr cv_ptr;
    try {
        cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
    } catch (cv_bridge::Exception& e) {
        ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what());
        return;
    }

    if (vImg.size() == stitcher_num_frames - 1) { 
        vImg.push_back(cv_ptr->image);
        
        cv::Stitcher stitcher = cv::Stitcher::createDefault();
        stitcher.stitch(vImg, cv_ptr->image);
        
        cv::imwrite("images/Panorama.jpg", cv_ptr->image);
        cv::imshow(WINDOW, cv_ptr->image);
        cv::waitKey(3);
        pub.publish(cv_ptr->toImageMsg());
        sleep(2);
        vImg.clear();
    } else {
        vImg.push_back(cv_ptr->image);
    }
}

    void imageCb(const sensor_msgs::ImageConstPtr& msg)
    {
        cv_bridge::CvImagePtr cv_ptr, gray_ptr;
        
        try
        {
            cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
            gray_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
        }

        if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
        {
           cvtColor(cv_ptr->image, gray_ptr->image, CV_BGR2GRAY);
        }    
        
        cv::imshow(WINDOW, gray_ptr->image);
        cv::waitKey(3);

        image_pub_.publish(cv_ptr->toImageMsg());
    }

void SobelDepthImage::sobelCallback( const ImageConstPtr& image ) {
	cv_bridge::CvImagePtr cv_ptr;
    try
    {
		cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::TYPE_32FC1);
		using namespace cv;
		Mat gauss, gradX, gradY, grad, thres;
		GaussianBlur(cv_ptr->image, gauss, Size(5,5), 0, 0, BORDER_DEFAULT);
		Sobel(gauss, gradX, CV_16S, 1, 0, 3, 1, 0, BORDER_DEFAULT);
		Sobel(gauss, gradY, CV_16S, 0, 1, 3, 1, 0, BORDER_DEFAULT);
		gradX = abs(gradX);
		gradY = abs(gradY);
		bitwise_or(gradX, gradY, grad);
		grad.convertTo(thres, CV_32FC1);
		double dynThres = 0.25*mean(grad).val[0];
		std::cout << dynThres << std::endl;
		threshold(thres, cv_ptr->image, dynThres , 1.0 , 0);
		
		//~ imshow(OPENCV_WINDOW, cv_ptr->image);
		//~ waitKey(3);
		
		out.publish(cv_ptr->toImageMsg());
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
}

	void imageCb(const sensor_msgs::ImageConstPtr& msg)
	{
		static int count = 0;
		cv_bridge::CvImagePtr cv_ptr;
		try
		{
			cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
		}
		catch (cv_bridge::Exception& e)
		{
			ROS_ERROR("cv_bridge exception: %s", e.what());
			return;
		}
		
		cv::Mat img = cv_ptr->image;
		int pos[2];
		search(img, pos);
		if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60 && pos[0] != -1 && pos[1] != -1)
			cv::circle(cv_ptr->image, cv::Point(pos[0], pos[1]), 10, CV_RGB(0, 0, 255));
		else {
			printf("not found %d\n", count);
			count++;
		}

		cv::imshow(OPENCV_WINDOW, cv_ptr->image);
		cv::waitKey(3);

		image_pub_.publish(cv_ptr->toImageMsg());
	}

// TEMP code to show output of frames
void IirImage::pubImage(const ros::TimerEvent& e)
{
  if (!dirty_)
    return;

  cv::Mat out_frame;

  for (size_t i = 0; i < in_frames_.size() && i < b_coeffs_.size(); ++i)
  {
    const double bn = b_coeffs_[i];
    if (i == 0)
      out_frame = in_frames_[i] * bn;
    else if ((out_frame.size() == in_frames_[i].size()) &&
             (out_frame.type() == in_frames_[i].type()))
    {
      if (bn > 0)
        out_frame += in_frames_[i] * bn;
      else
        out_frame -= in_frames_[i] * -bn;
    }
  }

  {
    // TODO(lucasw) this may be argument for keeping original Image messages around
    cv_bridge::CvImage cv_image;
    cv_image.header.stamp = ros::Time::now();  // or reception time of original message?
    cv_image.image = out_frame;
    cv_image.encoding = "rgb8";
    image_pub_.publish(cv_image.toImageMsg());
  }

  dirty_ = false;
}

    void imageCb(const sensor_msgs::ImageConstPtr& msg)
     {
       cv_bridge::CvImagePtr cv_ptr;
       try
       {
         cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
      }
       catch (cv_bridge::Exception& e)
       {
         ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
       }
       
   //imshow("pashmak",cv_ptr->image);
     cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(), 1, 1);
       //imshow("input", cv_ptr->image);
       
       cv::Mat rotated,rotated1,rotated2;
      
  
    rotate(cv_ptr->image, 180, rotated);
    rotate(cv_ptr->image, 90, rotated1);
    rotate(cv_ptr->image, 270, rotated2);
 imshow("rotate",rotated);
  
   
       // Update GUI Window
       //cv::imshow(OPENCV_WINDOW, cv_ptr->image);
       cv::waitKey(3);
       
       // Output modified video stream
       image_pub_.publish(cv_ptr->toImageMsg());
     }

	void imageCb(const sensor_msgs::ImageConstPtr& msg)
	{
		cv_bridge::CvImagePtr cv_ptr;
		try
		{
			cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
		}
		catch (cv_bridge::Exception& e)
		{
			ROS_ERROR("cv_bridge exception: %s", e.what());
			return;
		}
		
		cv::Mat img = cv_ptr->image;
		int pos[2];
		search(img, pos);
		if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60 && pos[0] != -1 && pos[1] != -1) {
			cv::circle(cv_ptr->image, cv::Point(pos[0], pos[1]), 10, CV_RGB(0, 0, 255), 3);
			printf("found\n");
			difx = img.cols/2 - pos[0];
			dify = img.rows/2 - pos[1];
		} else printf("not found\n");
		cv::imshow("image1", img);
		cv::waitKey(3);

		image_pub_.publish(cv_ptr->toImageMsg());
	}

  void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
    cv_bridge::CvImagePtr cv_ptr;

    try {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    } catch (cv_bridge::Exception& e) {
      JSK_ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    cv::Mat cv_img = cv_ptr->image;
    cv::Mat cv_og_img;
    cv::Mat cv_out_img;

    // calcOrientedGradient() will write oriented gradient to
    // 2nd arg image as HSV format,
    // H is orientation and V is intensity
    calcOrientedGradient(cv_img, cv_og_img);
    cv::cvtColor(cv_og_img, cv_out_img, CV_HSV2BGR);
    // cv::imshow("OrinetedGradient", cv_out_img);
    // cv::waitKey(1);
    sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(
      msg->header,
      sensor_msgs::image_encodings::BGR8,
      cv_out_img).toImageMsg();
    image_pub_.publish(out_img);
  }