ROS学习笔记8

1.准备工作与基础知识
2.待实现功能描述
3.C++实现
- (1)receive_process节点代码
- (2)show_img节点代码
4.Python实现
5.参考资料

本篇博客主要介绍ROS的传感器使用——RGB相机以及对应的消息类型sensor_msgs/Image、ROS中用于和OpenCV数据交换的cv_bridge模块。

1.准备工作与基础知识

一般常见的USB摄像头在ROS中名称是USB video device class(UVC)，UVC相机。在ROS中，要想调用摄像头有多种方法，例如和之前一样从OpenCV来调用摄像头等。这里通过ROS的uvc_camera功能包实现，更多与相机有关的内容可参考官方文档。

(1)安装uvc camera功能包

终端输入如下命令即可安装。

apt-get install ros-kinetic-uvc-camera

(2)安装image相关功能包

终端输入如下命令即可安装。

apt-get install ros-kinetic-image-*
apt-get install ros-kinetic-rqt-image-view

(3)通过ROS调用摄像头

我们需要先启动roscore，然后在终端中输入rosrun uvc_camera uvc_camera_node，ROS就会自动帮我们打开摄像头了，如下。需要注意的是启动uvc_camera_node后并不会展示出图像来，而是这个节点创建了多个Topic，直接向它们发送数据，我们可以使用rostopic list查看，如下。可以看到很多个以/image_raw为根节点的Topic，而我们需要的就是根节点/image_raw上的数据。

(4)查看图像

知道了uvc_camera_node是在向Topic不断发送数据后就可以有多种办法查看数据了，如用Rviz或者自己写Subscriber订阅查看等等。这里介绍三种：

a.image_view节点

在终端中输入如下内容即可查看影像：

rosrun image_view image_view image:=/image_raw

最后一个image:=表示订阅的Topic名称，uvc_camera_node节点发布的原图数据就是/image_raw，直接填它就行。效果如下：

b.rqt_image_view节点

在终端中输入如下内容即可查看影像：

rqt_image_view image:=/image_raw

参数解释同上，效果如下。

c.Rviz

在终端中输入rviz打开软件，然后点击左下角的“Add”按钮，在“By display type”标签中选择“Image”，点击“OK”。然后在Image的属性里找到“Image Topic”，选择/image_raw即可，效果如下：其它与RGB相机有关的知识，如ROS自带的相机标定功能等可以参考《ROS Robot Programming》这本书的8.3节，写得很好，也有中文版。

2.待实现功能描述

我们希望利用ROS获取摄像头的视频流，并对视频流进行实时处理，最后将处理后的数据再发布给其它节点调用。

在这个场景下，我们首先需要启动uvc_camera_node这个节点来实时读取相机的数据，它会自动把数据发布到/image_raw这个Topic上。我们可以建一个叫camera_process的包，包含一个接收数据并处理的节点receive_process，然后将处理好的数据发布到/processed_img这个Topic上，最后由节点show_img接收/processed_img上的数据并展示出来。这里让节点同步收发消息采用的方法是全局变量法，没有使用类的办法，更多介绍多Topic收发的内容可参考这篇博客。

注意ROS中有自己定义的影像传输格式sensor_msgs/Image.h，所以要想和OpenCV进行数据交换，最简单的办法就是用ROS中的cv_bridge库，其中已经写好了很多类型转换函数方便调用。

3.C++实现

创建包时需要包含roscpp、sensor_msgs、std_msgs、cv_bridge以及OpenCV，最后别忘了link一下OpenCV的库。

(1)receive_process节点代码

# include<ros/ros.h>    // ROS头文件
# include<cv_bridge/cv_bridge.h>    // cv_bridge头文件，实现类型转换
# include<sensor_msgs/Image.h>  // ROS相机传感器数据格式
# include<opencv-3.3.1-dev/opencv2/opencv.hpp>  //OpenCV核心头文件

// 定义的全局变量
ros::Subscriber sub;
ros::Publisher pub;

