$search
00001 /* 00002 * Copyright (c) 2010, Dejan Pangercic <dejan.pangercic@cs.tum.edu>, 00003 Zoltan-Csaba Marton <marton@cs.tum.edu>, Nico Blodow <blodow@cs.tum.edu> 00004 * All rights reserved. 00005 * 00006 * Redistribution and use in source and binary forms, with or without 00007 * modification, are permitted provided that the following conditions are met: 00008 * 00009 * * Redistributions of source code must retain the above copyright 00010 * notice, this list of conditions and the following disclaimer. 00011 * * Redistributions in binary form must reproduce the above copyright 00012 * notice, this list of conditions and the following disclaimer in the 00013 * documentation and/or other materials provided with the distribution. 00014 * * Neither the name of Willow Garage, Inc. nor the names of its 00015 * contributors may be used to endorse or promote products derived from 00016 * this software without specific prior written permission. 00017 * 00018 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00019 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00020 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00021 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00022 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00023 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00024 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00025 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00026 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00027 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00028 * POSSIBILITY OF SUCH DAMAGE. 00029 */ 00030 00031 00032 #include <image_algos/color_find_hsv.h> 00033 #include "highgui.h" 00034 00035 using namespace image_algos; 00036 00037 void ColorFindHSV::init (ros::NodeHandle& nh) 00038 { 00039 nh_ = nh; 00040 nh_.param("colors_yaml_file", colors_yaml_file_, colors_yaml_file_); 00041 result_image_pub_ = nh_.advertise<sensor_msgs::Image>("result", 0); 00042 if (colors_yaml_file_ == "") 00043 { 00044 ROS_ERROR("You have to provide file with colors"); 00045 exit(0); 00046 } 00047 std::ifstream fin(colors_yaml_file_.c_str()); 00048 YAML::Parser parser(fin); 00049 YAML::Node doc; 00050 parser.GetNextDocument(doc); 00051 threshold_values tv; 00052 for(unsigned i=0;i<doc.size();i++) 00053 { 00054 00055 doc[i]["sthreshold_min"] >> tv.sthreshold_min; 00056 doc[i]["sthreshold_max"] >> tv.sthreshold_max; 00057 doc[i]["hlower"] >> tv.hlower; 00058 doc[i]["hupper"] >> tv.hupper; 00059 color_values_map_[doc[i]["color"]] = tv; 00060 } 00061 ROS_INFO("[ColorFindHSV:] Map size: %ld", color_values_map_.size()); 00062 } 00063 00064 00065 void ColorFindHSV::pre () 00066 { 00067 color_ = boost::shared_ptr<OutputType> (new OutputType); 00068 } 00069 00070 00071 void ColorFindHSV::post () 00072 { 00073 //Nothing TODO 00074 } 00075 00076 00077 std::vector<std::string> ColorFindHSV::requires () 00078 { 00079 return std::vector<std::string>(); 00080 } 00081 00082 00083 std::vector<std::string> ColorFindHSV::provides () 00084 { 00085 return std::vector<std::string>(); 00086 } 00087 00088 00089 std::string ColorFindHSV::process (const boost::shared_ptr<const InputType> input) 00090 { 00091 ROS_INFO ("[ColorFindHSV:] Image message received on %s with width %d, height %d", default_input_topic().c_str (), input->width, input->height); 00092 int sthreshold_max, sthreshold_min; 00093 double hupper, hlower; 00094 int i, j; 00095 int height,width,step,channels; 00096 int stepmono,channelsmono; 00097 IplImage *frame = NULL; 00098 try 00099 { 00100 frame = bridge_.imgMsgToCv(input, "bgr8"); 00101 } 00102 catch (sensor_msgs::CvBridgeException error) 00103 { 00104 ROS_ERROR("Error converting top openCV image"); 00105 } 00106 00107 00108 IplImage *imageHSV = cvCloneImage(frame); 00109 IplImage *result = cvCreateImage(cvGetSize(frame), 8, 1); 00110 /*Converting the color space of the image....*/ 00111 cvCvtColor(frame,imageHSV,CV_BGR2HSV); 00112 //cvNamedWindow("image", CV_WINDOW_AUTOSIZE); 00113 //cvShowImage("image", frame); 00114 //cvWaitKey(3000); 00115 00116 uchar *data, *datamono; 00117 data = (uchar *)imageHSV->imageData; 00118 height = frame->height; 00119 width = frame->width; 00120 step = frame->widthStep; 00121 channels = frame->nChannels; 00122 datamono = (uchar *) result->imageData; 00123 stepmono = result->widthStep; 00124 channelsmono = result->nChannels; 00125 cvZero(result); 00126 std::map<std::string, threshold_values>::iterator it; 00127 for ( it=color_values_map_.begin(); it != color_values_map_.end(); it++) 00128 { 00129 cvZero(result); 00130 //set intensities to 0 00131 i=j=0; 00132 (*it).second.pixel_frequency = 0; 00133 sthreshold_min = (*it).second.sthreshold_min; 00134 sthreshold_max = (*it).second.sthreshold_max; 00135 hupper = (*it).second.hupper; 00136 hlower = (*it).second.hlower; 00137 //iterate over the image 00138 for (i = 0; i < height; i++) 00139 { 00140 for (j = 0; j < width; j++) 00141 { 00142 if( 00143 (data[(i) * step + j * channels] <= hupper) && 00144 (data[(i) * step + j * channels] >= hlower) //&& 00145 //(data[(i) * step + j * (channels) + 1] > sthreshold_min) && 00146 //(data[(i) * step + j * (channels) + 1] < sthreshold_max) 00147 ) 00148 { 00149 //datamono[(i) * stepmono + j * channelsmono] = 255; 00150 datamono[(i) * stepmono + j * channelsmono] = 255; 00151 } 00152 } 00153 } 00154 cvErode(result,result,0,1); 00155 cvDilate( result,result,0,1); 00156 i=j=0; 00157 for (i = 0; i < height; i++) 00158 { 00159 for (j = 0; j < width; j++) 00160 { 00161 if(datamono[(i) * stepmono + j * channelsmono] == 255) 00162 00163 { 00164 (*it).second.pixel_frequency++; 00165 } 00166 } 00167 } 00168 ROS_INFO("[ColorFindHSV:] Color: %s, Nr. of pixels: %d", (*it).first.c_str(), (*it).second.pixel_frequency); 00169 } 00170 00171 int max_freq = 0; 00172 std::string color(""); 00173 for ( it=color_values_map_.begin(); it != color_values_map_.end(); it++) 00174 if ((*it).second.pixel_frequency > max_freq) 00175 { 00176 max_freq = (*it).second.pixel_frequency; 00177 color = (*it).first; 00178 } 00179 ROS_INFO("[ColorFindHSV:] %s", color.c_str()); 00180 if (color != "") 00181 { 00182 color_->object_color = color; 00183 color_->perception_method = ros::this_node::getName();; 00184 color_->roi = *input; 00185 } 00186 try 00187 { 00188 result_image_pub_.publish(bridge_.cvToImgMsg(result, "passthrough")); 00189 } 00190 catch (sensor_msgs::CvBridgeException error) 00191 { 00192 ROS_ERROR("Error converting to ROS Image"); 00193 } 00194 00195 cvReleaseImage(&imageHSV); 00196 cvReleaseImage(&result); 00197 return std::string ("ok"); 00198 } 00199 00200 00201 boost::shared_ptr<const ColorFindHSV::OutputType> ColorFindHSV::output () 00202 { 00203 return color_; 00204 }; 00205 00206 00207 #ifdef CREATE_NODE 00208 int main (int argc, char* argv[]) 00209 { 00210 return standalone_node <ColorFindHSV> (argc, argv); 00211 } 00212 #endif 00213