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 #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
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
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
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
00289 float size = getSizeFromName(object.info.name);
00290
00291 if (size == -1.0f)
00292 return true;
00293
00294
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
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 }
00315
00316
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