ROS find_red_pixels

ImageConverter类声明

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

static const std::string OPENCV_WINDOW = "OpenCV display window";
using namespace std;
using namespace cv;

int g_redratio; // 红色阈值

class ImageConverter {
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    image_transport::Subscriber image_sub_;
    image_transport::Publisher image_pub_;

public:

    ImageConverter(ros::NodeHandle &nodehandle)
    : it_(nh_) {
        image_sub_ = it_.subscribe("simple_camera/image_raw", 1,
                &ImageConverter::imageCb, this);
        image_pub_ = it_.advertise("/image_converter/output_video", 1);

        namedWindow(OPENCV_WINDOW);
    }

    ~ImageConverter() {
        destroyWindow(OPENCV_WINDOW);
    }

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

imageCb回调函数定义

void ImageConverter::imageCb(const sensor_msgs::ImageConstPtr& msg){
    cv_bridge::CvImagePtr cv_ptr; //OpenCV data type
    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;
    }
    int npix = 0;   // 红色区域总的像素
    int isum = 0;   // 红色的列数
    int jsum = 0;   // 红色的行数
    int redval, blueval, greenval, testval;
    Vec3b rgbpix;
    for (int i = 0; i < cv_ptr->image.cols; i++) {
        for (int j = 0; j < cv_ptr->image.rows; j++) {
            // 提取某点像素值
            rgbpix = cv_ptr->image.at<Vec3b>(j, i);
            redval = rgbpix[2] + 1; // +1 防止除以0
            blueval = rgbpix[0] + 1;
            greenval = rgbpix[1] + 1;
            testval = redval / (blueval + greenval);
            // 如果这个点判定为红色，设为白色
            if (testval > g_redratio) {
                cv_ptr->image.at<Vec3b>(j, i)[0] = 255;
                cv_ptr->image.at<Vec3b>(j, i)[1] = 255;
                cv_ptr->image.at<Vec3b>(j, i)[2] = 255;
                npix++;     // 红色区域总的像素
                isum += i;  // 红色的列数
                jsum += j;  // 红色的行数
            } else { // 其他点设为黑色
                cv_ptr->image.at<Vec3b>(j, i)[0] = 0;
                cv_ptr->image.at<Vec3b>(j, i)[1] = 0;
                cv_ptr->image.at<Vec3b>(j, i)[2] = 0;
            }
        }
    }

    ROS_WARN("npix: %d", npix);

    int half_box = 5; // 矩形大小
    int i_centroid, j_centroid;
    double x_centroid, y_centroid;
    if (npix > 0) {
        i_centroid = isum / npix;   // 中间点位置
        j_centroid = jsum / npix;
        x_centroid = ((double) isum)/((double) npix);
        y_centroid = ((double) jsum)/((double) npix);
        ROS_INFO("u_avg: %f; v_avg: %f",x_centroid,y_centroid);

        ROS_WARN("center (col, row) : (%d, %d)", i_centroid, j_centroid);

        for (int i_box = i_centroid - half_box; i_box <= i_centroid + half_box; i_box++) {
            for (int j_box = j_centroid - half_box; j_box <= j_centroid + half_box; j_box++) {
                if ((i_box >= 0)&&(j_box >= 0)&&(i_box < cv_ptr->image.cols)&&(j_box < cv_ptr->image.rows)) {
                    cv_ptr->image.at<Vec3b>(j_box, i_box)[0] = 255; //(255,0,0) is pure blue
                    cv_ptr->image.at<Vec3b>(j_box, i_box)[1] = 0;
                    cv_ptr->image.at<Vec3b>(j_box, i_box)[2] = 0;
                }
            }
        }

    }
    imshow(OPENCV_WINDOW, cv_ptr->image);   // 图像显示
    waitKey(3); 
    image_pub_.publish(cv_ptr->toImageMsg());   // 将处理后的图像发布出去
}

主函数

int main(int argc, char** argv) {
    ros::init(argc, argv, "find_red_pixels_node");
    ros::NodeHandle n;    
    ImageConverter ic(n);   // 创建类实例
    g_redratio= 10;         // 红色的阈值
    ros::Duration timer(0.1);
    while (ros::ok()) {
        ros::spinOnce();
        timer.sleep();
    }
    return 0;
}

原始图像

找到红色区域中心位置

canny函数

Mat gray_image, contours;
cvtColor(cv_ptr->image, gray_image, COLOR_BGR2GRAY);
Canny(gray_image, contours, 125, 350);
imshow(OPENCV_WINDOW, contours);
waitKey(3); 
sensor_msgs::ImagePtr msg_contours;
try {
    msg_contours = cv_bridge::CvImage(std_msgs::Header(), "mono8", contours).toImageMsg();
} catch(cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
}
image_pub_.publish(msg_contours);

编码参数

mono8: CV_8UC1, grayscale image
mono16: CV_16UC1, 16-bit grayscale image
bgr8: CV_8UC3, color image with blue-green-red color order
rgb8: CV_8UC3, color image with red-green-blue color order
bgra8: CV_8UC4, BGR color image with an alpha channel
rgba8: CV_8UC4, RGB color image with an alpha channel

红色边缘