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 #ifndef _GEOTIFFWRITER_H__
00030 #define _GEOTIFFWRITER_H__
00031
00032 #include "map_writer_interface.h"
00033
00034 #include <Eigen/Geometry>
00035
00036 #include <nav_msgs/OccupancyGrid.h>
00037 #include <nav_msgs/MapMetaData.h>
00038
00039 #include <QtGui/QImage>
00040 #include <QtGui/QApplication>
00041 #include <QtGui/QFont>
00042 #include <QtGui/QPen>
00043
00044 #include <hector_map_tools/HectorMapTools.h>
00045
00046
00047 namespace hector_geotiff{
00048
00049
00050 class GeotiffWriter : public MapWriterInterface
00051 {
00052 public:
00053 GeotiffWriter(bool useCheckerboardCacheIn = false);
00054 virtual ~GeotiffWriter();
00055
00056
00057
00058 void setMapFileName(const std::string& mapFileName);
00059 void setMapFilePath(const std::string& mapFilePath);
00060 void setUseUtcTimeSuffix(bool useSuffix);
00061
00062 void setupImageSize();
00063 bool setupTransforms(const nav_msgs::OccupancyGrid& map);
00064 void drawBackgroundCheckerboard();
00065 void drawMap(const nav_msgs::OccupancyGrid& map, bool draw_explored_space_grid = true);
00066 void drawObjectOfInterest(const Eigen::Vector2f& coords, const std::string& txt, const Color& color);
00067 inline virtual void drawPath(const Eigen::Vector3f& start, const std::vector<Eigen::Vector2f>& points){
00068 drawPath(start, points, 120,0,240);
00069 }
00070 void drawPath(const Eigen::Vector3f& start, const std::vector<Eigen::Vector2f>& points, int color_r, int color_g, int color_b);
00071 void drawCoords();
00072 std::string getBasePathAndFileName() const;
00073 void writeGeotiffImage();
00074
00075
00076 protected:
00077
00078 void transformPainterToImgCoords(QPainter& painter);
00079 void drawCross(QPainter& painter, const Eigen::Vector2f& coords);
00080 void drawArrow(QPainter& painter);
00081 void drawCoordSystem(QPainter& painter);
00082
00083 float resolution;
00084 Eigen::Vector2f origin;
00085
00086 int resolutionFactor;
00087 float resolutionFactorf;
00088
00089 bool useCheckerboardCache;
00090 bool use_utc_time_suffix_;
00091
00092 float pixelsPerMapMeter;
00093 float pixelsPerGeoTiffMeter;
00094
00095 Eigen::Vector2i minCoordsMap;
00096 Eigen::Vector2i maxCoordsMap;
00097
00098 Eigen::Vector2i sizeMap;
00099 Eigen::Vector2f sizeMapf;
00100
00101 Eigen::Vector2f rightBottomMarginMeters;
00102 Eigen::Vector2f rightBottomMarginPixelsf;
00103 Eigen::Vector2i rightBottomMarginPixels;
00104
00105 Eigen::Vector2f leftTopMarginMeters;
00106
00107 Eigen::Vector2f totalMeters;
00108
00109 Eigen::Vector2i geoTiffSizePixels;
00110
00111 Eigen::Vector2f mapOrigInGeotiff;
00112 Eigen::Vector2f mapEndInGeotiff;
00113
00114 std::string map_file_name_;
00115 std::string map_file_path_;
00116
00117 QImage image;
00118 QImage checkerboard_cache;
00119 QApplication* app;
00120 QFont map_draw_font_;
00121
00122 HectorMapTools::CoordinateTransformer<float> world_map_transformer_;
00123 HectorMapTools::CoordinateTransformer<float> map_geo_transformer_;
00124 HectorMapTools::CoordinateTransformer<float> world_geo_transformer_;
00125
00126 nav_msgs::MapMetaData cached_map_meta_data_;
00127
00128 int fake_argc_;
00129 char** fake_argv_;
00130 };
00131
00132 }
00133
00134 #endif