Go to the documentation of this file.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 <geotiff_writer/map_writer_interface.h>
00030 #include <geotiff_writer/map_writer_plugin_interface.h>
00031
00032 #include <ros/ros.h>
00033 #include <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 namespace hector_worldmodel_geotiff_plugins {
00042
00043 using namespace hector_geotiff;
00044
00045 class MapWriterPlugin : public MapWriterPluginInterface
00046 {
00047 public:
00048 MapWriterPlugin();
00049 virtual ~MapWriterPlugin();
00050
00051 virtual void initialize(const std::string& name);
00052 virtual void draw(MapWriterInterface *interface) = 0;
00053
00054 protected:
00055 ros::NodeHandle nh_;
00056 ros::ServiceClient service_client_;
00057
00058 bool initialized_;
00059 std::string name_;
00060 bool draw_all_objects_;
00061 std::string class_id_;
00062 };
00063
00064 MapWriterPlugin::MapWriterPlugin()
00065 : initialized_(false)
00066 {}
00067
00068 MapWriterPlugin::~MapWriterPlugin()
00069 {}
00070
00071 void MapWriterPlugin::initialize(const std::string& name)
00072 {
00073 ros::NodeHandle plugin_nh("~/" + name);
00074 std::string service_name_;
00075
00076 plugin_nh.param("service_name", service_name_, std::string("worldmodel/get_object_model"));
00077 plugin_nh.param("draw_all_objects", draw_all_objects_, false);
00078 plugin_nh.param("class_id", class_id_, std::string());
00079
00080 service_client_ = nh_.serviceClient<worldmodel_msgs::GetObjectModel>(service_name_);
00081
00082 initialized_ = true;
00083 this->name_ = name;
00084 ROS_INFO_NAMED(name_, "Successfully initialized hector_geotiff MapWriter plugin %s.", name_.c_str());
00085 }
00086
00087 class VictimMapWriter : public MapWriterPlugin
00088 {
00089 public:
00090 virtual ~VictimMapWriter() {}
00091
00092 void draw(MapWriterInterface *interface)
00093 {
00094 if (!initialized_) return;
00095
00096 worldmodel_msgs::GetObjectModel data;
00097 if (!service_client_.call(data)) {
00098 ROS_ERROR_NAMED(name_, "Cannot draw victims, service %s failed", service_client_.getService().c_str());
00099 return;
00100 }
00101
00102 int counter = 0;
00103 for(worldmodel_msgs::ObjectModel::_objects_type::const_iterator it = data.response.model.objects.begin(); it != data.response.model.objects.end(); ++it) {
00104 const worldmodel_msgs::Object& object = *it;
00105 if (!draw_all_objects_ && object.state.state != worldmodel_msgs::ObjectState::CONFIRMED) continue;
00106 if (!class_id_.empty() && object.info.class_id != class_id_) continue;
00107
00108 Eigen::Vector2f coords;
00109 coords.x() = object.pose.pose.position.x;
00110 coords.y() = object.pose.pose.position.y;
00111 interface->drawObjectOfInterest(coords, boost::lexical_cast<std::string>(++counter), MapWriterInterface::Color(240,10,10));
00112 }
00113 }
00114 };
00115
00116 class QRCodeMapWriter : public MapWriterPlugin
00117 {
00118 public:
00119 virtual ~QRCodeMapWriter() {}
00120
00121
00122 void draw(MapWriterInterface *interface)
00123 {
00124 if (!initialized_) return;
00125
00126 worldmodel_msgs::GetObjectModel data;
00127 if (!service_client_.call(data)) {
00128 ROS_ERROR_NAMED(name_, "Cannot draw victims, service %s failed", service_client_.getService().c_str());
00129 return;
00130 }
00131
00132 std::string team_name;
00133 std::string mission_name;
00134 nh_.getParamCached("/team", team_name);
00135 nh_.getParamCached("/mission", mission_name);
00136
00137 boost::posix_time::ptime now = ros::Time::now().toBoost();
00138 boost::gregorian::date now_date(now.date());
00139 boost::posix_time::time_duration now_time(now.time_of_day().hours(), now.time_of_day().minutes(), now.time_of_day().seconds(), 0);
00140
00141 std::ofstream description_file((interface->getBasePathAndFileName() + "_qr.csv").c_str());
00142 if (description_file.is_open()) {
00143 if (!team_name.empty()) description_file << team_name << std::endl;
00144 description_file << now_date << "; " << now_time << std::endl;
00145 if (!mission_name.empty()) description_file << mission_name << std::endl;
00146 description_file << std::endl;
00147 }
00148
00149 int counter = 0;
00150 for(worldmodel_msgs::ObjectModel::_objects_type::const_iterator it = data.response.model.objects.begin(); it != data.response.model.objects.end(); ++it) {
00151 const worldmodel_msgs::Object& object = *it;
00152 if (!class_id_.empty() && object.info.class_id != class_id_) continue;
00153 if (!draw_all_objects_ && object.state.state != worldmodel_msgs::ObjectState::CONFIRMED) continue;
00154 if (object.state.state == worldmodel_msgs::ObjectState::DISCARDED) continue;
00155
00156 Eigen::Vector2f coords;
00157 coords.x() = object.pose.pose.position.x;
00158 coords.y() = object.pose.pose.position.y;
00159 interface->drawObjectOfInterest(coords, boost::lexical_cast<std::string>(++counter), MapWriterInterface::Color(10,10,240));
00160
00161 if (description_file.is_open()) {
00162 boost::posix_time::time_duration time_of_day(object.header.stamp.toBoost().time_of_day());
00163 boost::posix_time::time_duration time(time_of_day.hours(), time_of_day.minutes(), time_of_day.seconds(), 0);
00164 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;
00165 }
00166 }
00167
00168 description_file.close();
00169 }
00170 };
00171
00172 }
00173
00174
00175 #include <pluginlib/class_list_macros.h>
00176 PLUGINLIB_DECLARE_CLASS(hector_worldmodel_geotiff_plugins, VictimMapWriter, hector_worldmodel_geotiff_plugins::VictimMapWriter, hector_geotiff::MapWriterPluginInterface)
00177 PLUGINLIB_DECLARE_CLASS(hector_worldmodel_geotiff_plugins, QRCodeMapWriter, hector_worldmodel_geotiff_plugins::QRCodeMapWriter, hector_geotiff::MapWriterPluginInterface)
00178