ROS开发笔记1——运动检测程序

这两天在ros环境下用帧间逐差法做了一个运动检测的程序。用的是笔记本自己的摄像头捕捉目标。代码不是很难，不过小坑有点多。

帧间逐差法的原理大致如此：通过读取视频中的前后两帧，并在每一个通道上进行绝对值的比较，再将结果输出到另一个Mat矩阵中。不难看出，对于静止的背景，帧间逐差法输出的结果就为0，即纯黑的背景；而运动的目标就会在输出结果矩阵中产生不同颜色的像素。将输出的矩阵转灰度图，则可以简化寻找运动目标的算法。

差不多了，开始讲步骤吧：

1.建立工作空间

mkdir roscv
cd roscv
mkdir src
catkin_create_pkg motiondet roscpp std_msgs cv_bridge image_transport sensor_msgs
cd motiondet
mkdir src

这里讲讲catkin_create_pkg.我们可以看到这条命令后面跟了motiondet、roscpp、std_msgs等等等等。但实际上我们创建的服务名称就是motiondet，而后面跟的就是我们这个程序运行需要的包名称。后面讲CMake.txt的时候会看到这条命令的方便之处。

当然，程序是写在${你的目录}/roscv/src/motiondet/src下的。这里你是用自带的gedit还是vim都是方便舒服就好啦。ros没有自己的可视ide，基本过程全靠终端。

2.motiondet.cpp代码

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

using namespace std;
using namespace cv;

Mat framepro,frame,dframe;
bool flag=false;
int value=25,core=120;

void frameprocess(Mat);
void thres(int,void*){};

void callback1(const sensor_msgs::ImageConstPtr &msg)//想了用两个回调函数，中间隔一段时间的办法，但是并不能产生间隔的效果
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr=cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);
frame=cv_ptr->image;
waitKey(30);
}
catch (cv_bridge::Exception &e)
{
ROS_ERROR("error:%s",e.what());
return ;
}
if (!cv_ptr->image.empty())
{
if (flag==false) {framepro=cv_ptr->image;flag=true<
4000
/span>;}
else {frame=cv_ptr->image;flag=false;}
absdiff(framepro,frame,dframe);
frameprocess(dframe);
}
}

int main(int argc,char** argv)
{
ros::init(argc,argv,"motiondet");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
image_transport::Subscriber sub1;
sub1=it.subscribe("/camera/rgb/image_raw",1,&callback1);
ros::spin();

}

void frameprocess(Mat dframe)
{
Mat edge;
vector<Point> points;
vector<vector<Point> > contours;
vector<Vec4i> hier;
Mat element = getStructuringElement(MORPH_RECT,Size(core,core));
int i=0;
cvtColor(dframe,dframe,CV_BGR2GRAY);//看了一下例程,知道图像是CV_8UC3的bgr8型，那就是用这个宏了
createTrackbar("value","view",&value,255,thres);//动作阈值，调value就是调对动作幅度的敏感度
createTrackbar("core","view",&core,180,thres);//没有用roslaunch而直接用opencv自带的Trackbar函数 感觉可能更方便直观一点？
//core是腐蚀核，调core就是调目标精细度
threshold(dframe,edge,value,255,0);
medianBlur(edge,edge,5);//用了一个中值去噪声 效果好像还行。如果对运动精细程度要求比较高就会考虑换用更小的核来滤波
dilate(edge,edge,element);//腐蚀函数。在测试的时候因为是拍自己的脑袋，所以我用了大核。核的大小调整直接影响对运动物体大小的探测
Canny(edge,edge,3,150);//后面是基本操作，canny取边缘然后在灰度图里画出来
findContours(edge,contours,hier,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE,Point(0,0));//一个问题：opencv2的findContours函数貌似没有external的选项
//所以我的矩形框里经常有小矩形框
drawContours(edge,contours,-1,Scalar(255),0,CV_AA);
for (vector <vector<Point> >::iterator it=contours.begin();it!=contours.end();++it)
for (vector<Point>::iterator inner=it->begin();inner!=it->end();++inner)
{
points.push_back(*inner);
}
vector<Rect> boundrect(contours.size());
for (vector <vector<Point> >::iterator it=contours.begin();it!=contours.end();++it)
{
boundrect[i]=boundingRect(Mat(contours[i]));
rectangle(framepro,boundrect[i],Scalar(255,255,255),2);
i++;
} //外接长方形
imshow("view",framepro);
}

