34 #include <image_transport/image_transport.h>
35 #include <cv_bridge/cv_bridge.h>
36 #include <sensor_msgs/image_encodings.h>
37 #include <opencv2/imgproc/imgproc.hpp>
38 #include <opencv2/highgui/highgui.hpp>
41 namespace enc = sensor_msgs::image_encodings;
44 static const char WINDOW1[] =
"Image Processed";
45 static const char WINDOW0[] =
"Image Raw";
48 image_transport::Publisher
pub;
58 cv_bridge::CvImagePtr cv_ptr;
59 cv_bridge::CvImagePtr cv_ptr_clone;
66 cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
67 cv_ptr_clone = cv_bridge::toCvCopy(original_image, enc::BGR8);
68 cv::Mat cv_clone=cv_ptr->image.clone();
71 catch (cv_bridge::Exception& e)
74 ROS_ERROR(
"tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what());
82 for(
int i = 0; i<cv_ptr->image.rows; i++)
85 for(
int j = 0; j<cv_ptr->image.cols; j++)
87 if (cv_ptr->image.at<cv::Vec3b>(i,j)[0]>100 && cv_ptr->image.at<cv::Vec3b>(i,j)[1]>120 && cv_ptr->image.at<cv::Vec3b>(i,j)[2]>110 && cv_ptr->image.at<cv::Vec3b>(i,j)[0]<255 && cv_ptr->image.at<cv::Vec3b>(i,j)[1]<255 && cv_ptr->image.at<cv::Vec3b>(i,j)[2]<255)
89 if (cv_ptr->image.at<cv::Vec3b>(i,j)[0]>120 && cv_ptr->image.at<cv::Vec3b>(i,j)[1]>190 && cv_ptr->image.at<cv::Vec3b>(i,j)[2]>120 && cv_ptr->image.at<cv::Vec3b>(i,j)[0]<160 && cv_ptr->image.at<cv::Vec3b>(i,j)[1]<255 && cv_ptr->image.at<cv::Vec3b>(i,j)[2]<150)
91 cv_ptr_clone->image.at<cv::Vec3b>(i,j)[0] = 255;
92 cv_ptr_clone->image.at<cv::Vec3b>(i,j)[1] = 255;
93 cv_ptr_clone->image.at<cv::Vec3b>(i,j)[2] = 255;
97 cv_ptr_clone->image.at<cv::Vec3b>(i,j)[0] = 0;
98 cv_ptr_clone->image.at<cv::Vec3b>(i,j)[1] = 0;
99 cv_ptr_clone->image.at<cv::Vec3b>(i,j)[2] = 0;
104 cv_ptr_clone->image.at<cv::Vec3b>(i,j)[0] = 255;
105 cv_ptr_clone->image.at<cv::Vec3b>(i,j)[1] = 255;
106 cv_ptr_clone->image.at<cv::Vec3b>(i,j)[2] = 255;
112 std::vector<cv::Mat> channels;
115 cv::split(cv_ptr_clone->image, channels);
118 cv::Mat imageB = channels[0];
119 cv::Mat imageG = channels[1];
120 cv::Mat imageR = channels[2];
131 if ( (imageR.at<uchar>(imageR.rows-1 , imageR.cols/2) <= 2) && (imageR.at<uchar>(imageR.rows - 2, imageR.cols / 2) <= 2) )
135 for (
int i=1 ; i<imageR.rows-10 ; i++)
137 if ( imageR.at<uchar>(i , 1) && imageR.at<uchar>(i+1 , 1) && imageR.at<uchar>(i+4 , 1) && imageR.at<uchar>(i+5 , 1)==255 && imageR.at<uchar>(i+10 , 1)==255)
142 if ( imageR.at<uchar>(i , cv_ptr->image.cols) && imageR.at<uchar>(i+2 , cv_ptr->image.cols) && imageR.at<uchar>(i+4 , cv_ptr->image.cols)==255 && imageR.at<uchar>(i+10 , cv_ptr->image.cols)==255)
185 cv::imshow(
WINDOW1, cv_ptr_clone->image);
186 cv::imshow(
WINDOW0, cv_ptr->image);
191 pub.publish(cv_ptr_clone->toImageMsg());
197 int main(
int argc,
char **argv)
199 ros::init(argc, argv,
"image_processor");
203 image_transport::ImageTransport it(nh);
205 cv::namedWindow(
WINDOW0, CV_WINDOW_AUTOSIZE);
206 cv::namedWindow(
WINDOW1, CV_WINDOW_AUTOSIZE);
208 image_transport::Subscriber sub = it.subscribe(
"image", 1,
imageCallback);
214 pub = it.advertise(
"image_processed", 1);
217 ROS_INFO(
"tutorialROSOpenCV::main.cpp::No error.");
static const char WINDOW1[]
static const char WINDOW0[]
int main(int argc, char **argv)
image_transport::Publisher pub
void imageCallback(const sensor_msgs::ImageConstPtr &original_image)