在学习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,结果如下:
更多推荐
Python和C++程序发布和订阅ROS话题
发布评论