讲讲思路。

先从main函数开始。

ros::init(argc,argv,"motiondet");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
image_transport::Subscriber sub1;
sub1=it.subscribe("/camera/rgb/image_raw",1,&callback1);
ros::spin();

因为要有两帧图像，所以我第一反应自然是用两个订阅器sub1和sub2.

sub1=it.subscribe("/camera/rgb/image_raw",1,&callback1);
sub2=it.subscribe("/camera/rgb/image_raw",1,&callback2);

然后中间隔一段时间。但是这样写了以后发现只显示sub2的callback2回调函数的内容。然后才发现，在ros浅析里就说了，ros::spin()这个函数是循环最后一个订阅器的。所以两个订阅器的方法只好作罢。同理，一个订阅器然后中间用cv::waitKey(30)或者ros::sleep(),再写一个订阅器的方法都不行，因为只会循环最后一个订阅器命令。

想了一会儿，突然想到可以用一个flag作为标记交替输入需要的帧。

if (!cv_ptr->image.empty())
{
if (flag==false) {framepro=cv_ptr->image;flag=true;}
else {frame=cv_ptr->image;flag=false;}
absdiff(framepro,frame,dframe);
frameprocess(dframe);
}

（图像消息处理的，cv_bridge的用法可以去看看这个博客Robogreen的博客 人家写的特别好，我就不多说献丑了）

然后讲讲这个图像处理的函数。

void frameprocess(Mat dframe)
{
Mat edge;
vector<Point> points;
vector<vector<Point> > contours;
vector<Vec4i> hier;
Mat element = getStructuringElement(MORPH_RECT,Size(core,core));
int i=0;
cvtColor(dframe,dframe,CV_BGR2GRAY);//看了一下例程,知道图像是CV_8UC3的bgr8型，那就是用这个宏了
createTrackbar("value","view",&value,255,thres);//动作阈值，调value就是调对动作幅度的敏感度
createTrackbar("core","view",&core,180,thres);//没有用roslaunch而直接用opencv自带的Trackbar函数 感觉可能更方便直观一点？core是腐蚀核，调core就是调目标精细度
threshold(dframe,edge,value,255,0);
medianBlur(edge,edge,5);//用了一个中值去噪声 效果好像还行。如果对运动精细程度要求比较高就会考虑换用更小的核来滤波
dilate(edge,edge,element);//腐蚀函数。在测试的时候因为是拍自己的脑袋，所以我用了大核。核的大小调整直接影响对运动物体大小的探测
Canny(edge,edge,3,150);//后面是基本操作，canny取边缘然后在灰度图里画出来
findContours(edge,contours,hier,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE,Point(0,0));//一个问题：opencv2的findContours函数貌似没有external的选项，所以我的矩形框里经常有小矩形框

drawContours(edge,contours,-1,Scalar(255),0,CV_AA)
for (vector <vector<Point> >::iterator it=contours.begin();it!=contours.end();++it)
for (vector<Point>::iterator inner=it->begin();inner!=it->end();++inner)
{
points.push_back(*inner);
}
vector<Rect> boundrect(contours.size());
for (vector <vector<Point> >::iterator it=contours.begin();it!=contours.end();++it)
{
boundrect[i]=boundingRect(Mat(contours[i]));
rectangle(framepro,boundrect[i],Scalar(255,255,255),2);
i++;
} //外接长方形
imshow("view",framepro);
}

说回来感谢浅墨dalao写的opencv3教程。opencv教程 这个程序的思路其实倒很简单：先把结果图像转成灰度，然后canny算子找边缘，之后再findContours找出边缘以后，drawContours，最后用一个不规则多边形的外接长方形圈出目标就好了。由于帧间逐差法本身的缺陷，一方面是探测到的物体空洞会比较大，另一方面是物体自身割裂比较严重，会被切割成很多小块，然后结果就会圈的到处都是，就像这样：


