$search
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 <geotiff_writer/geotiff_writer.h> 00048 #include <geotiff_writer/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("GeoTiffMap")); 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_->createClassInstance(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 mission_name; 00143 if (n_.getParamCached("/mission", mission_name) && !mission_name.empty()) map_file_name = map_file_name + "_" + mission_name; 00144 geotiff_writer_.setMapFileName(map_file_name); 00145 bool transformSuccess = geotiff_writer_.setupTransforms(map); 00146 00147 if(!transformSuccess){ 00148 ROS_INFO("Couldn't set map transform"); 00149 return; 00150 } 00151 00152 geotiff_writer_.setupImageSize(); 00153 00154 if (p_draw_background_checkerboard_){ 00155 geotiff_writer_.drawBackgroundCheckerboard(); 00156 } 00157 00158 geotiff_writer_.drawMap(map, p_draw_free_space_grid_); 00159 geotiff_writer_.drawCoords(); 00160 00161 //ROS_INFO("Sum: %ld", (long int)srv.response.sum); 00162 } 00163 else 00164 { 00165 ROS_ERROR("Failed to call map service"); 00166 return; 00167 } 00168 00169 for (size_t i = 0; i < plugin_vector_.size(); ++i){ 00170 plugin_vector_[i]->draw(&geotiff_writer_); 00171 } 00172 00176 /* 00177 if (req_object_model_){ 00178 worldmodel_msgs::GetObjectModel srv_objects; 00179 if (object_service_client_.call(srv_objects)) 00180 { 00181 ROS_INFO("GeotiffNode: Object service called successfully"); 00182 00183 const worldmodel_msgs::ObjectModel& objects_model (srv_objects.response.model); 00184 00185 size_t size = objects_model.objects.size(); 00186 00187 00188 unsigned int victim_num = 1; 00189 00190 for (size_t i = 0; i < size; ++i){ 00191 const worldmodel_msgs::Object& object (objects_model.objects[i]); 00192 00193 if (object.state.state == worldmodel_msgs::ObjectState::CONFIRMED){ 00194 geotiff_writer_.drawVictim(Eigen::Vector2f(object.pose.pose.position.x,object.pose.pose.position.y),victim_num); 00195 victim_num++; 00196 } 00197 } 00198 } 00199 else 00200 { 00201 ROS_ERROR("Failed to call objects service"); 00202 } 00203 } 00204 */ 00205 00206 hector_nav_msgs::GetRobotTrajectory srv_path; 00207 00208 if (path_service_client_.call(srv_path)) 00209 { 00210 ROS_INFO("GeotiffNode: Path service called successfully"); 00211 00212 std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses); 00213 00214 size_t size = traj_vector.size(); 00215 00216 std::vector<Eigen::Vector2f> pointVec; 00217 pointVec.resize(size); 00218 00219 for (size_t i = 0; i < size; ++i){ 00220 const geometry_msgs::PoseStamped& pose (traj_vector[i]); 00221 00222 pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y); 00223 } 00224 00225 if (size > 0){ 00226 //Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z); 00227 Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f); 00228 geotiff_writer_.drawPath(startVec, pointVec); 00229 } 00230 } 00231 else 00232 { 00233 ROS_ERROR("Failed to call path service"); 00234 } 00235 00236 00237 00238 geotiff_writer_.writeGeotiffImage(); 00239 running_saved_map_num_++; 00240 00241 ros::Duration elapsed_time (ros::Time::now() - start_time); 00242 00243 ROS_INFO("GeoTiff created in %f seconds", elapsed_time.toSec()); 00244 } 00245 00246 void timerSaveGeotiffCallback(const ros::TimerEvent& e) 00247 { 00248 this->writeGeotiff(); 00249 } 00250 00251 void sysCmdCallback(const std_msgs::String& sys_cmd) 00252 { 00253 if ( !(sys_cmd.data == "savegeotiff")){ 00254 return; 00255 } 00256 00257 this->writeGeotiff(); 00258 } 00259 00260 std::string p_map_file_path_; 00261 std::string p_map_file_base_name_; 00262 std::string p_plugin_list_; 00263 bool p_draw_background_checkerboard_; 00264 bool p_draw_free_space_grid_; 00265 00266 //double p_geotiff_save_period_; 00267 00268 GeotiffWriter geotiff_writer_; 00269 00270 ros::ServiceClient map_service_client_;// = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints"); 00271 ros::ServiceClient object_service_client_; 00272 ros::ServiceClient path_service_client_; 00273 00274 ros::Subscriber sys_cmd_sub_; 00275 00276 ros::NodeHandle n_; 00277 ros::NodeHandle pn_; 00278 00279 std::vector<boost::shared_ptr<hector_geotiff::MapWriterPluginInterface> > plugin_vector_; 00280 00281 pluginlib::ClassLoader<hector_geotiff::MapWriterPluginInterface>* plugin_loader_; 00282 00283 ros::Timer map_save_timer_; 00284 00285 unsigned int running_saved_map_num_; 00286 }; 00287 00288 } 00289 00290 int main(int argc, char** argv) 00291 { 00292 ros::init(argc, argv, "geotiff_node"); 00293 00294 hector_geotiff::MapGenerator mg; 00295 00296 //ros::NodeHandle pn_; 00297 //double p_geotiff_save_period = 60.0f; 00298 //pn_.param("geotiff_save_period", p_geotiff_save_period, 60.0); 00299 //ros::Timer timer = pn_.createTimer(ros::Duration(p_geotiff_save_period), &MapGenerator::timerSaveGeotiffCallback, &mg, false); 00300 00301 ros::spin(); 00302 00303 return 0; 00304 } 00305