00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00042 #include <ros/ros.h>
00043 #include "opencv_apps/nodelet.h"
00044 #include <image_transport/image_transport.h>
00045 #include <sensor_msgs/image_encodings.h>
00046 #include <cv_bridge/cv_bridge.h>
00047 #include <sensor_msgs/image_encodings.h>
00048
00049 #include <opencv2/highgui/highgui.hpp>
00050 #include <opencv2/imgproc/imgproc.hpp>
00051
00052 #include <dynamic_reconfigure/server.h>
00053 #include "opencv_apps/GeneralContoursConfig.h"
00054 #include "opencv_apps/RotatedRect.h"
00055 #include "opencv_apps/RotatedRectArray.h"
00056 #include "opencv_apps/RotatedRectArrayStamped.h"
00057
00058 namespace general_contours {
00059 class GeneralContoursNodelet : public opencv_apps::Nodelet
00060 {
00061 image_transport::Publisher img_pub_;
00062 image_transport::Subscriber img_sub_;
00063 image_transport::CameraSubscriber cam_sub_;
00064 ros::Publisher rects_pub_, ellipses_pub_;
00065
00066 boost::shared_ptr<image_transport::ImageTransport> it_;
00067
00068 typedef general_contours::GeneralContoursConfig Config;
00069 typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00070 Config config_;
00071 boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00072
00073 bool debug_view_;
00074 ros::Time prev_stamp_;
00075
00076 int threshold_;
00077
00078 std::string window_name_;
00079 static bool need_config_update_;
00080
00081 void reconfigureCallback(Config &new_config, uint32_t level)
00082 {
00083 config_ = new_config;
00084 threshold_ = config_.threshold;
00085 }
00086
00087 const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
00088 {
00089 if (frame.empty())
00090 return image_frame;
00091 return frame;
00092 }
00093
00094 void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
00095 {
00096 do_work(msg, cam_info->header.frame_id);
00097 }
00098
00099 void imageCallback(const sensor_msgs::ImageConstPtr& msg)
00100 {
00101 do_work(msg, msg->header.frame_id);
00102 }
00103
00104 static void trackbarCallback( int, void* )
00105 {
00106 need_config_update_ = true;
00107 }
00108
00109 void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
00110 {
00111
00112 try
00113 {
00114
00115 cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
00116
00117
00118 opencv_apps::RotatedRectArrayStamped rects_msg, ellipses_msg;
00119 rects_msg.header = msg->header;
00120 ellipses_msg.header = msg->header;
00121
00122
00123 cv::Mat src_gray;
00124
00126 if ( frame.channels() > 1 ) {
00127 cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY );
00128 } else {
00129 src_gray = frame;
00130 }
00131 cv::blur( src_gray, src_gray, cv::Size(3,3) );
00132
00134 if( debug_view_) {
00135 cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
00136 }
00137
00138 cv::Mat threshold_output;
00139 int max_thresh = 255;
00140 std::vector<std::vector<cv::Point> > contours;
00141 std::vector<cv::Vec4i> hierarchy;
00142 cv::RNG rng(12345);
00143
00145 cv::threshold( src_gray, threshold_output, threshold_, 255, cv::THRESH_BINARY );
00146
00148 cv::findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
00149
00151 std::vector<cv::RotatedRect> minRect( contours.size() );
00152 std::vector<cv::RotatedRect> minEllipse( contours.size() );
00153
00154 for( size_t i = 0; i < contours.size(); i++ )
00155 { minRect[i] = cv::minAreaRect( cv::Mat(contours[i]) );
00156 if( contours[i].size() > 5 )
00157 { minEllipse[i] = cv::fitEllipse( cv::Mat(contours[i]) ); }
00158 }
00159
00161 cv::Mat drawing = cv::Mat::zeros( threshold_output.size(), CV_8UC3 );
00162 for( size_t i = 0; i< contours.size(); i++ )
00163 {
00164 cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
00165
00166 cv::drawContours( drawing, contours, (int)i, color, 1, 8, std::vector<cv::Vec4i>(), 0, cv::Point() );
00167
00168 cv::ellipse( drawing, minEllipse[i], color, 2, 8 );
00169
00170 cv::Point2f rect_points[4]; minRect[i].points( rect_points );
00171 for( int j = 0; j < 4; j++ )
00172 cv::line( drawing, rect_points[j], rect_points[(j+1)%4], color, 1, 8 );
00173
00174 opencv_apps::RotatedRect rect_msg;
00175 opencv_apps::Point2D rect_center;
00176 opencv_apps::Size rect_size;
00177 rect_center.x = minRect[i].center.x;
00178 rect_center.y = minRect[i].center.y;
00179 rect_size.width = minRect[i].size.width;
00180 rect_size.height = minRect[i].size.height;
00181 rect_msg.center = rect_center;
00182 rect_msg.size = rect_size;
00183 rect_msg.angle = minRect[i].angle;
00184 opencv_apps::RotatedRect ellipse_msg;
00185 opencv_apps::Point2D ellipse_center;
00186 opencv_apps::Size ellipse_size;
00187 ellipse_center.x = minEllipse[i].center.x;
00188 ellipse_center.y = minEllipse[i].center.y;
00189 ellipse_size.width = minEllipse[i].size.width;
00190 ellipse_size.height = minEllipse[i].size.height;
00191 ellipse_msg.center = ellipse_center;
00192 ellipse_msg.size = ellipse_size;
00193 ellipse_msg.angle = minEllipse[i].angle;
00194
00195 rects_msg.rects.push_back(rect_msg);
00196 ellipses_msg.rects.push_back(ellipse_msg);
00197 }
00198
00200 if( debug_view_) {
00201 if (need_config_update_) {
00202 config_.threshold = threshold_;
00203 reconfigure_server_->updateConfig(config_);
00204 need_config_update_ = false;
00205 }
00206 cv::createTrackbar( "Threshold:", window_name_, &threshold_, max_thresh, trackbarCallback);
00207
00208 cv::imshow( window_name_, drawing );
00209 int c = cv::waitKey(1);
00210 }
00211
00212
00213 sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg();
00214 img_pub_.publish(out_img);
00215 rects_pub_.publish(rects_msg);
00216 ellipses_pub_.publish(ellipses_msg);
00217 }
00218 catch (cv::Exception &e)
00219 {
00220 NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
00221 }
00222
00223 prev_stamp_ = msg->header.stamp;
00224 }
00225
00226 void subscribe()
00227 {
00228 NODELET_DEBUG("Subscribing to image topic.");
00229 if (config_.use_camera_info)
00230 cam_sub_ = it_->subscribeCamera("image", 3, &GeneralContoursNodelet::imageCallbackWithInfo, this);
00231 else
00232 img_sub_ = it_->subscribe("image", 3, &GeneralContoursNodelet::imageCallback, this);
00233 }
00234
00235 void unsubscribe()
00236 {
00237 NODELET_DEBUG("Unsubscribing from image topic.");
00238 img_sub_.shutdown();
00239 cam_sub_.shutdown();
00240 }
00241
00242 public:
00243 virtual void onInit()
00244 {
00245 Nodelet::onInit();
00246 it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_));
00247
00248 pnh_->param("debug_view", debug_view_, false);
00249 if (debug_view_) {
00250 always_subscribe_ = true;
00251 }
00252 prev_stamp_ = ros::Time(0, 0);
00253
00254 window_name_ = "Contours";
00255 threshold_ = 100;
00256
00257 reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
00258 dynamic_reconfigure::Server<Config>::CallbackType f =
00259 boost::bind(&GeneralContoursNodelet::reconfigureCallback, this, _1, _2);
00260 reconfigure_server_->setCallback(f);
00261
00262 img_pub_ = advertiseImage(*pnh_, "image", 1);
00263 rects_pub_ = advertise<opencv_apps::RotatedRectArrayStamped>(*pnh_, "rectangles", 1);
00264 ellipses_pub_ = advertise<opencv_apps::RotatedRectArrayStamped>(*pnh_, "ellipses", 1);
00265
00266 onInitPostProcess();
00267 }
00268 };
00269 bool GeneralContoursNodelet::need_config_update_ = false;
00270 }
00271
00272 #include <pluginlib/class_list_macros.h>
00273 PLUGINLIB_EXPORT_CLASS(general_contours::GeneralContoursNodelet, nodelet::Nodelet);