所以想到的办法是用一个膨胀，把小块都膨胀到一起变成一大块。同时可以自己调整膨胀核的大小，这样也可以保证能检测到细碎的部分。效果还是可以的：

同时侦测到了我的手和我的脸在运动。看左上的圈层图效果还是不错的。

3.讲讲CMakeLists.txt文件和package.xml文件

在之前我们写过这个命令：

catkin_create_pkg motiondet roscpp std_msgs cv_bridge image_transport sensor_msgs

所以CMakeLists.txt里

find_package(catkin REQUIRED COMPONENTS
cv_bridge
image_transport
roscpp
sensor_msgs
std_msgs
)

相当于motiondet后面的包都是运行需要的，被加到了这里。所以package.xml你也不需要自己去改了。但是如果需要另外加包，也还是需要自己手动去改这两个文件的。

在CMakeLists.txt最后要自己编的add_executable：

add_executable(motiondet src/motiondet.cpp)
add_dependencies(motiondet ${${motiondet}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(motiondet ${catkin_LIBRARIES})

要注意，这一段一定要放在文件的末尾，不能放在注释中间，不然就会出错（为什么我也不明白）。

4.ROS驱动usb_cam

这个问题很好。实际上很早就有人做好了ros的usb_cam驱动。

$ cd ~/catkin_ws/src
$ #git clone https://github.com/bosch-ros-$pkg/usb_cam.git #这里这个驱动已经不再维护了 我们换另一个
$ git clone https://github.com/ros-drivers/usb_cam.git $ cd ~/catkin_ws
$ catkin_make
$ rospack profile

干完这些，然后写一个自己的usb_cam.launch。内容如下：

<launch>
<arg name="device" default="/dev/video0" />

<node name="usb_cam_node" pkg="usb_cam" type="usb_cam_node" output="screen">
<remap from="usb_c
acf8
am_node/image_raw" to="camera/rgb/image_raw" />
<param name="video_device" value="$(arg device)" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="framerate" value="30" />
<param name="brightness" value="128" />
<param name="contrast" value="128" />
<param name="saturation " value="70" />
</node>
</launch>

放进自己的功能包里，然后在编译注册终端完了以后，roslaunch这个usb_cam.launch就好了。我自己的是这样的：

roslaunch motiondet usb_cam.launch device:=/dev/video0

会出来一些warn，不过不用管他也行。然后再到另一个终端里rosrun motiondet motiondet，就可以看到自己的脑袋被动作捕捉了。

有关摄像头的设置，看看这个博客可以：白巧克力亦唯心

下面是写的过程中的手记：

首先是include和link的问题。应该是在cmake文件里解决。

现在一个新的问题是帧间逐差法要有两帧不同的图像，但是现在貌似只能做一个订阅器。

image_transport::CvImagePtr是一个很怪的类，直接用cv_ptr->image赋值给Mat也不行。但是imshow(“”,cv_ptr->image)却是可以的。

应该说思路是调用订阅的回调函数两次，但是好像过程又不能在主函数里运行，只能在回调函数里运行（imshow在主函数中无法实现，但是回调函数里可以）。

对，主要问题就是回调函数只运行一次，而ros::Spin()只循环最后一个订阅的回调函数。我尝试了while(1)循环订阅器，但是没有图像窗口出现。

试了一下，两个订阅器是可以实现的。但是似乎不能实现延迟画面？另外，还要想办法把两幅画面数据取出来。

想到了。就用笨办法，调用两次接收器，矩阵用全局变量。设计一个flag决定应该存framepro还是frame，flag交替变化达成帧间逐差的要求。

啊。百度ros 帧间逐差法都没有结果

最后还是低头了。一个教训：ROS的程序处理目前只能在生成器的回调函数订阅过程中完成。暂时还不能通过回调函数return自己想要的data然后处理。