// 对彩色图像进行灰度直方图均衡化，增强对比度
cv::Mat imgStretch(cv::Mat cv_img){
    cv::Mat img_hsv;
    cv::cvtColor(cv_img,img_hsv,cv::COLOR_BGR2HSV);
    std::vector<cv::Mat> hsv_bands;
    cv::split(img_hsv,hsv_bands);
    cv::equalizeHist(hsv_bands[2],hsv_bands[2]);
    cv::Mat equal_img;
    cv::merge(hsv_bands,equal_img);
    cv::cvtColor(equal_img,equal_img,cv::COLOR_HSV2BGR);
    return equal_img;
}

cv::Mat cvtImgMsg2Mat(const sensor_msgs::ImageConstPtr& msg){
    // 利用cv_bridge::toCvCopy函数将消息格式的图片转成CV格式
    cv_bridge::CvImagePtr cvImgPtr;
    cvImgPtr = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);
    return cvImgPtr->image;
}

sensor_msgs::ImagePtr cvtMat2ImgMsg(cv::Mat img){
    // 利用cv_bridge::CvImage函数将CV图像再次转成消息格式
    sensor_msgs::ImagePtr img_msg;
    img_msg = cv_bridge::CvImage(std_msgs::Header(),"bgr8",img).toImageMsg();
    return img_msg;
}

void callback(const sensor_msgs::ImageConstPtr& msg){
    // 调用函数，将msg格式img转成mat
    cv::Mat img = cvtImgMsg2Mat(msg);

    // 对转换后的数据进行处理
    cv::Mat equal_img = imgStretch(img);
    
    // 将处理后的数据重新转成msg格式
    sensor_msgs::ImagePtr img_msg = cvtMat2ImgMsg(equal_img);

    // 发送处理后的数据
    pub.publish(img_msg);
}

int main(int argc, char *argv[])
{
    // 初始化节点并指定名称
    ros::init(argc, argv, "receive_process");
    // 新建节点管理器
    ros::NodeHandle nh;

    // 初始化pub和sub
    sub = nh.subscribe("image_raw",1,callback);
    pub = nh.advertise<sensor_msgs::Image>("processed_img",1);
    
    // 循环执行
    ros::spin();
    return 0;
}

(2)show_img节点代码

# include<ros/ros.h>    // ROS头文件
# include<cv_bridge/cv_bridge.h>    // cv_bridge头文件，实现类型转换
# include<sensor_msgs/Image.h>  // ROS相机传感器数据格式
# include<opencv-3.3.1-dev/opencv2/opencv.hpp>  //OpenCV核心头文件

// 定义的全局变量
ros::Subscriber sub;

cv::Mat cvtImgMsg2Mat(const sensor_msgs::ImageConstPtr& msg){
    // 利用cv_bridge::toCvCopy函数将消息格式的图片转成CV格式
    cv_bridge::CvImagePtr cvImgPtr;
    cvImgPtr = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);
    return cvImgPtr->image;
}

void callback(const sensor_msgs::ImageConstPtr& msg){
    // 调用函数，将msg格式img转成mat
    cv::Mat img = cvtImgMsg2Mat(msg);
    
    // 展示图像
    cv::imshow("processed_img",img);
    cv::waitKey(3);
}

int main(int argc, char *argv[])
{
    // 初始化节点并指定名称
    ros::init(argc, argv, "show_img");
    // 新建节点管理器
    ros::NodeHandle nh;

    // 初始化pub和sub
    sub = nh.subscribe("processed_img",1,callback);
    
    // 循环执行
    ros::spin();
    return 0;
}

4.Python实现

相比于C++，新建包时只需要依赖rospy即可。

(1)receive_process代码

根据之前的习惯，src文件夹里一般放C/C++文件，Python代码一般放在scripts文件夹里。代码如下：

# coding=utf-8
import rospy  # ROS库
from sensor_msgs.msg import Image   # ROS中接收传感器的图片格式，要注意的是别漏了.msg，否则导入失败
import cv_bridge    # 用于方便实现OpenCV与ROS的类型转换
import cv2  # OpenCV库


