$search
00001 /* 00002 * Player - One Hell of a Robot Server 00003 * Copyright (C) 2000 00004 * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard 00005 * Andrew Martignoni III 00006 * 00007 * This program is free software; you can redistribute it and/or modify 00008 * it under the terms of the GNU General Public License as published by 00009 * the Free Software Foundation; either version 2 of the License, or 00010 * (at your option) any later version. 00011 * 00012 * This program is distributed in the hope that it will be useful, 00013 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00014 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00015 * GNU General Public License for more details. 00016 * 00017 * You should have received a copy of the GNU General Public License 00018 * along with this program; if not, write to the Free Software 00019 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00020 * 00021 */ 00022 00023 /* 00024 * Uses CMVision to retrieve the blob data 00025 */ 00026 // author Andy Martignoni III, Brian Gerkey, Brendan Burns, Ben Grocholsky, Brad Kratochvil 00027 00028 #include <opencv/highgui.h> 00029 #include <ros/time.h> 00030 00031 #include "cmvision_color_blob_finder.h" 00032 00033 using namespace color_blob_track; 00034 00035 CMVisionColorBlobFinder::CMVisionColorBlobFinder() : 00036 debug_on_(false), width_(0), height_(0), color_filename_(""), uyvy_image_(NULL), blob_count_(0), vision_(NULL), mean_shift_on_(false), spatial_radius_(0), 00037 color_radius_(0) 00038 { 00039 } 00040 00041 CMVisionColorBlobFinder::~CMVisionColorBlobFinder() 00042 { 00043 if (vision_) 00044 { 00045 delete vision_; 00046 } 00047 } 00048 00049 bool CMVisionColorBlobFinder::initialize(ros::NodeHandle &node_handle) 00050 { 00051 00052 uyvy_image_ = NULL; 00053 width_ = 0; 00054 height_ = 0; 00055 00056 vision_ = new CMVision(); 00057 00058 blob_count_ = 0; 00059 blob_message_.blob_count = 0; 00060 00061 // Get the color file. This defines what colors to track 00062 if (!node_handle.getParam("/cmvision/color_file", color_filename_)) 00063 { 00064 ROS_ERROR("Could not find color calibration file name \"/cmvision/color_file\" in namespace: %s.", node_handle.getNamespace().c_str()); 00065 return false; 00066 } 00067 00068 // Get the level of debug output 00069 node_handle.param("/cmvision/debug_on", debug_on_, false); 00070 00071 // output regions with areas bigger than: 00072 node_handle.param("/cmvision/region_area", region_area_, 0); 00073 00074 // check whether mean shift is turned on 00075 if (!node_handle.getParam("/cmvision/mean_shift_on", mean_shift_on_)) 00076 { 00077 ROS_ERROR("Could not find mean shift flag \"/cmvision/mean_shift_on\" in namespace: %s.", node_handle.getNamespace().c_str()); 00078 return false; 00079 } 00080 else 00081 { 00082 if (!node_handle.getParam("/cmvision/spatial_radius_pix", spatial_radius_)) 00083 { 00084 ROS_ERROR("Could not get spatial_radius_pix from param server \"/cmvision/spatial_radius_pix\" in namespace: %s.", node_handle.getNamespace().c_str()); 00085 return false; 00086 } 00087 00088 if (!node_handle.getParam("/cmvision/color_radius_pix", color_radius_)) 00089 { 00090 ROS_ERROR("Could not get color_radius_pix from param server \"/cmvision/color_radius_pix\" in namespace: %s.", node_handle.getNamespace().c_str()); 00091 return false; 00092 } 00093 } 00094 00095 // Subscribe to an image stream 00096 image_subscriber_ = node_handle.subscribe("image", 1, &CMVisionColorBlobFinder::imageCB, this); 00097 00098 // Advertise our blobs 00099 blob_publisher_ = node_handle.advertise<cmvision::Blobs> ("blobs", 1); 00100 00101 if (debug_on_) 00102 { 00103 cvNamedWindow("Image"); 00104 } 00105 00106 return true; 00107 } 00108 00109 void CMVisionColorBlobFinder::imageCB(const sensor_msgs::ImageConstPtr& msg) 00110 { 00111 IplImage *cvImage; 00112 CvSize size; 00113 00114 const sensor_msgs::Image img = *msg; 00115 00116 // Timing 00117 // struct timeval timeofday; 00118 // gettimeofday(&timeofday,NULL); 00119 // ros::Time startt = ros::Time().fromNSec(1e9*timeofday.tv_sec + 1e3*timeofday.tv_usec); 00120 ros::WallTime startt = ros::WallTime::now(); 00121 00122 // Get the image as and RGB image 00123 if (img.encoding == "rgb8") 00124 { 00125 image_bridge_.fromImage(img, "bgr8"); 00126 std::cerr << "rgb8" << std::endl; 00127 } 00128 else 00129 { 00130 image_bridge_.fromImage(img, "bgr8"); 00131 std::cerr << "bgr8" << std::endl; 00132 } 00133 cvImage = image_bridge_.toIpl(); 00134 00135 size = cvGetSize(cvImage); 00136 00137 // this shouldn't change often 00138 if ((size.width != width_) || (size.height != height_)) 00139 { 00140 if (!(vision_->initialize(size.width, size.height))) 00141 { 00142 width_ = height_ = 0; 00143 ROS_ERROR("Vision init failed."); 00144 return; 00145 } 00146 00147 if (!color_filename_.empty()) 00148 { 00149 if (!vision_->loadOptions(color_filename_.c_str())) 00150 { 00151 ROS_ERROR("Error loading color file"); 00152 return; 00153 } 00154 } 00155 else 00156 { 00157 ROS_ERROR("No color file given. Use the \"mColorFile\" " 00158 "option in the configuration file."); 00159 return; 00160 } 00161 00162 width_ = size.width; 00163 height_ = size.height; 00164 00165 blob_message_.image_width = size.width; 00166 blob_message_.image_height = size.height; 00167 00168 if (uyvy_image_) 00169 { 00170 delete[] uyvy_image_; 00171 } 00172 uyvy_image_ = new uint8_t[width_ * height_ * 2]; 00173 00174 } 00175 00176 // Smooth the image, if turned on 00177 if (mean_shift_on_) 00178 { 00179 cvPyrMeanShiftFiltering(cvImage, cvImage, spatial_radius_, color_radius_); 00180 } 00181 00182 // Convert image to YUV color space 00183 rgb2uyvy((unsigned char *) cvImage->imageData, uyvy_image_, width_ * height_); 00184 00185 // Find the color blobs 00186 if (!vision_->processFrame(reinterpret_cast<image_pixel*> (uyvy_image_))) 00187 { 00188 ROS_ERROR("Frame error."); 00189 return; 00190 } 00191 00192 // if (img.encoding == "rgb8") 00193 // { 00194 // image_bridge_.fromImage(img, "bgr8"); 00195 // cvImage = image_bridge_.toIpl(); 00196 // } 00197 00198 // Get all the blobs 00199 blob_count_ = 0; 00200 for (int ch = 0; ch < CMV_MAX_COLORS; ++ch) 00201 { 00202 // Get the descriptive color 00203 rgb c = vision_->getColorVisual(ch); 00204 00205 // Grab the regions for this color 00206 CMVision::region* r = NULL; 00207 00208 for (r = vision_->getRegions(ch); r != NULL; r = r->next) 00209 { 00210 // Resize the blob message 00211 if (blob_count_ >= blob_message_.blobs.size()) 00212 { 00213 blob_message_.blobs.resize(blob_message_.blobs.size() + 1); 00214 } 00215 int cur_reg = (r->x2 - r->x1) * (r->y2 - r->y1); 00216 if (debug_on_ && (cur_reg > region_area_)) 00217 { 00218 cvRectangle(cvImage, cvPoint(r->x1, r->y1), cvPoint(r->x2, r->y2), CV_RGB(c.red, c.green, c.blue), 10); 00219 } 00220 00221 blob_message_.blobs[blob_count_].red = c.red; 00222 blob_message_.blobs[blob_count_].green = c.green; 00223 blob_message_.blobs[blob_count_].blue = c.blue; 00224 blob_message_.blobs[blob_count_].area = r->area; 00225 blob_message_.blobs[blob_count_].x = rint(r->cen_x + .5); 00226 blob_message_.blobs[blob_count_].y = rint(r->cen_y + .5); 00227 blob_message_.blobs[blob_count_].left = r->x1; 00228 blob_message_.blobs[blob_count_].right = r->x2; 00229 blob_message_.blobs[blob_count_].top = r->y1; 00230 blob_message_.blobs[blob_count_].bottom = r->y2; 00231 00232 blob_count_++; 00233 } 00234 } 00235 00236 if (debug_on_) 00237 { 00238 cvShowImage("Image", cvImage); 00239 cvWaitKey(3); 00240 } 00241 00242 blob_message_.blob_count = blob_count_; 00243 00244 blob_message_.header.stamp = ros::Time::now(); 00245 00246 blob_publisher_.publish(blob_message_); 00247 00248 // Timing 00249 // gettimeofday(&timeofday,NULL); 00250 // ros::Time endt = ros::Time().fromNSec(1e9*timeofday.tv_sec + 1e3*timeofday.tv_usec); 00251 ros::WallTime endt = ros::WallTime::now(); 00252 ros::WallDuration diff = endt - startt; 00253 ROS_DEBUG_STREAM_NAMED("cmvision", "Color Blob Detection duration " << diff.toSec()); 00254 }