turtlebot3在gazebo中自己添加相机利用opencv巡线

最新推荐文章于 2023-01-03 14:09:50 发布

千里飞刀客

最新推荐文章于 2023-01-03 14:09:50 发布

阅读量3.8k

点赞数 1

分类专栏： ros 文章标签： opencv 人工智能计算机视觉

本文链接：https://blog.csdn.net/blue_skyLZW/article/details/121304775

版权

ros 专栏收录该内容

20 篇文章 6 订阅

订阅专栏

效果

在这里插入图片描述

代码

附上初始写的一个类

class LineDetect_firstway {
 public:
    cv::Mat img;
    geometry_msgs::Twist move;  //include Linear velocity and angular velocity  

    void imageCallback(const sensor_msgs::ImageConstPtr& msg){
        cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
	    img = cv_ptr -> image;

        GaussianBlur(img, img_gauss, Size(3, 3), 0, 0, BORDER_DEFAULT);
        cvtColor(img_gauss, img_gray, COLOR_BGR2GRAY);
        Canny(img_gray, dst2, 50, 200, 3);

        cv::namedWindow("Canny", cv::WINDOW_AUTOSIZE);
        cv::imshow("Canny", dst2);
        waitKey(10);

        cv::cvtColor(dst2, cdst2, CV_GRAY2BGR);
        vector< Vec2f > lines;
        cv::HoughLines(dst2, lines, 1, CV_PI/180, 100, 0, 0 );
        x_interset_center=0.0f;
        count_valid_point = 0;
        vector<cv::Point> valid_line_2points;

        for( size_t i = 0; i < lines.size(); i++ )
        {
            float rho = lines[i][0], theta = lines[i][1];
            cout<< "rho="<<rho<<endl;
            // valid point judge
            if(rho/cos(theta) >0 && rho/cos(theta)<800){
            x_interset_center = cvRound(x_interset_center + rho/cos(theta));
            count_valid_point = count_valid_point+1;
            cv::Point p1(cvRound(rho/cos(theta)), 0);
            cv::Point p2(cvRound(rho*cos(theta)), rho*sin(theta));
            valid_line_2points.push_back(p1);
            valid_line_2points.push_back(p2);
            } 
        }
        cout<<"the number of valid points："<< count_valid_point << endl;
        
        for(int i=0;i++;i<count_valid_point){
            cv::line(img, valid_line_2points[i], valid_line_2points[i+1], Scalar(0,0,255), thicknessLine);
        }
        // cv::namedWindow("Display window", cv::WINDOW_AUTOSIZE);
        // cv::imshow("Display window", img);
        // waitKey(1);
        for(int i=0;i++;i<count_valid_point){
            valid_line_2points.pop_back();
            valid_line_2points.pop_back();
        }

        if (count_valid_point == 0){
            move.angular.z=0.0;
            move.linear.x=0.1;
            return;
        }
        else{
            x_interset_center = cvRound(x_interset_center /count_valid_point );
            cout<< "x_interset_center: " <<x_interset_center <<endl; //Where the line intersects the X-axis
            error_offset = x_axis_center - x_interset_center;
            if(abs(error_offset)<5){
            move.angular.z=0.0;
            move.linear.x=0.5;
            }
            else{
            move.angular.z = 1 * error_offset / x_axis_center;
            move.linear.x = 0.3 - abs(error_offset) /x_axis_center * 1 ;
            }
            }
    };
 private:
    cv::Mat dst2;
    cv::Mat cdst2;
    cv::Mat img_gauss;
    cv::Mat img_gray;
    int x_axis_center = 400;
    float x_interset_center=0.0f;  //The midpoint where the line intersects the X-axis
    int count_valid_point = 0; //the number of vaild point
    int error_offset=0;
    int thicknessLine = 20;
};