Python和C++程序发布和订阅ROS话题

在学习ROS的过程中，发现基于ROS框架进行开发确实有很大的优势，可以把各个不同的模块统一起来，互相交换信息。相信很多人对如何编写ROS的功能包都很熟悉了。可是有的程序我们想它能够利用ROS的框架，可是又很难把它们编写成ROS包，我们能够怎么做呢？最近就遇到了这个问题，发现融入ROS框架并不需要一定是ROS的包，只需要包含ROS相关的库以及头文件即可，记录如下。

Python版本

在Python中编写代码时可以直接import ROS相应的模板。

编写talker.py:

这个文件调用了opencv打开摄像头获取图像并发布出去。

#!/usr/bin/env python
# license removed for brevity
import rospy
from sensor_msgs.msg import Image
import cv2
from cv_bridge import CvBridge

def talker():
     pub = rospy.Publisher('/tutorial/image', Image, queue_size=1) 
     rospy.init_node('talker', anonymous=True) 
     rate = rospy.Rate(30)
     bridge = CvBridge()
     Video = cv2.VideoCapture(0)
     while not rospy.is_shutdown():
         ret, img = Video.read()
         cv2.imshow("talker", img)
         cv2.waitKey(3)
         pub.publish(bridge.cv2_to_imgmsg(img, "bgr8"))
         rate.sleep()

if __name__ == '__main__':
     try:
         talker()
     except rospy.ROSInterruptException:
         pass

编写listener.py:

这个文件订阅talker的话题并获取图像。

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image
import cv2
from cv_bridge import CvBridge

def callback(imgmsg):
    bridge = CvBridge()
    img = bridge.imgmsg_to_cv2(imgmsg, "bgr8")
    cv2.imshow("listener", img)
    cv2.waitKey(3)

def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("/tutorial/image", Image, callback)
    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()
if __name__ == '__main__':
    listener()

打开三个终端，运行roscore和这两个py文件就可以看到如下结果：

C++版本

编写c++的listener并生成可执行文件：

建立如下结构的文件系统：

├── CMakeLists.txt

└── src

    └── img_sub.cpp

其中CMakeLists.txt的内容如下：

cmake_minimum_required(VERSION 2.8.3)
project(img_sub)
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
find_package(catkin REQUIRED
OpenCV
cv_bridge
roscpp
sensor_msgs
image_transport
)
include_directories (include ${catkin_INCLUDE_DIRS}
)
set(CPP_SOURCES src/img_sub.cpp)
add_executable(img_sub ${CPP_SOURCES})
target_link_libraries(img_sub 
    ${OpenCV_LIBS}
    ${catkin_LIBRARIES}
)

img_sub.cpp的内容如下：

#include <opencv2/opencv.hpp>
#include <signal.h>
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
using namespace cv;
using namespace std;
Mat img;
struct Flag{
    bool shutdown = false;
    bool img = false;
} flag;
void img_sub(const sensor_msgs::Image& msg_img); //func to get img
void siginthd_kb(int sig) //func to shutdown this node
{
    cout << "going to shudown!" << endl;
    flag.shutdown = true;
}
int main(int argc, char** argv){
    ros::init(argc, argv, "listener_C", ros::init_options::NoSigintHandler);
    ros::NodeHandle nh;
    signal(SIGINT, siginthd_kb);
    ros::Subscriber img_rcv = nh.subscribe("/tutorial/image", 10, &img_sub);
    char key = ' ';
    cout << "press Esc to exit.\n";
    while (key != 27 && flag.shutdown == false)
    {
        if (flag.img)
        {
            imshow("img", img);
            flag.img = false;
        }
        else
            cout << "img not ready!\n";
        key = waitKey(3);
        ros::spinOnce();
    }
   return 0;
}//end for main
void img_sub(const sensor_msgs::Image &msg_img)
{
    cv_bridge::CvImagePtr cv_ptr;
    cv_ptr = cv_bridge::toCvCopy(msg_img, "bgr8");
    img = cv_ptr->image;
    flag.img = true;
}

在根目录下建立cbuild文件夹，进入该目录

cmake ..
make

就可以得到img_sub可执行程序，运行roscore，Python的talker以及img_sub,结果如下: