Ardrone2 ROS Image和OpenCV Image相互转化

本文主要介绍在ar drone2四旋翼飞行器上，基于ROS，使用cv_bridge将ROS Image和OpenCV Image相互转化，编写简单的Publisher和Sublisher程序，把结果图像显示出来。

开发平台：AR drone2  ubuntu14.04  ROS indigo

ROS Image messages 和OpenCV Mat相互转化可参考  

http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages

ardrone_autonomy使用手册

http://ardrone-autonomy.readthedocs.io/en/latest/index.html

image_transport example

http://wiki.ros.org/image_transport/Tutorials

Step 1：创建一个工作空间dronework，然后利用catkin_create_pkg创建dronevideo package，dronevideopackage开发包依赖于cv_bridge image_transport sensor_msgs roscpp std_msgs

mkdir -p /root/dronework/src

cd /root/dronework/src

source /opt/ros/indigo/setup.bash

catkin_create_pkg dronevideo cv_bridge image_transport sensor_msgs roscpp std_msgs

cd /root/dronework

catkin_make

在root目录下.bashrc文件中添加  

source /opt/ros/indigo/setup.bash

source /root/dronework/devel/setup.bash

这样可以避免每次打开一个新的终端，需要source对应的setup.bash

Step 2：在dronevideo package的src目录下添加dronevideo_pub.cpp

#include <ros/ros.h>#include <image_transport/image_transport.h>#include <cv_bridge/cv_bridge.h>#include <sensor_msgs/image_encodings.h>#include <opencv2/imgproc/imgproc.hpp>#include <opencv2/highgui/highgui.hpp>using namespace std;using namespace cv;static const string OPENCV_WINDOW = "Image window";image_transport::Subscriber image_sub_;image_transport::Publisher image_pub_;  void imageCb(const sensor_msgs::ImageConstPtr& msg){  cv_bridge::CvImagePtr cv_ptr;  try  { cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");  }  catch (cv_bridge::Exception& e)  { ROS_ERROR("cv_bridge exception: %s", e.what()); return;  }  Mat img_rgb,img_gray;  img_rgb = cv_ptr->image;  cvtColor(img_rgb,img_gray,CV_RGB2GRAY);  // Update GUI Window  imshow(OPENCV_WINDOW, img_gray);  waitKey(3);    // Output modified video stream  sensor_msgs::ImagePtr msg_pub;  msg_pub = cv_bridge::CvImage(std_msgs::Header(), "mono8", img_gray).toImageMsg();  image_pub_.publish(msg_pub);}int main(int argc, char** argv){  ros::init(argc, argv, "dronevideo_pub");    ros::NodeHandle nh_;  image_transport::ImageTransport it_(nh_);  // Subscrive to input video feed and publish output video feed  image_sub_ = it_.subscribe("/ardrone/image_raw", 1, imageCb);  image_pub_ = it_.advertise("/image_converter/output_video", 1);  namedWindow(OPENCV_WINDOW);  ros::spin();    destroyWindow(OPENCV_WINDOW);  return 0;}

运行ardrone_autonomy  ardrone_driver可以产生/ardrone/image_raw，通过订阅该话题可以获取ar drone2摄像头 ROS Image message

/image_converter/output_video话题是为了把转换后的灰度图像message发布出去。

 

toCvCopy toCvShare toImageMsg关键函数

Step 3：在dronevideo package的src目录下添加dronevideo_sub.cpp

#include <ros/ros.h>#include <cv_bridge/cv_bridge.h>#include <sensor_msgs/image_encodings.h>#include <image_transport/image_transport.h>#include <opencv2/imgproc/imgproc.hpp>#include <opencv2/highgui/highgui.hpp>using namespace std;using namespace cv;void imageCallback(const sensor_msgs::ImageConstPtr& msg){  try  {    imshow("view", cv_bridge::toCvShare(msg, "mono8")->image);    waitKey(30);   //30ms  }  catch (cv_bridge::Exception& e)  {    ROS_ERROR("Could not convert from '%s' to 'mono8'.", msg->encoding.c_str());  }}int main(int argc, char **argv){  ros::init(argc, argv, "dronevideo_sub");  ros::NodeHandle nh_;  cv::namedWindow("view");  cv::startWindowThread();  image_transport::ImageTransport it_(nh_);  image_transport::Subscriber sub = it_.subscribe("/image_converter/output_video", 1, imageCallback);  ros::spin();  cv::destroyWindow("view");}

Step 4：修改package.xml

<?xml version="1.0"?><package>  <name>dronevideo</name>  <version>0.0.0</version>  <description>The dronevideo package</description>  <maintainer email="[e-mail]">root</maintainer>  <license>TODO</license>  <buildtool_depend>catkin</buildtool_depend>    <build_depend>cv_bridge</build_depend>  <build_depend>image_transport</build_depend>  <build_depend>sensor_msgs</build_depend>  <build_depend>message_generation</build_depend>  <build_depend>opencv2</build_depend>  <run_depend>cv_bridge</run_depend>  <run_depend>image_transport</run_depend>  <run_depend>sensor_msgs</run_depend>  <run_depend>message_runtime</run_depend>  <run_depend>opencv2</run_depend></package>

添加  opencv2 message_generationmessage_runtime依赖项

Step 5：修改CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)project(dronevideo)find_package(catkin REQUIRED COMPONENTS roscppstd_msgscv_bridge image_transport sensor_msgs genmsg)#generate_messages(DEPENDENCIES sensor_msgs)catkin_package()find_package(OpenCV)include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})add_executable(dronevideo_pub src/dronevideo_pub.cpp)target_link_libraries(dronevideo_pub ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})add_dependencies(dronevideo_pub dronevideo_generate_messages_cpp)add_executable(dronevideo_sub src/dronevideo_sub.cpp)target_link_libraries(dronevideo_sub ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})add_dependencies(dronevideo_sub dronevideo_generate_messages_cpp)

主要注意要包含OpenCV依赖项，然后Build Targets部分分别创建dronevideo_pub和dronevideo_sub节点。

Step 6：cmake & run

cd /root/dronework

catkin_make

 

First one terminal ：         roscore

Next another terminal：  rosrun ardrone_autonomy ardrone_driver

And then a terminal：    rosrun dronevideo dronevideo_pub

Finally the last one terminal：  rosrun dronevideo dronevideo_sub

 

最后效果图

出现问题：

当分别运行

# 200Hz real-time update$ rosrun ardrone_autonomy ardrone_driver _realtime_navdata:=True _navdata_demo:=0# 15Hz real-rime update$ rosrun ardrone_autonomy ardrone_driver _realtime_navdata:=True _navdata_demo:=1

pub和sub节点实现图像偶尔会出现卡顿，难道navdata 更新频率会对Image message 有影响，后面再详细研究ardrone_autonomy Parameter。