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 
#include 
#include 
#include 
#include 
#include 

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 
#include 
#include 
#include 
#include 
#include 

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

  dronevideo
  0.0.0
  The dronevideo package
  root
  TODO

  catkin
  
  cv_bridge
  image_transport
  sensor_msgs
  message_generation
  opencv2

  cv_bridge
  image_transport
  sensor_msgs
  message_runtime
  opencv2

添加  opencv2 message_generationmessage_runtime依赖项

Step 5：修改CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(dronevideo)

find_package(catkin REQUIRED COMPONENTS 
roscpp
std_msgs
cv_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。