使用方法：
- 修改主控IP地址，编译后在bin文件夹中运行./run执行文件，激活机器人在进入站立后按遥控Start按键进入自动模式
项目简介：
通过OpenCV的HSV颜色通道可以完成对颜色线路的识别，典型的颜色通道HSV值可以设置如下，通过调节拖动条可以快速完成对识别颜色的改变：
巡线首先要求相机需要斜视朝下，这样保证能看到直线的首尾，首先完成颜色的提取进一步通过OpenCv轮廓提取函数findContours查询到封闭线段的轮廓，最终通过索引轮廓的顶点、中点和底部绘制出直线段，采用其角度完成对机器人转向的控制，并实现巡线的功能。
本项目的示例代码如下：
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <iostream>
#include <sstream>
#include <stdio.h>
#include <unistd.h>
#include <stdio.h>
#include <sys/time.h>
#include <string.h>
#include <unistd.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core/eigen.hpp>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
using namespace std;
using namespace cv;
Point2f Line_center;
float Line_angle;
int Line_num;
struct send_msg
{
char gait_mode;
float att_cmd[3];
float rate_cmd[3];
float speed_cmd[3];
float pos_cmd[3];
} _send_msg;
void warpFfine(cv::Mat &inputIm, cv::Mat &tempImg, float angle)
{
CV_Assert(!inputIm.empty());
Mat inputImg;
inputIm.copyTo(inputImg);
float radian = (float) (angle / 180.0 * CV_PI);
int uniSize = (int) (max(inputImg.cols, inputImg.rows) * 1.414);
int dx = (int) (uniSize - inputImg.cols) / 2;
int dy = (int) (uniSize - inputImg.rows) / 2;
copyMakeBorder(inputImg, tempImg, dy, dy, dx, dx, BORDER_CONSTANT);
Point2f center((float) (tempImg.cols / 2), (float) (tempImg.rows / 2));
Mat affine_matrix = getRotationMatrix2D(center, angle, 1.0);
warpAffine(tempImg, tempImg, affine_matrix, tempImg.size());
float sinVal = fabs(sin(radian));
float cosVal = fabs(cos(radian));
Size targetSize((int) (inputImg.cols * cosVal + inputImg.rows * sinVal),
(int) (inputImg.cols * sinVal + inputImg.rows * cosVal));
int x = (tempImg.cols - targetSize.width) / 2;
int y = (tempImg.rows - targetSize.height) / 2;
Rect rect(x, y, targetSize.width, targetSize.height);
tempImg = Mat(tempImg, rect);
}
void thresholdIntegral(cv::Mat &inputMat, cv::Mat &outputMat,int Ds, float T)
{
// accept only char type matrices
CV_Assert(!inputMat.empty());
CV_Assert(inputMat.depth() == CV_8U);
CV_Assert(inputMat.channels() == 1);
CV_Assert(!outputMat.empty());
CV_Assert(outputMat.depth() == CV_8U);
CV_Assert(outputMat.channels() == 1);
// rows -> height -> y
int nRows = inputMat.rows;
// cols -> width -> x
int nCols = inputMat.cols;
// create the integral image
cv::Mat sumMat;
cv::integral(inputMat, sumMat);
CV_Assert(sumMat.depth() == CV_32S);
CV_Assert(sizeof(int) == 4);
int S = MAX(nRows, nCols)/Ds;
// perform thresholding
int s2 = S/2;
int x1, y1, x2, y2, count, sum;
// CV_Assert(sizeof(int) == 4);
int *p_y1, *p_y2;
uchar *p_inputMat, *p_outputMat;
for( int i = 0; i < nRows; ++i)
{
y1 = i-s2;
y2 = i+s2;
if (y1 < 0){
y1 = 0;
}
if (y2 >= nRows) {
y2 = nRows-1;
}
p_y1 = sumMat.ptr<int>(y1);
p_y2 = sumMat.ptr<int>(y2);
p_inputMat = inputMat.ptr<uchar>(i);
p_outputMat = outputMat.ptr<uchar>(i);
for ( int j = 0; j < nCols; ++j)
{
// set the SxS region
x1 = j-s2;
x2 = j+s2;
if (x1 < 0) {
x1 = 0;
}
if (x2 >= nCols) {
x2 = nCols-1;
}
count = (x2-x1)*(y2-y1);
// I(x,y)=s(x2,y2)-s(x1,y2)-s(x2,y1)+s(x1,x1)
sum = p_y2[x2] - p_y1[x2] - p_y2[x1] + p_y1[x1];
if ((int)(p_inputMat[j] * count) < (int)(sum*(1.0-T)))
p_outputMat[j] = 0;
else
p_outputMat[j] = 255;
}
}
}
void thread_line(void)
{
Mat InImage;
cout<<"CV_VERSION"<<CV_VERSION<<endl;
int SW=320,SH=240;
cv::Size InImage_size(SW,SH);
ostringstream ostr_pos;
ostringstream ostr_angle;
cv::TickMeter tm,tm_uart;
#if 1
VideoCapture cap1(-1);
#else
string video_read_s("/home/exbot/SLAM/Line_Tracker/video_r/6.avi");
cout<<"read_video"<<video_read_s<<endl;
VideoCapture cap1(video_read_s);
#endif
int iLowH = 0;
int iHighH = 176;
int iLowS = 0;
int iHighS = 255;
int iLowV = 0;
int iHighV = 46;
int Min_RectS=4,Min_RectW=88,TOP_good_rate=0.66*100;
cvNamedWindow("Line Result",CV_WINDOW_AUTOSIZE);
createTrackbar("Min_RectS", "Line Result", &Min_RectS, 30);
createTrackbar("Min_RectW", "Line Result", &Min_RectW, SW/2);
createTrackbar("TOP_good_rate", "Line Result", &TOP_good_rate, 100);
namedWindow("Control", CV_WINDOW_AUTOSIZE);
cvCreateTrackbar("LowH", "Control", &iLowH, 179); //Hue (0 - 179)
cvCreateTrackbar("HighH", "Control", &iHighH, 179);
cvCreateTrackbar("LowS", "Control", &iLowS, 255); //Saturation (0 - 255)
cvCreateTrackbar("HighS", "Control", &iHighS, 255);
cvCreateTrackbar("LowV", "Control", &iLowV, 255); //Value (0 - 255)
cvCreateTrackbar("HighV", "Control", &iHighV, 255);
//----------------------UDP init----------------------------
int SERV_PORT= 8888;
int sock_fd;
sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
if(sock_fd < 0)
{
exit(1);
}
struct sockaddr_in addr_serv;
int len;
memset(&addr_serv, 0, sizeof(addr_serv));
addr_serv.sin_family = AF_INET;
string UDP_IP="192.168.1.106";
addr_serv.sin_addr.s_addr = inet_addr(UDP_IP.c_str());
addr_serv.sin_port = htons(SERV_PORT);
len = sizeof(addr_serv);
printf("UPD Tinymal-ROS CV Port Open Suess!!");
int recv_num=0,send_num=0;
char send_buf[1024*100]={0},recv_buf[1024*10]={0};
for(;;)
{
int bad_frame=0;
tm.reset();
tm.start();
cap1>>InImage;
if(!InImage.empty())
cv::resize(InImage, InImage, InImage_size);
else
bad_frame=1;
float alpha=4,beta=1.0;
for(int i=0;i<InImage.rows;i++)
for (int j = 0; j < InImage.cols; j++) {
for (int k = 0; k < 3; k++) {
int tmp= (uchar)InImage.at<Vec3b>(i, j)[k] * alpha + beta;
if (tmp > 255)
InImage.at<Vec3b>(i, j)[k] = 2 * 255 - tmp;
else
InImage.at<Vec3b>(i, j)[k] = tmp;
}
}
if(!bad_frame){
int DEAD_W=35;
float H_SIZE=0.5;
Rect rect_bottom1(DEAD_W,SH*H_SIZE,SW-DEAD_W*2,SH*H_SIZE);
Mat ROI;
InImage(rect_bottom1).copyTo(ROI);
//imshow("Origin", InImage);
//imshow("In", ROI);
Line_center.x=Line_center.y=0;
Line_angle=0;
Line_num=0;
Mat imgHSV;
vector<Mat> hsvSplit;
cvtColor(ROI, imgHSV, COLOR_BGR2HSV);
split(imgHSV, hsvSplit);
equalizeHist(hsvSplit[2],hsvSplit[2]);
merge(hsvSplit,imgHSV);
Mat imgThresholded;
inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded); //Threshold the image
Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
morphologyEx(imgThresholded, imgThresholded, MORPH_OPEN, element);
morphologyEx(imgThresholded, imgThresholded, MORPH_CLOSE, element);
int Bolder_W;
Rect bolder(0,0,SW-DEAD_W*2,SH*H_SIZE);
rectangle(imgThresholded,bolder, Scalar(0, 0, 0), 3);
imshow("Control", imgThresholded);
vector<vector<Point>> contours;
vector<Vec4i> hierarchy;
findContours(imgThresholded, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_NONE, Point());
Mat Draw = Mat::zeros(imgThresholded.size(), CV_8UC1); //��С��Ӿ��λ���
//cout<<"contours.size(): "<<contours.size()<<endl;
for (int i = 0; i<contours.size(); i++)
{
drawContours(Draw, contours, i, Scalar(255), 1, 8, hierarchy);
RotatedRect rect = minAreaRect(contours[i]);
Point2f center=rect.center;
float angle=rect.angle;
Size2f size=rect.size;
if(size.area()/SH>Min_RectS&&(size.width<Min_RectW||size.height<Min_RectW)){
Point2f P[4];
rect.points(P);
Point2f Top,Mid,Bot;
if(size.height>size.width){
Top=Point2f((P[1].x+P[2].x)/2,(P[1].y+P[2].y)/2);
Mid=center;
Bot=Point2f((P[0].x+P[3].x)/2,(P[0].y+P[3].y)/2);
}else
{
angle=90+angle;
Top=Point2f((P[2].x+P[3].x)/2,(P[2].y+P[3].y)/2);
Mid=center;
Bot=Point2f((P[1].x+P[0].x)/2,(P[1].y+P[0].y)/2);
}
//cout<<size.width<<" "<<size.height<<" "<<angle<<endl;
if(fabs(Top.y)<SH*H_SIZE*TOP_good_rate/100.&&
fabs(Bot.y)>SH*H_SIZE*(100-TOP_good_rate)/100.){
cout<<"Find_Line: "<<"Size: "<<size.area()/SH<<" Angle: "<<angle<<endl;
circle(Draw, Top, 5,Scalar(255,255,0),-1);
circle(Draw, Mid, 3,Scalar(111),-1);
circle(Draw, Bot, 10,Scalar(255,255,0),-1);
for (int j = 0; j <= 3; j++)
{
line(Draw, P[j], P[(j + 1) % 4], Scalar(111), 2);
}
line(ROI, Mid,Bot, Scalar(125,0,255), 2);
circle(ROI, Mid, 6,Scalar(0,255,255),-1);
circle(ROI, Bot, 3,Scalar(255,255,0),-1);
Line_center=Mid;
Line_angle=angle;
Line_num++;
ostr_pos.clear();
ostr_pos.str("");
ostr_pos << "X=" << (int)Line_center.x << " Y=" << (int)Line_center.y;
putText(ROI, ostr_pos.str(), Point(Line_center.x+20, Line_center.y+20),
CV_FONT_HERSHEY_SIMPLEX, 0.5, CV_RGB(55, 255, 0), 2);
}
}
}
//imshow("Line Result", Draw);
imshow("In", ROI);
//Control
float flt_cmd=0.05;
float flt_att=0.025;
if(Line_num>0){
int err_pix=0;
_send_msg.gait_mode=1;
_send_msg.speed_cmd[0]=0.2;
err_pix=(160-Line_center.y);
if(err_pix>25)err_pix=25;
if(err_pix<-25)err_pix=-25;
if(fabs(err_pix)<3)err_pix=0;
_send_msg.rate_cmd[2]=err_pix*0.003*10*flt_cmd+(1-flt_cmd)*_send_msg.rate_cmd[2];
_send_msg.speed_cmd[1]=err_pix*0.0025*flt_cmd+(1-flt_cmd)*_send_msg.speed_cmd[1];
_send_msg.att_cmd[0]=-10*flt_att+(1-flt_att)*_send_msg.att_cmd[0];
}else{
_send_msg.speed_cmd[0]=0*flt_cmd+(1-flt_cmd)*_send_msg.speed_cmd[0];
_send_msg.speed_cmd[1]=0*flt_cmd+(1-flt_cmd)*_send_msg.speed_cmd[1];
_send_msg.rate_cmd[2]=0*flt_cmd+(1-flt_cmd)*_send_msg.rate_cmd[2];
_send_msg.att_cmd[0]=0*flt_att+(1-flt_att)*_send_msg.att_cmd[0];
}
//UDP Send
memcpy(send_buf,&_send_msg,sizeof(_send_msg));
send_num = sendto(sock_fd, send_buf, sizeof(_send_msg), MSG_DONTWAIT, (struct sockaddr *)&addr_serv,len);
recv_num = recvfrom(sock_fd, recv_buf, sizeof(recv_buf), MSG_DONTWAIT, (struct sockaddr *)&addr_serv, (socklen_t *)&len);
if(recv_num <= 0)
{
}
else{//接收机器人底盘 里程计等数据
//解码
//memcpy(&_recv_msg,recv_buf,sizeof(_recv_msg));
}
static float time;
time+=(float)tm.getTimeMilli()/1000.;
}//bad_frame
else{
cout<<"bad_frame"<<endl;
}
char c = (char)waitKey(20);
if(c == 27)
break;
}
}
int main(void)
{
thread_line();
}
