ROS订阅两个图像节点

Posted 2020-10-28 kekeoutlook

双目相机左右视图获取

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sstream> // for converting the command line parameter to integer

char command;
cv::Mat left,right;

void imageCallbackLeft(const sensor_msgs::ImageConstPtr &msg)
{
   try
   {
       left = cv_bridge::toCvShare(msg,"bgr8")->image;
       cv::imshow("left",left );
       command=cv::waitKey(1);
       if(command == ‘ ‘)
       {
	 cv::imwrite("left.jpg",left);
         cv::imwrite("right.jpg",right);
       }
   }
   catch(cv_bridge::Exception &e)
   {
    
   }
}

void imageCallbackRight(const sensor_msgs::ImageConstPtr &msg)
{
   try
   {
       right =  cv_bridge::toCvShare(msg,"bgr8")->image;
       cv::imshow("right",right);
       //cv::waitKey(1);
   }
   catch(cv_bridge::Exception &e)
   {
    
   }
}

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

  ros::init(argc, argv, "video_publish");
  ros::NodeHandle nh("~");
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub_left = it.subscribe("/left/image_rect_color", 1,imageCallbackLeft);
  image_transport::Subscriber sub_right = it.subscribe("/right/image_rect_color", 1,imageCallbackRight);

 ros::spin();

     
}

　　

ROS发布图像

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sstream> // for converting the command line parameter to integer

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

  ros::init(argc, argv, "video_publish");
  ros::NodeHandle nh("~");
  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub = it.advertise("/usb_cam/image_raw", 1);

  std::string device_name;
  //nh.param<std::string>("device_name",device_name,"");

 // nh.param<std::string>( "device_name", device_name, device_name );
    //std::cout << "device_name = "<< device_name  << std::endl;
  cv::VideoCapture cap(0);
  // Check if video device can be opened with the given index
  //cap.set(CV_CAP_PROP_FRAME_WIDTH,1280);
  //cap.set(CV_CAP_PROP_FRAME_HEIGHT,720);
  
if(!cap.isOpened())
  {
    std::cout << "can‘t open video stream..." << std::endl;
     return 1;
  }
  cv::Mat frame;
  sensor_msgs::ImagePtr msg;

  ros::Rate loop_rate(25);


  
  //cv::namedWindow("src",0);
  //cv::resizeWindow("src",640,480);

  int count = 0;
  char frame_count[256];

  while (nh.ok()) {
    cap >> frame;

    resize(frame,frame,cv::Size(1280,720));
    // Check if grabbed frame is actually full with some content
    if(!frame.data) {
     cap.open(device_name);
     continue;
     //break;
    }
    msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
    pub.publish(msg);

    //sprintf(frame_count,"frame # %d",count++);
   // cv::putText(frame,frame_count,cv::Point(20,20),cv::FONT_HERSHEY_SIMPLEX,1,cv::Scalar(255,0,255),4,8);
    cv::imshow("src",frame);
    cv::waitKey(1);
    
    ros::spinOnce();
    loop_rate.sleep();
  }
   cv::destroyAllWindows();
   pub.shutdown();
   return 0;
}