利用zeroMQ 把c++ 從baxter 機器人獲取的CV Mat圖像數據發給python

C++發佈者

#include<baxter_xiang.hpp>
namespace cv
{
   using std::vector;
}

//進行初始化
    ImageConverter::ImageConverter(std::string topicname )
    {
          init(0,nullptr,topicname);
            void* context = zmq_ctx_new();
            assert(context!=NULL);
            publisher = zmq_socket(context,ZMQ_PUB);
            zmq_bind(publisher,"tcp://*:5556");
    }

void ImageConverter::run()
{
         ros::NodeHandle nh_ = shetNh();
        sub =  nh_.subscribe("/cameras/head_camera/image", 1000, &ImageConverter::imageCb,this);
        cv::namedWindow(OPENCV_WINDOW);
   
    while (ros::ok())
    {
        /* code for loop body */
        ros::spinOnce();      
    }   
}


//圖像訂閱的回調函數
void ImageConverter::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
    datatypeconversion data;
    cv_bridge::CvImagePtr cv_ptr;
 py::array_t<unsigned char> numpy1;
    try
    {
    //baxter機器人的圖像轉化成opencv圖像
        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;
    }

    
    std::cout<<"flags:"<<cv_ptr->image.flags<<std::endl;
    std::cout<<"size:"<<cv_ptr->image.size<<std::endl;
    std::cout<<"cols:"<<cv_ptr->image.cols<<std::endl;
    std::cout<<"dims:"<<cv_ptr->image.dims<<std::endl;
    std::cout<<"channels:"<<cv_ptr->image.channels()<<std::endl;
    auto sz = cv_ptr->image.size();
    int x = sz.width;
    int y = sz.height;
    int channels = cv_ptr->image.channels();
    int height = cv_ptr->image.rows;
    int width = cv_ptr->image.cols;
    cv::vector<uchar> buffer;
    cv::imencode(".jpg",cv_ptr->image,buffer);
       numpy1= data.mat_uint8_3c_to_numpy(cv_ptr->image);
       zmq_send(publisher,buffer.data(),buffer.size(),0);
       cv::imshow(OPENCV_WINDOW,cv_ptr->image);
       cv::waitKey(3);

}

void ImageConverter::run()
{
         ros::NodeHandle nh_ = shetNh();
        sub =  nh_.subscribe("/cameras/head_camera/image", 1000, &ImageConverter::imageCb,this);
        cv::namedWindow(OPENCV_WINDOW);
   
    while (ros::ok())
    {
        /* code for loop body */
        ros::spinOnce();
        
    }

python 接受者

# import baxter_xiang
import time
import zmq
import numpy
import io
import PIL
import cv2
import PIL.Image as Image
def staS():
    context = zmq.Context()
    socket = context.socket(zmq.SUB)
    socket.connect("tcp://127.0.0.1:5556")
    socket.setsockopt(zmq.SUBSCRIBE, b"")
    # cv2.nameWindow("Image")
    while True:
        message = socket.recv()
        print("Received request: %s" % message)
        image = numpy.array(Image.open(io.BytesIO(message)))
        image = image[...,::-1]   #把RGB轉爲GBR
        print(image.shape)
        
        cv2.imwrite("hahha.jpg",image)
if __name__ == '__main__':
    staS()

參考