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 <cstdio>
00030 #include <ros/ros.h>
00031 #include <ros/console.h>
00032 #include <pluginlib/class_loader.h>
00033
00034 #include <boost/algorithm/string.hpp>
00035
00036
00037 #include "nav_msgs/GetMap.h"
00038 #include "std_msgs/String.h"
00039 #include "geometry_msgs/Quaternion.h"
00040
00041 #include <Eigen/Geometry>
00042
00043 #include <QtGui/QApplication>
00044
00045 #include <hector_map_tools/HectorMapTools.h>
00046
00047 #include <hector_geotiff/geotiff_writer.h>
00048 #include <hector_geotiff/map_writer_plugin_interface.h>
00049
00050 #include <hector_nav_msgs/GetRobotTrajectory.h>
00051
00052 using namespace std;
00053
00054 namespace hector_geotiff{
00055
00059 class MapGenerator
00060 {
00061 public:
00062 MapGenerator()
00063 : geotiff_writer_(false)
00064 , pn_("~")
00065 , plugin_loader_(0)
00066 , running_saved_map_num_(0)
00067 {
00068 pn_.param("map_file_path", p_map_file_path_, std::string("."));
00069 geotiff_writer_.setMapFilePath(p_map_file_path_);
00070 geotiff_writer_.setUseUtcTimeSuffix(true);
00071
00072 pn_.param("map_file_base_name", p_map_file_base_name_, std::string());
00073
00074 pn_.param("draw_background_checkerboard", p_draw_background_checkerboard_, true);
00075 pn_.param("draw_free_space_grid", p_draw_free_space_grid_, true);
00076
00077 sys_cmd_sub_ = n_.subscribe("syscommand", 1, &MapGenerator::sysCmdCallback, this);
00078
00079 map_service_client_ = n_.serviceClient<nav_msgs::GetMap>("map");
00080
00081 path_service_client_ = n_.serviceClient<hector_nav_msgs::GetRobotTrajectory>("trajectory");
00082
00083
00084 double p_geotiff_save_period = 0.0;
00085 pn_.param("geotiff_save_period", p_geotiff_save_period, 0.0);
00086
00087 if(p_geotiff_save_period > 0.0){
00088
00089
00090 map_save_timer_ = pn_.createTimer(ros::Duration(p_geotiff_save_period), &MapGenerator::timerSaveGeotiffCallback, this, false );
00091 }
00092
00093
00094 pn_.param("plugins", p_plugin_list_, std::string(""));
00095
00096 std::vector<std::string> plugin_list;
00097 boost::algorithm::split(plugin_list, p_plugin_list_, boost::is_any_of("\t "));
00098
00099
00100 if ((plugin_list.size() > 0) && (plugin_list[0].length() > 0)){
00101 plugin_loader_ = new pluginlib::ClassLoader<hector_geotiff::MapWriterPluginInterface>("hector_geotiff", "hector_geotiff::MapWriterPluginInterface");
00102
00103 for (size_t i = 0; i < plugin_list.size(); ++i){
00104 try
00105 {
00106 boost::shared_ptr<hector_geotiff::MapWriterPluginInterface> tmp (plugin_loader_->createInstance(plugin_list[i]));
00107 tmp->initialize(plugin_loader_->getName(plugin_list[i]));
00108 plugin_vector_.push_back(tmp);
00109 }
00110 catch(pluginlib::PluginlibException& ex)
00111 {
00112 ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
00113 }
00114 }
00115 }else{
00116 ROS_INFO("No plugins loaded for geotiff node");
00117 }
00118
00119 ROS_INFO("Geotiff node started");
00120 }
00121
00122 ~MapGenerator()
00123 {
00124 if (plugin_loader_){
00125 delete plugin_loader_;
00126 }
00127 }
00128
00129 void writeGeotiff()
00130 {
00131 ros::Time start_time (ros::Time::now());
00132
00133 std::stringstream ssStream;
00134
00135 nav_msgs::GetMap srv_map;
00136 if (map_service_client_.call(srv_map))
00137 {
00138 ROS_INFO("GeotiffNode: Map service called successfully");
00139 const nav_msgs::OccupancyGrid& map (srv_map.response.map);
00140
00141 std::string map_file_name = p_map_file_base_name_;
00142 std::string competition_name;
00143 std::string team_name;
00144 std::string mission_name;
00145 std::string postfix;
00146 if (n_.getParamCached("/competition", competition_name) && !competition_name.empty()) map_file_name = map_file_name + "_" + competition_name;
00147 if (n_.getParamCached("/team", team_name) && !team_name.empty()) map_file_name = map_file_name + "_" + team_name;
00148 if (n_.getParamCached("/mission", mission_name) && !mission_name.empty()) map_file_name = map_file_name + "_" + mission_name;
00149 if (pn_.getParamCached("map_file_postfix", postfix) && !postfix.empty()) map_file_name = map_file_name + "_" + postfix;
00150 if (map_file_name.substr(0, 1) == "_") map_file_name = map_file_name.substr(1);
00151 if (map_file_name.empty()) map_file_name = "GeoTiffMap";
00152 geotiff_writer_.setMapFileName(map_file_name);
00153 bool transformSuccess = geotiff_writer_.setupTransforms(map);
00154
00155 if(!transformSuccess){
00156 ROS_INFO("Couldn't set map transform");
00157 return;
00158 }
00159
00160 geotiff_writer_.setupImageSize();
00161
00162 if (p_draw_background_checkerboard_){
00163 geotiff_writer_.drawBackgroundCheckerboard();
00164 }
00165
00166 geotiff_writer_.drawMap(map, p_draw_free_space_grid_);
00167 geotiff_writer_.drawCoords();
00168
00169
00170 }
00171 else
00172 {
00173 ROS_ERROR("Failed to call map service");
00174 return;
00175 }
00176
00177 for (size_t i = 0; i < plugin_vector_.size(); ++i){
00178 plugin_vector_[i]->draw(&geotiff_writer_);
00179 }
00180
00184
00185
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
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247 geotiff_writer_.writeGeotiffImage();
00248 running_saved_map_num_++;
00249
00250 ros::Duration elapsed_time (ros::Time::now() - start_time);
00251
00252 ROS_INFO("GeoTiff created in %f seconds", elapsed_time.toSec());
00253 }
00254
00255 void timerSaveGeotiffCallback(const ros::TimerEvent& e)
00256 {
00257 this->writeGeotiff();
00258 }
00259
00260 void sysCmdCallback(const std_msgs::String& sys_cmd)
00261 {
00262 if ( !(sys_cmd.data == "savegeotiff")){
00263 return;
00264 }
00265
00266 this->writeGeotiff();
00267 }
00268
00269 std::string p_map_file_path_;
00270 std::string p_map_file_base_name_;
00271 std::string p_plugin_list_;
00272 bool p_draw_background_checkerboard_;
00273 bool p_draw_free_space_grid_;
00274
00275
00276
00277 GeotiffWriter geotiff_writer_;
00278
00279 ros::ServiceClient map_service_client_;
00280 ros::ServiceClient object_service_client_;
00281 ros::ServiceClient path_service_client_;
00282
00283 ros::Subscriber sys_cmd_sub_;
00284
00285 ros::NodeHandle n_;
00286 ros::NodeHandle pn_;
00287
00288 std::vector<boost::shared_ptr<hector_geotiff::MapWriterPluginInterface> > plugin_vector_;
00289
00290 pluginlib::ClassLoader<hector_geotiff::MapWriterPluginInterface>* plugin_loader_;
00291
00292 ros::Timer map_save_timer_;
00293
00294 unsigned int running_saved_map_num_;
00295 };
00296
00297 }
00298
00299 int main(int argc, char** argv)
00300 {
00301 ros::init(argc, argv, "geotiff_node");
00302
00303 hector_geotiff::MapGenerator mg;
00304
00305
00306
00307
00308
00309
00310 ros::spin();
00311
00312 return 0;
00313 }
00314