视觉里程计学习(一) 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 
#include 
#include 
#include 
#include 

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>

小结
调通之后,我们就可以顺利地得到图像信息了,之后的一切视觉里程计都会在这一基础上进行开发.