ROS——C++与Python3的图像节点基于OpenCV相互发送和接收

文章目录

• • C++与Python相互传输的数据格式
  • 读取摄像头并发布
  • • Python读取摄像头
    • C++读取摄像头
  • 接收节点
  • • Python接收图像
    • C++接收图像

C++与Python相互传输的数据格式

• 基于ROS利用客户端和服务端实现C++节点和python节点间传送图像的数据格式

读取摄像头并发布

需要注意的是，如果要使用C++节点接收Python的图像格式，那么Python和部分必须都统一使用opnecv和cv_bridge进行图像的转换。

Python读取摄像头

#!/usr/bin/env python3
# coding:utf-8

import cv2
import numpy as np
import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image
from cv_bridge import CvBridge , CvBridgeError
import time

if __name__=="__main__":
    import sys 
    print(sys.version) # 查看python版本
    capture = cv2.VideoCapture(0) # 定义摄像头
    bridge=CvBridge()
    rospy.init_node('camera_node', anonymous=True) #定义节点
    image_pub=rospy.Publisher('/image_view/image_raw', Image, queue_size = 1) #定义话题
    
    while not rospy.is_shutdown():    # Ctrl C正常退出，如果异常退出会报错device busy！
        ret, frame = capture.read()
        if ret: # 如果有画面再执行
            image_pub.publish(bridge.cv2_to_imgmsg(image, "bgr8")) # 发布消息（只需要一次转换即可！）
            rate = rospy.Rate(25) # 10hz 
    capture.release()
    cv2.destroyAllWindows() 
    print("quit successfully!")

C++读取摄像头

#include 
#include 
#include 
#include 
#include 
#include 
 
int main(int argc, char** argv)
{
  ros::init(argc, argv, "img_publisher");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub = it.advertise("/camera/color/image_raw", 1);
 
  cv::VideoCapture cap;
  cv::Mat frame;
  int deviceID=0;
  if(argc>1)
	deviceID=argv[1][0]-'0';
  int apiID=cv::CAP_ANY;
  cap.open(deviceID+apiID);
  if(!cap.isOpened()){
	std::cerr<<"ERROR! Unable to open camera"<<std::endl;
	return -1;
  }
 
  ros::Rate loop_rate(30);
  while (nh.ok()) {
	cap.read(frame);
	if(!frame.empty()){
		sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
		pub.publish(msg);
	}
    	ros::spinOnce();
    	loop_rate.sleep();
  }
  return 0;
}

接收节点 Python接收图像

#!/usr/bin/env python3
# coding:utf-8

import numpy as np
import cv2

import rospy
from std_msgs.msg import Header
from cv_bridge import CvBridge 
from sensor_msgs.msg import Image

import sys 
print(sys.version) # 查看python版本

def callback(data):
    img = bridge.imgmsg_to_cv2(data,'bgr8')  #转换格式

if __name__ == '__main__':
    bridge=CvBridge()
    rospy.init_node('img_sub_node', anonymous=True) #定义节点   
    rospy.Subscriber('/rgb_camera/image_raw', Image, callback) #定义接收话题    
    rospy.spin()

C++接收图像

#include 
#include 
#include 
#include 

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
  ROS_INFO_STREAM("Get Msg");
  try
  {
    cv::Mat image = cv_bridge::toCvShare(msg, "bgr8") -> image;
    cv::imshow("view", image);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
  cv::waitKey(1);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "image_listener");
  ros::NodeHandle nh;
  cv::namedWindow("view");
  cv::startWindowThread();
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub = it.subscribe("/rgb_camera/image_seg", 1, imageCallback);
  ros::spin();
  cv::destroyWindow("view");
}

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  roscpp
  OpenCV
  cv_bridge
  image_transport
)

find_package(OpenCV REQUIRED)
find_package(Boost REQUIRED)

catkin_package()

include_directories(
  include 
  
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

add_executable(image_pub src/image_pub.cpp)
target_link_libraries(image_pub ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES})

add_executable(image_sub  src/image_sub.cpp)
target_link_libraries(image_sub  ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES})