worldmodel_geotiff_plugins.cpp
Go to the documentation of this file.
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 
00029 #include <hector_geotiff/map_writer_interface.h>
00030 #include <hector_geotiff/map_writer_plugin_interface.h>
00031 
00032 #include <ros/ros.h>
00033 #include <hector_worldmodel_msgs/GetObjectModel.h>
00034 
00035 #include <pluginlib/class_loader.h>
00036 #include <fstream>
00037 
00038 #include <boost/date_time/posix_time/posix_time.hpp>
00039 #include <boost/date_time/gregorian/formatters.hpp>
00040 
00041 #include <boost/tokenizer.hpp>
00042 
00043 namespace hector_worldmodel_geotiff_plugins {
00044 
00045 using namespace hector_geotiff;
00046 
00047 class MapWriterPlugin : public MapWriterPluginInterface
00048 {
00049 public:
00050   MapWriterPlugin();
00051   virtual ~MapWriterPlugin();
00052 
00053   virtual void initialize(const std::string& name);
00054   virtual void draw(MapWriterInterface *interface) = 0;
00055 
00056 protected:
00057 
00058   ros::NodeHandle nh_;
00059   ros::ServiceClient service_client_;
00060 
00061   bool initialized_;
00062   std::string name_;
00063   bool draw_all_objects_;
00064   std::string class_id_;
00065 };
00066 
00067 MapWriterPlugin::MapWriterPlugin()
00068   : initialized_(false)
00069 {}
00070 
00071 MapWriterPlugin::~MapWriterPlugin()
00072 {}
00073 
00074 void MapWriterPlugin::initialize(const std::string& name)
00075 {
00076   ros::NodeHandle plugin_nh("~/" + name);
00077   std::string service_name_;
00078 
00079   plugin_nh.param("service_name", service_name_, std::string("worldmodel/get_object_model"));
00080   plugin_nh.param("draw_all_objects", draw_all_objects_, false);
00081   plugin_nh.param("class_id", class_id_, std::string());
00082 
00083   service_client_ = nh_.serviceClient<hector_worldmodel_msgs::GetObjectModel>(service_name_);
00084 
00085   initialized_ = true;
00086   this->name_ = name;
00087   ROS_INFO_NAMED(name_, "Successfully initialized hector_geotiff MapWriter plugin %s.", name_.c_str());
00088 }
00089 
00090 class VictimMapWriter : public MapWriterPlugin
00091 {
00092 public:
00093   virtual ~VictimMapWriter() {}
00094 
00095   void draw(MapWriterInterface *interface)
00096   {
00097     if (!initialized_) return;
00098 
00099     hector_worldmodel_msgs::GetObjectModel data;
00100     if (!service_client_.call(data)) {
00101       ROS_ERROR_NAMED(name_, "Cannot draw victims, service %s failed", service_client_.getService().c_str());
00102       return;
00103     }
00104 
00105     std::string team_name;
00106     std::string country;
00107     std::string mission_name;
00108     nh_.getParamCached("/team", team_name);
00109     nh_.getParamCached("/country", country);
00110     nh_.getParamCached("/mission", mission_name);
00111 
00112     boost::posix_time::ptime now = ros::Time::now().toBoost();
00113     boost::gregorian::date now_date(now.date());
00114     boost::posix_time::time_duration now_time(now.time_of_day().hours(), now.time_of_day().minutes(), now.time_of_day().seconds(), 0);
00115 
00116     std::ofstream description_file((interface->getBasePathAndFileName() + "_victims.csv").c_str());
00117     if (description_file.is_open()) {
00118       description_file << "\"victims\"" << std::endl;
00119       description_file << "\"1.0\"" << std::endl;
00120       if (!team_name.empty()) description_file << "\"" << team_name << "\"" << std::endl;
00121       if (!country.empty()) description_file << "\"" << country << "\"" << std::endl;
00122       description_file << "\"" << now_date << "\"" << std::endl;
00123       description_file << "\"" << now_time << "\"" << std::endl;
00124       if (!mission_name.empty()) description_file << "\"" << mission_name << "\"" << std::endl;
00125       description_file << std::endl;
00126       description_file << "id,time,name,x,y,z" << std::endl;
00127     }
00128 
00129     int counter = 0;
00130     for(hector_worldmodel_msgs::ObjectModel::_objects_type::const_iterator it = data.response.model.objects.begin(); it != data.response.model.objects.end(); ++it) {
00131       const hector_worldmodel_msgs::Object& object = *it;
00132       if (!draw_all_objects_ && object.state.state != hector_worldmodel_msgs::ObjectState::CONFIRMED) continue;
00133       if (!class_id_.empty() && object.info.class_id != class_id_) continue;
00134 
00135       Eigen::Vector2f coords;
00136       coords.x() = object.pose.pose.position.x;
00137       coords.y() = object.pose.pose.position.y;
00138       interface->drawObjectOfInterest(coords, boost::lexical_cast<std::string>(++counter), MapWriterInterface::Color(240,10,10));
00139 
00140       if (description_file.is_open()) {
00141         boost::posix_time::time_duration time_of_day(object.header.stamp.toBoost().time_of_day());
00142         boost::posix_time::time_duration time(time_of_day.hours(), time_of_day.minutes(), time_of_day.seconds(), 0);
00143         description_file << counter << "," << time << "," << object.info.object_id << "," << object.pose.pose.position.x << "," << object.pose.pose.position.y << "," << object.pose.pose.position.z << std::endl;
00144       }
00145     }
00146   }
00147 };
00148 
00149 class QRCodeMapWriter : public MapWriterPlugin
00150 {
00151 public:
00152   virtual ~QRCodeMapWriter() {}
00153 
00154   void draw(MapWriterInterface *interface)
00155   {
00156     if (!initialized_) return;
00157 
00158     hector_worldmodel_msgs::GetObjectModel data;
00159     if (!service_client_.call(data)) {
00160       ROS_ERROR_NAMED(name_, "Cannot draw victims, service %s failed", service_client_.getService().c_str());
00161       return;
00162     }
00163 
00164     std::string team_name;
00165     std::string country;
00166     std::string mission_name;
00167     nh_.getParamCached("/team", team_name);
00168     nh_.getParamCached("/country", country);
00169     nh_.getParamCached("/mission", mission_name);
00170 
00171     boost::posix_time::ptime now = ros::Time::now().toBoost();
00172     boost::gregorian::date now_date(now.date());
00173     boost::posix_time::time_duration now_time(now.time_of_day().hours(), now.time_of_day().minutes(), now.time_of_day().seconds(), 0);
00174 
00175     std::ofstream description_file((interface->getBasePathAndFileName() + "_qr.csv").c_str());
00176     if (description_file.is_open()) {
00177       description_file << "\"qr codes\"" << std::endl;
00178       description_file << "\"1.0\"" << std::endl;
00179       if (!team_name.empty()) description_file << "\"" << team_name << "\"" << std::endl;
00180       if (!country.empty()) description_file << "\"" << country << "\"" << std::endl;
00181       description_file << "\"" << now_date << "\"" << std::endl;
00182       description_file << "\"" << now_time << "\"" << std::endl;
00183       if (!mission_name.empty()) description_file << "\"" << mission_name << "\"" << std::endl;
00184       description_file << std::endl;
00185       description_file << "id,time,text,x,y,z" << std::endl;
00186     }
00187 
00188 //    hector_worldmodel_msgs::Object test_obj;
00189 //    test_obj.state.state = hector_worldmodel_msgs::ObjectState::CONFIRMED;
00190 
00191 //    test_obj.info.class_id = "qrcode";
00192 //    test_obj.info.name = "Y_25_arena_2.4mm_Yaounde";
00193 //    test_obj.info.object_id = "qrcode_0";
00194 //    data.response.model.objects.push_back(test_obj);
00195 
00196 //    test_obj.info.class_id = "qrcode";
00197 //    test_obj.info.name = "Y_25_arena_6mm_yuccas";
00198 //    test_obj.info.object_id = "qrcode_1";
00199 //    data.response.model.objects.push_back(test_obj);
00200 
00201 //    test_obj.info.class_id = "qrcode";
00202 //    test_obj.info.name = "Y_23_arena_6mm_yoked";
00203 //    test_obj.info.object_id = "qrcode_0";
00204 //    test_obj.pose.pose.position.x = 0.3;
00205 //    data.response.model.objects.push_back(test_obj);
00206 
00207 //    test_obj.info.class_id = "qrcode";
00208 //    test_obj.info.name = "Y_28_arena_6mm_yukking";
00209 //    test_obj.info.object_id = "qrcode_0";
00210 //    test_obj.pose.pose.position.x = 0.3;
00211 //    data.response.model.objects.push_back(test_obj);
00212 
00213 //    test_obj.info.class_id = "qrcode";
00214 //    test_obj.info.name = "Y_23_arena_6mm_yoked";
00215 //    test_obj.info.object_id = "qrcode_0";
00216 //    test_obj.pose.pose.position.x = 0.3;
00217 //    data.response.model.objects.push_back(test_obj);
00218 
00219 //    test_obj.info.class_id = "qrcode";
00220 //    test_obj.info.name = "Y_13_arena_6mm_Yorkshire";
00221 //    test_obj.info.object_id = "qrcode_0";
00222 //    test_obj.pose.pose.position.x = 0.3;
00223 //    data.response.model.objects.push_back(test_obj);
00224 
00225 //    test_obj.info.class_id = "qrcode";
00226 //    test_obj.info.name = "Y_11_arena_6mm_yakking";
00227 //    test_obj.info.object_id = "qrcode_0";
00228 //    test_obj.pose.pose.position.x = 0.3;
00229 //    data.response.model.objects.push_back(test_obj);
00230 
00231 //    test_obj.info.class_id = "qrcode";
00232 //    test_obj.info.name = "VictYel_1_yelled";
00233 //    test_obj.info.object_id = "qrcode_0";
00234 //    test_obj.pose.pose.position.x = 0.3;
00235 //    data.response.model.objects.push_back(test_obj);
00236 
00237     int counter = 0;
00238     for(hector_worldmodel_msgs::ObjectModel::_objects_type::const_iterator it = data.response.model.objects.begin(); it != data.response.model.objects.end(); ++it) {
00239       const hector_worldmodel_msgs::Object& object = *it;
00240       if (!class_id_.empty() && object.info.class_id != class_id_) continue;
00241       if (!draw_all_objects_ && object.state.state != hector_worldmodel_msgs::ObjectState::CONFIRMED) continue;
00242       if (object.state.state == hector_worldmodel_msgs::ObjectState::DISCARDED) continue;
00243 
00244       ++counter;
00245 
00246       // add only largest qr codes into geotiff
00247       if (isLargest(object, data.response.model.objects)) {
00248           Eigen::Vector2f coords;
00249           coords.x() = object.pose.pose.position.x;
00250           coords.y() = object.pose.pose.position.y;
00251           interface->drawObjectOfInterest(coords, boost::lexical_cast<std::string>(counter), MapWriterInterface::Color(10,10,240));
00252       }
00253 
00254       if (description_file.is_open()) {
00255         boost::posix_time::time_duration time_of_day(object.header.stamp.toBoost().time_of_day());
00256         boost::posix_time::time_duration time(time_of_day.hours(), time_of_day.minutes(), time_of_day.seconds(), 0);
00257         description_file << counter << "," << time << "," << object.info.name << "," << object.pose.pose.position.x << "," << object.pose.pose.position.y << "," << object.pose.pose.position.z << std::endl;
00258       }
00259     }
00260 
00261     description_file.close();
00262   }
00263 
00264 protected:
00265   float getSizeFromName(const std::string& name)
00266   {
00267     try {
00268       boost::tokenizer<boost::char_separator<char> > tokens(name, boost::char_separator<char>("_"));
00269       boost::tokenizer<boost::char_separator<char> >::const_iterator it = tokens.begin();
00270 
00271       if (it == tokens.end())
00272         return -1.0f;
00273 
00274       for (unsigned int i = 0; i < 3; i++) {
00275         if (++it == tokens.end())
00276           return -1.0f;
00277       }
00278 
00279       return it->size() > 2 ? boost::lexical_cast<float>(it->substr(0, it->size()-2)) : -1.0f;
00280     }
00281     catch (boost::bad_lexical_cast&) {
00282       return -1.0f;
00283     }
00284   }
00285 
00286   bool isLargest(const hector_worldmodel_msgs::Object& object, const std::vector<hector_worldmodel_msgs::Object>& objects )
00287   {
00288     // determine size of qr code
00289     float size = getSizeFromName(object.info.name);
00290 
00291     if (size == -1.0f) // QR does not include size information
00292       return true;
00293 
00294     // compare size of other qr codes
00295     for (hector_worldmodel_msgs::ObjectModel::_objects_type::const_iterator it = objects.begin(); it != objects.end(); ++it) {
00296       const hector_worldmodel_msgs::Object& object2 = *it;
00297 
00298       float dist_sqr = (object.pose.pose.position.x-object2.pose.pose.position.x) * (object.pose.pose.position.x-object2.pose.pose.position.x)
00299                       +(object.pose.pose.position.y-object2.pose.pose.position.y) * (object.pose.pose.position.y-object2.pose.pose.position.y);
00300 
00301       // check if both qrcodes are at same position+tolerance
00302       if (dist_sqr < 0.75f) {
00303         float size2 = getSizeFromName(object2.info.name);
00304         if (size2 > size) {
00305           return false;
00306         }
00307       }
00308     }
00309 
00310     return true;
00311   }
00312 };
00313 
00314 } // namespace
00315 
00316 //register this planner as a MapWriterPluginInterface plugin
00317 #include <pluginlib/class_list_macros.h>
00318 PLUGINLIB_EXPORT_CLASS(hector_worldmodel_geotiff_plugins::VictimMapWriter, hector_geotiff::MapWriterPluginInterface)
00319 PLUGINLIB_EXPORT_CLASS(hector_worldmodel_geotiff_plugins::QRCodeMapWriter, hector_geotiff::MapWriterPluginInterface)
00320 


hector_worldmodel_geotiff_plugins
Author(s): Johannes Meyer
autogenerated on Thu Jun 6 2019 20:20:17