00001 //================================================================================================= 00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Flight Systems and Automatic Control group, 00013 // TU Darmstadt, nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 //================================================================================================= 00028 #include <pcl/ModelCoefficients.h> 00029 #include <pcl/io/pcd_io.h> 00030 #include <pcl/point_types.h> 00031 #include <pcl/filters/extract_indices.h> 00032 #include <pcl/filters/passthrough.h> 00033 #include <pcl/features/normal_3d.h> 00034 #include <pcl/sample_consensus/method_types.h> 00035 #include <pcl/sample_consensus/model_types.h> 00036 #include <pcl/segmentation/sac_segmentation.h> 00037 00038 00039 #include <hector_geotiff/map_writer_interface.h> 00040 #include <hector_geotiff/map_writer_plugin_interface.h> 00041 00042 #include <ros/ros.h> 00043 #include <nav_msgs/GetMap.h> 00044 00045 #include <pluginlib/class_loader.h> 00046 #include <fstream> 00047 00048 #include <boost/date_time/posix_time/posix_time.hpp> 00049 #include <boost/date_time/gregorian/formatters.hpp> 00050 00051 #include <boost/tokenizer.hpp> 00052 00053 #include <opencv/highgui.h> 00054 #include<opencv/cv.h> 00055 00056 #include <hector_worldmodel_msgs/GetObjectModel.h> 00057 00058 namespace hector_barrel_geotiff_plugin { 00059 00060 using namespace hector_geotiff; 00061 00062 class SemanticMapWriterPlugin : public MapWriterPluginInterface 00063 { 00064 public: 00065 SemanticMapWriterPlugin(); 00066 virtual ~SemanticMapWriterPlugin(); 00067 00068 virtual void initialize(const std::string& name); 00069 virtual void draw(MapWriterInterface *interface) = 0; 00070 00071 protected: 00072 00073 ros::NodeHandle nh_; 00074 ros::ServiceClient service_client_; 00075 ros::ServiceClient service_client_no_; 00076 00077 00078 bool initialized_; 00079 std::string name_; 00080 bool draw_all_objects_; 00081 std::string class_id_; 00082 std::string pkg_path; 00083 double barrel_threshold; 00084 }; 00085 00086 SemanticMapWriterPlugin::SemanticMapWriterPlugin() 00087 : initialized_(false) 00088 {} 00089 00090 SemanticMapWriterPlugin::~SemanticMapWriterPlugin() 00091 {} 00092 00093 void SemanticMapWriterPlugin::initialize(const std::string& name) 00094 { 00095 ros::NodeHandle plugin_nh("~/" + name); 00096 std::string service_name_; 00097 00098 plugin_nh.param("service_name", service_name_, std::string("worldmodel/get_object_model")); 00099 plugin_nh.param("draw_all_objects", draw_all_objects_, false); 00100 plugin_nh.param("class_id", class_id_, std::string()); 00101 00102 service_client_ = nh_.serviceClient<hector_worldmodel_msgs::GetObjectModel>(service_name_); 00103 00104 initialized_ = true; 00105 this->name_ = name; 00106 ROS_INFO_NAMED(name_, "Successfully initialized hector_geotiff MapWriter plugin %s.", name_.c_str()); 00107 } 00108 00109 class BarrelMapWriter : public SemanticMapWriterPlugin 00110 { 00111 public: 00112 virtual ~BarrelMapWriter() {} 00113 00114 void draw(MapWriterInterface *interface) 00115 { 00116 if (!initialized_) return; 00117 00118 hector_worldmodel_msgs::GetObjectModel data; 00119 if (!service_client_.call(data)) { 00120 ROS_ERROR_NAMED(name_, "Cannot draw victims, service %s failed", service_client_.getService().c_str()); 00121 return; 00122 } 00123 00124 int counter =0; 00125 for(hector_worldmodel_msgs::ObjectModel::_objects_type::const_iterator it = data.response.model.objects.begin(); it != data.response.model.objects.end(); ++it) { 00126 const hector_worldmodel_msgs::Object& object = *it; 00127 if (!class_id_.empty() && object.info.class_id != class_id_) continue; 00128 Eigen::Vector2f coords; 00129 coords.x() = object.pose.pose.position.x; 00130 coords.y() = object.pose.pose.position.y; 00131 interface->drawObjectOfInterest(coords, boost::lexical_cast<std::string>(++counter), MapWriterInterface::Color(180,0,200)); 00132 00133 00134 } 00135 } 00136 }; 00137 00138 //class BarrelMapWriter : public SemanticMapWriterPlugin 00139 //{ 00140 //public: 00141 // virtual ~BarrelMapWriter() {} 00142 00143 // void draw(MapWriterInterface *interface) 00144 // { 00145 // if (!initialized_) return; 00146 00147 // nav_msgs::GetMap map_barrels; 00148 // nav_msgs::GetMap map_no_barrels; 00149 // if (!service_client_.call(map_barrels)) { 00150 // ROS_ERROR_NAMED(name_, "Cannot draw barrels, service %s failed", service_client_.getService().c_str()); 00151 // return; 00152 // } 00153 00154 00155 // cv::Mat cv_map_small, cv_map; 00156 00157 // cv_map_small= occupancyGridToCvMat(&map_barrels.response.map); 00158 00159 // cv::Mat cv_map_small_bw; 00160 // cv::threshold( cv_map_small, cv_map_small_bw, 0.1, 255, 0); 00161 00162 00163 // cv::copyMakeBorder( cv_map_small_bw, cv_map, 50, 50, 50, 50, cv::BORDER_CONSTANT, cv::Scalar(255) ); 00164 00165 00166 // cv::imshow("map_big", cv_map); 00167 00168 // cv::imwrite(pkg_path+"/templates/map.png",cv_map); 00169 // cv::waitKey(2000); 00170 // cv::Mat barrel_template = cv::imread(pkg_path+"/templates/barrel_3.png"); 00171 00172 // cv::Mat gref, gtpl, tmpl_gray; 00173 // cv::cvtColor(barrel_template, tmpl_gray, CV_BGR2GRAY); 00174 00175 00176 // gref = getSobel(cv_map); 00177 00178 // ROS_WARN("blablabla "); 00179 // cv::Mat ref = gref; 00180 // gtpl = getSobel(tmpl_gray); 00181 // ROS_WARN("blublublu "); 00182 // cv::Mat tpl = gtpl; 00185 00186 // if (ref.empty() || tpl.empty()) { 00187 // ROS_WARN("Empty map layer for barrels"); 00188 // return; 00189 // } 00190 // else{ 00191 00192 // cv::Mat res(ref.rows-tpl.rows+1, ref.cols-tpl.cols+1, CV_32FC1); 00193 // cv::matchTemplate(gref, gtpl, res, CV_TM_SQDIFF_NORMED); 00194 00195 // cv::threshold(res, res, barrel_threshold, 1., CV_THRESH_TOZERO); 00196 00197 // std::vector<cv::Point> barrel_locations; 00198 // while (true) 00199 // { 00200 // double minval, maxval, threshold = barrel_threshold; 00201 // cv::Point minloc, maxloc; 00202 // cv::minMaxLoc(res, &minval, &maxval, &minloc, &maxloc); 00203 00204 // ROS_INFO("max %d, %d",maxloc.x,maxloc.y); 00205 // if (maxval >= threshold) 00206 // { 00207 // cv::rectangle( 00208 // ref, 00209 // maxloc, 00210 // cv::Point(maxloc.x + tpl.cols, maxloc.y + tpl.rows), 00211 // cv::Scalar(255,0,0), 2 00212 // ); 00213 // barrel_locations.push_back(maxloc); 00214 // cv::floodFill(res, maxloc, cv::Scalar(0), 0, cv::Scalar(.1), cv::Scalar(1.)); 00215 // } 00216 // else 00217 // break; 00218 // } 00219 // std::cout << barrel_locations <<std::endl; 00220 // //cv::namedWindow( "reference", CV_WINDOW_AUTOSIZE ); 00221 00222 // cv::imshow("result", ref); 00223 // cv::waitKey(2000); 00224 // int counter = 0; 00225 // double pixelsPerMapMeter = 1.0f / map_barrels.response.map.info.resolution; 00226 00227 // Eigen::Vector2f origin = Eigen::Vector2f(map_barrels.response.map.info.origin.position.x, map_barrels.response.map.info.origin.position.y); 00228 // std::cout<< "origin :" <<origin <<std::endl; 00229 // for (int i=0;i<barrel_locations.size();i++){ 00230 // cv::Point barrel_point = barrel_locations.at(i); 00231 // Eigen::Vector2f coords; 00232 // coords.x() = (barrel_point.x+50)/pixelsPerMapMeter; 00233 // coords.y() = (barrel_point.y+50)/pixelsPerMapMeter; 00234 // interface->drawObjectOfInterest(coords, boost::lexical_cast<std::string>(++counter), MapWriterInterface::Color(0,255,0)); 00235 00236 // std::cout<< "pixels per map !!!!!!!!!!!!!!!!!!!!!!!:"<<pixelsPerMapMeter <<std::endl; 00237 // std::cout<< "coords !!!!!!!!!!!!!!!!!!!!!!!:"<<coords <<std::endl; 00238 // interface->drawObjectOfInterest(coords, boost::lexical_cast<std::string>(++counter), MapWriterInterface::Color(0,255,0)); 00239 00240 // } 00241 // } 00242 // } 00243 00244 // cv::Mat getSobel(cv::Mat src_gray){ 00245 00246 // cv::Mat grad; 00247 00248 // int scale = 1; 00249 // int delta = 0; 00250 // int ddepth = CV_16S; 00251 00252 // int c; 00253 00254 // cv::GaussianBlur( src_gray, src_gray, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT ); 00255 00256 // /// Convert it to gray 00257 00258 // /// Create window 00259 00260 00261 // /// Generate grad_x and grad_y 00262 // cv::Mat grad_x, grad_y; 00263 // cv::Mat abs_grad_x, abs_grad_y; 00264 00265 // /// Gradient X 00266 // //Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT ); 00267 // cv::Sobel( src_gray, grad_x, ddepth, 1, 0, 3, scale, delta, cv::BORDER_DEFAULT ); 00268 // convertScaleAbs( grad_x, abs_grad_x ); 00269 00270 // /// Gradient Y 00271 // //Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT ); 00272 // Sobel( src_gray, grad_y, ddepth, 0, 1, 3, scale, delta, cv::BORDER_DEFAULT ); 00273 // convertScaleAbs( grad_y, abs_grad_y ); 00274 00275 // /// Total Gradient (approximate) 00276 // cv::addWeighted( abs_grad_x, 0.5, abs_grad_y, 0.5, 0, grad ); 00277 // ROS_WARN("gradient "); 00278 // // cv::imshow( "sobel", grad ); 00279 // return grad; 00280 // } 00281 00282 // cv::Mat occupancyGridToCvMat(const nav_msgs::OccupancyGrid *map) 00283 // { 00284 // uint8_t *data = (uint8_t*) map->data.data(), 00285 // testpoint = data[0]; 00286 // bool mapHasPoints = false; 00287 00288 // cv::Mat im(map->info.height, map->info.width, CV_8UC1); 00289 00290 // // transform the map in the same way the map_saver component does 00291 // for (size_t i=0; i<map->data.size(); i++) 00292 // { 00293 // if (data[i] == 0) im.data[i] = 254; 00294 // else if (data[i] == 100) im.data[i] = 0; 00295 // else im.data[i] = 205; 00296 00297 // // just check if there is actually something in the map 00298 // if (i!=0) mapHasPoints = mapHasPoints || (data[i] != testpoint); 00299 // testpoint = data[i]; 00300 // } 00301 00302 // // sanity check 00303 // if (!mapHasPoints) { ROS_WARN("map is empty, ignoring update."); } 00304 00305 // return im; 00306 // } 00307 //}; 00308 00309 00310 00311 } // namespace 00312 00313 //register this planner as a MapWriterPluginInterface plugin 00314 #include <pluginlib/class_list_macros.h> 00315 PLUGINLIB_EXPORT_CLASS(hector_barrel_geotiff_plugin::BarrelMapWriter, hector_geotiff::MapWriterPluginInterface) 00316