ROS机械臂开发：机器视觉应用

一、ROS图像接口

摄像头驱动安装

i5@i5-ThinkPad-T460p:~$ sudo apt install ros-kinetic-usb-cam

编写摄像头启动usb-cam.launch文件

usb_cam功能包中的参数

参数	类型	默认值	描述
~video_device	string	"/dev/video0"	摄像头设备号
~image_width	integer	640	横向分辨率
~image_height	integer	480	纵向分辨率
~pixel_format	string	"mjpeg"	像素编码
~io_method	string	"mmap"	IO通道
~camera_frame_id	string	"head_camera"	摄像头坐标系
~framerate	integer	30	帧率
~contrast	integer	32	对比度，0-255
~brightness	integer	32	亮度，0-255
~saturation	integer	32	饱和度，0-255
~sharpness	integer	22	清晰度，0-255
~autofocus	boolean	false	自动对焦
~focus	integer	51	焦点（非自动对焦状态下有效）
~camera_info_url	string	""	摄像头校准文件路径
~camera_name	string	"head_camera"	摄像头名称

usb_cam功能包中的话题

名称	类型	描述
~/image	sensor_msgs/Image	发布图像数据

i5@i5-ThinkPad-T460p:~$ rosmsg show sensor_msgs/Image 
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data

消息中各个域的含义如下：

• header
  消息头，包含消息序号，时间戳，绑定坐标系
• height
  图像纵向分辨率
• width
  图像横向分辨率
• encoding
  图像编码格式，包含RGB、YUV等常用格式，不涉及图像压缩编码
• is_bigendian
  图像数据的大小端存储模式
• step
  一行图像数据的字节数量，作为数据的步长参数
• data
  存储图像数据的数组，大小为step*height个字节

启动摄像头

i5@i5-ThinkPad-T460p:~$ roslaunch vision_application usb_cam.launch 

如果使用的是带内置USB摄像头的笔记本，此时摄像头应当已经启动了：

摄像头启动

查看摄像头图像

i5@i5-ThinkPad-T460p:~$ rqt_image_view

rqt_image_view

二、摄像头内参标定

内参属于摄像头自身参数，外参是指和机械臂之间的位置关系。标定内参是为了消除图像的畸变，再做后面的识别。ROS提供了标定功能包，直接命令行安装：

i5@pop-os:~$ sudo apt install ros-melodic-camera-calibration

安装后启动摄像头和标定功能程序：

i5@pop-os:~$ rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.024 image:=/usb_cam/image_raw camera:=/usb_cam

将标定板摆放不同的位置和角度，直到CALIBRATE变为绿色，点击，即可完成摄像头内参标定。

内参标定

采样点

标定Ready

点击SAVE，程序将标定文件写入磁盘保存。

保存标定参数

解压后会得到一系列标定过程中获得的图片和一个yaml标定文件，这个标定文件即包含了摄像头的内参，可直接在usb_cam功能包中使用。

标定文件

得到标定文件后，在开头的usb_cam.lauch文件中加入tag：

再次启动摄像头后获得的图像即为标定后的图像了。如果启动过程中有警告：does not match name narrow_stereo in file，可尝试将yaml文件中的camera_name修改为head_camera后再重新启动。

三、ROS+OpenCV物体识别

在ROS中使用OpenCV，可以通过CvBridge功能包来实现ROS图像消息和OpenCV图像数据结构间的转换：

cv_bridge

在ROS中进行OpenCV物体识别开发一般经过如下的流程：

• ROS驱动摄像头，发布图像消息
• 将ROS图像消息转换成OpenCV图像数据
• OpenCV图像处理
• OpenCV图像转换成ROS消息
  下面给出在ROS中使用OpenCV人脸识别API编写的程序示例：

头文件

#ifndef FACE_DETECTOR_HPP_
#define FACE_DETECTOR_HPP_

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "opencv2/highgui/highgui.hpp"

#include 
#include 
#include 

////定义7种颜色，用于标记人脸
cv::Scalar colors[] =
{
    // 红橙黄绿青蓝紫
   CV_RGB(255,0,0),
   CV_RGB(255, 97, 0),
   CV_RGB(255, 255, 0),
   CV_RGB(0, 255, 0),
   CV_RGB(255, 97, 0),
   CV_RGB(0, 0, 255),
   CV_RGB(160, 32, 240),
};

class FaceDetector
{
public:
    FaceDetector(ros::NodeHandle& nh);

    ~FaceDetector();

private:
    void detectFace(const sensor_msgs::ImageConstPtr &msg);
    void drawFace(cv::Mat &image, const std::vector &rect);
    void imageCb(const sensor_msgs::ImageConstPtr &msg);

    image_transport::ImageTransport it_;
    image_transport::Subscriber image_sub_;
    image_transport::Publisher image_pub_;
    cv_bridge::CvImagePtr cv_ptr_;
    cv::Mat gray_img_;
    cv::Mat process_img_;
    cv::CascadeClassifier cascade_;
    std::vector rect_;
};

#endif /* FACE_DETECTOR_HPP_ */

cpp文件

#include "face_detector.hpp"

FaceDetector::FaceDetector(ros::NodeHandle& nh) : it_(nh)
{
    // 加载人脸分类器
    cascade_.load("/usr/share/opencv/haarcascades/haarcascade_frontalface_alt2.xml");
    image_sub_ = it_.subscribe("/usb_cam/image_raw", 1, &FaceDetector::imageCb, this);
    image_pub_ = it_.advertise("/face_detect", 1);
}

FaceDetector::~FaceDetector()
{
    ROS_INFO("Bye bye\n");
}

void FaceDetector::drawFace(cv::Mat &image, const std::vector &rect)
{
    cv::Point center;
    int radius;
    for(int i = 0; i < rect.size(); i++)
    {
        center.x = cvRound((rect[i].x + rect[i].width * 0.5));
        center.y = cvRound((rect[i].y + rect[i].height * 0.5));
        radius = cvRound((rect[i].width + rect[i].height) *0.25);
        cv::circle(image, center, radius, colors[i % 7], 2);
    }
}

void FaceDetector::detectFace(const sensor_msgs::ImageConstPtr &msg)
{
    try
    {
        cv_ptr_ = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception &e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    cv::Mat &image = cv_ptr_->image;
    process_img_ = image.clone();
    cv::cvtColor(image, gray_img_, CV_BGR2GRAY);
    cascade_.detectMultiScale(gray_img_, rect_, 1.3, 4, 0);
    ROS_INFO("%d face detected\n", static_cast(rect_.size()));
    drawFace(process_img_, rect_);
    cv_bridge::CvImage out_image;
    out_image.encoding = cv_ptr_->encoding;
    out_image.header = cv_ptr_->header;
    out_image.image = process_img_;
    image_pub_.publish(out_image.toImageMsg());
}

void FaceDetector::imageCb(const sensor_msgs::ImageConstPtr &msg)
{
    detectFace(msg);
}

int main(int argc, char** argv )
{
    ros::init(argc, argv, "simple_face_vision_detection");
    ros::NodeHandle n_;

    ros::WallDuration(0.1).sleep();

    FaceDetector fd(n_);

    while (ros::ok())
    {
        // Process image callback
        ros::spinOnce();

        ros::WallDuration(0.1).sleep();
    }
    return 0;
}

launch文件

启动后将看到标注人脸（如果有）的视频流，终端会打印检测到人脸的数目：

face detection