# 相关内容在之前OpenCV笔记中已经说过，此处不再赘述
def imgStretch(cv_img):
    img_hsv = cv2.cvtColor(cv_img, cv2.COLOR_BGR2HSV)
    img_h = img_hsv[:, :, 0]
    img_s = img_hsv[:, :, 1]
    img_v = img_hsv[:, :, 2]
    equal_img_v = cv2.equalizeHist(img_v)
    img_hsv[:, :, 2] = equal_img_v
    equal_img = cv2.cvtColor(img_hsv, cv2.COLOR_HSV2BGR)
    return equal_img


# 接收到数据后的回调函数
def callback(img):
    # 新建一个cv_bridge对象
    bridge = cv_bridge.CvBridge()

    # 调用bridge对象的成员函数imgmsg_to_cv2将ROS的Image转成OpenCV的NDArray
    # 第一个参数是待转换数据
    # 第二个参数是编码格式字符串，在sensor_msgs/image_encodings.h中有定义
    cv_img = bridge.imgmsg_to_cv2(img, "bgr8")

    # 调用直方图均衡化函数对彩色图像进行拉伸
    equal_img = imgStretch(cv_img)

    # 再次调用bridge的成员函数cv2_to_imgmsg将NDArray类型转成ROS的Image类型
    # 第一个参数是待转换数据
    # 第二个参数是待编码格式字符串，这里因为还是8位彩色图像，所以不变
    img_msg = bridge.cv2_to_imgmsg(equal_img, "bgr8")

    # 调用发布函数发布数据
    pub.publish(img_msg)

    # 调用OpenCV展示直接接收到的原始数据
    cv2.imshow("data from camera", cv_img)
    cv2.waitKey(3)


if __name__ == "__main__":
    # 初始化节点并指定名称
    rospy.init_node("receive_process")

    # 新建Subscriber用于接收原始数据
    # 第一个参数是接收Topic的名称
    # 第二个参数是接收数据的类型
    # 第三个参数是接收到数据后的回调函数
    rospy.Subscriber("image_raw", Image, callback)

    # 新建Publisher用于发布处理后的数据
    # 第一个参数是发布的Topic名称
    # 第二个参数是发布的数据类型
    # 第三个参数是队列长度，一般是一个较小的整数即可
    pub = rospy.Publisher("processed_img", Image, queue_size=1)

    # 循环执行
    rospy.spin()

(2)show_img代码

# coding=utf-8
import rospy  # ROS库
from sensor_msgs.msg import Image   # ROS中接收传感器的图片格式，要注意的是别漏了.msg，否则导入失败
import cv_bridge    # 用于方便实现OpenCV与ROS的类型转换
import cv2  # OpenCV库


# 接收到数据后的回调函数
def callback(img):
    # 新建一个cv_bridge对象
    bridge = cv_bridge.CvBridge()

    # 调用bridge对象的成员函数imgmsg_to_cv2将ROS的Image转成OpenCV的NDArray
    cv_img = bridge.imgmsg_to_cv2(img, "bgr8")
    cv2.imshow("data processed", cv_img)
    cv2.waitKey(3)


if __name__ == "__main__":
    # 初始化节点并指定名称
    rospy.init_node("show_img")

    # 新建Subscriber用于接收发布的处理后的数据
    rospy.Subscriber("processed_img", Image, callback)

    # 循环执行
    rospy.spin()

(3)运行测试

首先在终端中输入roscore启动ROS，然后输入rosrun uvc_camera uvc_camera_node打开摄像头。然后可以切换到脚本路径下，输入python receive_process.py启动接收-处理-发送数据的节点。输入rostopic list即可以看到我们发布的Topic了，并且还可以查看它的信息，如下。然后输入python show_img.py启动展示节点，最终运行效果如下。受限于录制的GIF质量，效果没有直接看明显。但也可以能看出来右边影像对比度比左边要大一些，背景部分更黑了。

运行时的Topic/Node图如下：可以看到/uvc_camera节点发布了很多个Topic，其中/image_raw被我们写的节点/receive_process接受并处理，处理完成之后发布到/processed_img，这个Topic又被我们写的show_img节点所订阅，用于展示。

最后，所有代码上传到了Github，点击查看。

5.参考资料

[1]http://wiki.ros.org/Sensors/Cameras
[2]http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages

Menu