ROS学习——巡线(C++ 、opencv)

这个功能需要摄像头的配合，如有需要可以查看我之前的博客，
话不多说，直接上代码

#include
#include
#include
#include
#include
#include
#include
#include

double twist_linear_x , twist_angular_z;
sensor_msgs::Image hsv_image ;

void image_Callback(const sensor_msgs::Image& msg);

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

    ros::Subscriber img_sub = nh.subscribe("/sensor_msgs/image_gray", 10, image_Callback);
    ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    ros::Publisher img_pub = nh.advertise<sensor_msgs::Image>("/image_hsv",10);

    while(ros::ok()){
        geometry_msgs::Twist twist;
        twist.linear.x = twist_linear_x;
        twist.angular.z = twist_angular_z;
        cmd_pub.publish(twist);
        img_pub.publish(hsv_image);
        ros::spinOnce();
    }
    return 0;
}

void image_Callback(const sensor_msgs::Image& msg){
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
    cv::Mat image = cv_ptr -> image;
    cv::Mat hsv = image.clone();
    cv::Mat res = image.clone();
    cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV);
    cv::inRange(hsv, cv::Scalar(0, 0, 0), cv::Scalar(180, 255, 46), res);
    
    int h = image.rows;
    int w = image.cols;
    int search_top = 3 * h / 4, search_bot = search_top + 20;
    for(int i = 0; i < search_top; i ++){
        for(int j = 0; j < w; j ++){
            res.at<uchar>(i,j) = 0;
        }
    }
    for(int i = search_bot; i < h; i++){
        for(int j = 0; j < w; j ++){
            res.at<uchar>(i,j) = 0;
        }
    }
    cv::Moments M = cv::moments(res);

    if(M.m00 > 0){
        int cx = int (cvRound(M.m10 / M.m00));
        int cy = int (cvRound(M.m01 / M.m00));
        ROS_INFO("cx: %d cy: %d", cx, cy);
        cv::circle(image, cv::Point(cx, cy), 10, (0, 0, 255));
        int v = cx - w / 2;
        twist_linear_x = 0.1;
        twist_angular_z = -float(v) / 300 * 0.4;
        //cmd_pub.publish(twist);
    }
    else{
        ROS_INFO("not found line!");
        twist_linear_x = 0;
        twist_angular_z = 0;
        //cmd_pub.publish(twist);
    }
    sensor_msgs::ImagePtr hsv_image_ = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
    hsv_image = *hsv_image_;
}

其中比较有意思的是各种图像类型的转换，一不小心就会出错