geotiff_node.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Stefan Kohlbrecher, 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 Simulation, Systems Optimization and Robotics
00013 //       group, 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 <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     //object_service_client_ = n_.serviceClient<worldmodel_msgs::GetObjectModel>("worldmodel/get_object_model");
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       //ros::Timer timer = pn_.createTimer(ros::Duration(p_geotiff_save_period), &MapGenerator::timerSaveGeotiffCallback, false);
00089       //publish_trajectory_timer_ = private_nh.createTimer(ros::Duration(1.0 / p_trajectory_publish_rate_), &PathContainer::publishTrajectoryTimerCallback, this, false);
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     //We always have at least one element containing "" in the string list
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       //ROS_INFO("Sum: %ld", (long int)srv.response.sum);
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     if (req_object_model_){
00186       worldmodel_msgs::GetObjectModel srv_objects;
00187       if (object_service_client_.call(srv_objects))
00188       {
00189         ROS_INFO("GeotiffNode: Object service called successfully");
00190 
00191         const worldmodel_msgs::ObjectModel& objects_model (srv_objects.response.model);
00192 
00193         size_t size = objects_model.objects.size();
00194 
00195 
00196         unsigned int victim_num  = 1;
00197 
00198         for (size_t i = 0; i < size; ++i){
00199           const worldmodel_msgs::Object& object (objects_model.objects[i]);
00200 
00201           if (object.state.state == worldmodel_msgs::ObjectState::CONFIRMED){
00202             geotiff_writer_.drawVictim(Eigen::Vector2f(object.pose.pose.position.x,object.pose.pose.position.y),victim_num);
00203             victim_num++;
00204           }
00205         }
00206       }
00207       else
00208       {
00209         ROS_ERROR("Failed to call objects service");
00210       }
00211     }
00212     */
00213 
00214     /*
00215     hector_nav_msgs::GetRobotTrajectory srv_path;
00216 
00217     if (path_service_client_.call(srv_path))
00218     {
00219       ROS_INFO("GeotiffNode: Path service called successfully");
00220 
00221       std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses);
00222 
00223       size_t size = traj_vector.size();
00224 
00225       std::vector<Eigen::Vector2f> pointVec;
00226       pointVec.resize(size);
00227 
00228       for (size_t i = 0; i < size; ++i){
00229         const geometry_msgs::PoseStamped& pose (traj_vector[i]);
00230 
00231         pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y);
00232       }
00233 
00234       if (size > 0){
00235         //Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z);
00236         Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f);
00237         geotiff_writer_.drawPath(startVec, pointVec);
00238       }
00239     }
00240     else
00241     {
00242       ROS_ERROR("Failed to call path service");
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   //double p_geotiff_save_period_;
00276 
00277   GeotiffWriter geotiff_writer_;
00278 
00279   ros::ServiceClient map_service_client_;// = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");
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   //ros::NodeHandle pn_;
00306   //double p_geotiff_save_period = 60.0f;
00307   //pn_.param("geotiff_save_period", p_geotiff_save_period, 60.0);
00308   //ros::Timer timer = pn_.createTimer(ros::Duration(p_geotiff_save_period), &MapGenerator::timerSaveGeotiffCallback, &mg, false);
00309 
00310   ros::spin();
00311 
00312   return 0;
00313 }
00314 


hector_geotiff
Author(s): Stefan Kohlbrecher
autogenerated on Mon Jun 27 2016 04:57:28