視覺里程計學習(一) ROS實時讀取並顯示攝像頭圖像

任務描述

---

• test_image_publisher.cpp : 從計算機攝像頭讀取圖像併發布圖像信息”camera/image”

• test_image_subscriber.cpp : 讀取publisher發佈的圖像信息

---

test_image_publisher.cpp

---

一. 代碼結構分析

1. 初始化

ros::init(argc, argv, "image_publisher");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
image_transport::Publisher pub = it.advertise("camera/image", 1);

初始化節點,設置發送器.這裏要發送一個名叫"camera/image"的消息到公共空間("\"目錄)

1. 從攝像頭讀取圖像

cv::VideoCapture cap(0);

得到一個攝像頭句柄,接下來靠

cap >> frame;

直接得到圖片.
3. 發佈圖像消息到公共空間

pub.publish(msg);  

二. 完整代碼

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sstream>

int main(int argc, char** argv) {


    ros::init(argc, argv, "image_publisher");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher pub = it.advertise("camera/image", 1);

    cv::VideoCapture cap(0);
    if (!cap.isOpened()) {
    ROS_INFO("cannot open video device\n");
    return 1;
    }

    cv::Mat frame;
    sensor_msgs::ImagePtr msg;

    ros::Rate loop_rate(10);//以10ms間隔發送圖片
    while (nh.ok()) {
        cap >> frame;  

        if (!frame.empty()) {  
            msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();  
            pub.publish(msg);  
        }
            ROS_INFO("runnning!");
        ros::spinOnce();  
        loop_rate.sleep();//與ros::Rate loop_rate相對應,休息10ms
    }
}

---

test_image_subscriber.cpp

---

一. 代碼結構分析

1. 主程序初始化

ros::init(argc, argv, "image_subscriber");  
ros::NodeHandle nh;  
//ros::NodeHandle nh_private("~");  
cv::namedWindow("view", CV_WINDOW_NORMAL);   
cv::startWindowThread();  

初始化ros系統,創建一個node節點,設置名字.然後打開一個圖像顯示窗口,並單獨安排一個線程顯示圖像
2. 設置接聽器subscriber,若接收到對應名字的message直接跳到響應函數imageCallback

image_transport::ImageTransport it(nh);  
image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);

通過node句柄nh創建一個接聽器.由於nh是公共節點,因此它接收所有同是在公共節點發布的信息(空間範圍爲”/”).可以注意到nh_private是一個私有節點,因此它只能接收特定空間範圍發佈的信息(“/image_subscriber”),關於私有和公有節點的具體討論可以參考這篇文章[【ROS學習】（七）ROS參數服務（1）].(http://blog.csdn.net/wengge987/article/details/50620121)

1. 等待message的到來

ros::spin();  

從這時起,程序就一直在等待着圖像信息的到來.

二. 完整程序(cpp, launch, CMakeLists.txt)

//test_image_subscriber.cpp
#include <ros/ros.h>  
#include <image_transport/image_transport.h>  
#include <opencv2/highgui/highgui.hpp>  
#include <cv_bridge/cv_bridge.h>  

void imageCallback(const sensor_msgs::ImageConstPtr& msg)  
{  

  cv_bridge::CvImagePtr cv_ptr;  
  try  
  {  
     cv_ptr = cv_bridge::toCvCopy(msg, "mono8");  
  }  
  catch (cv_bridge::Exception& e)  
  {  
     ROS_ERROR("cv_bridge exception: %s", e.what());  
     return;  
  }  

  cv::Mat img_rgb;  
  img_rgb = cv_ptr->image;  

  cv::imshow("view", img_rgb);  
  // fail if don't have waitKey(3).
  cv::waitKey(3);
}  

int main(int argc, char **argv)  
{  
  ros::init(argc, argv, "image_subscriber");  
  ros::NodeHandle nh;  
  cv::namedWindow("view", CV_WINDOW_NORMAL);   
  cv::startWindowThread();  
  image_transport::ImageTransport it(nh);  
  image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);
  ros::spin();  
  cv::destroyWindow("view");  
  return 0;
} 

CMakeLists關鍵部分

cmake_minimum_required(VERSION 2.8.3)
project(my_vio)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  sensor_msgs
  image_transport 
  cv_bridge
)

find_package(OpenCV REQUIRED)

include_directories(
  include
  include/my_vio
  ${catkin_INCLUDE_DIRS}
 # "/usr/include/eigen3"
)

catkin_package()

# add the publisher example  

add_executable(test_image_subscriber src/test_image_subscriber.cpp)  
target_link_libraries(test_image_subscriber ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})  

add_executable(test_image_publisher src/test_image_publisher.cpp)  
target_link_libraries(test_image_publisher ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})  

launch文件

<launch>
    <node name="image_publisher" pkg="my_vio" type="test_image_publisher" output="screen"/>
    <node name="image_subscriber" pkg="my_vio" type="test_image_subscriber" output="screen"/>
</launch>

小結
調通之後,我們就可以順利地得到圖像信息了,之後的一切視覺里程計都會在這一基礎上進行開發.