robobuggy / buggymaps / src / buggymaps.cpp @ 8db47e6e
History | View | Annotate | Download (2.93 KB)
1 |
#include "buggymaps.h" |
---|---|
2 |
|
3 |
#include <ros/ros.h> |
4 |
#include <fstream> |
5 |
#include <cstdlib> |
6 |
#include <ctime> |
7 |
|
8 |
#define DEFAULT_BG_R 0x45 |
9 |
#define DEFAULT_BG_G 0x56 |
10 |
#define DEFAULT_BG_B 0xff |
11 |
|
12 |
// TODO: add meter_ parameter
|
13 |
namespace buggymaps
|
14 |
{ |
15 |
bool file_exist(QString fileName)
|
16 |
{ |
17 |
std::ifstream infile(fileName.toUtf8().constData()); |
18 |
return infile.good();
|
19 |
} |
20 |
|
21 |
|
22 |
Map::Map(const std::string map_path, |
23 |
const float x_offset, |
24 |
const float y_offset) |
25 |
{ |
26 |
x_offset_ = x_offset; |
27 |
y_offset_ = y_offset; |
28 |
|
29 |
// Load Images
|
30 |
base_path_ = QString(map_path.c_str()); |
31 |
|
32 |
// Strip all double-slashes
|
33 |
map_image_path_ = QString((map_path+".bmp").c_str());
|
34 |
height_image_path_ = QString((map_path+"_height.bmp").c_str());
|
35 |
|
36 |
// Check if images exist
|
37 |
if(!file_exist(map_image_path_)) {
|
38 |
ROS_INFO("Incorrect map path passed in");
|
39 |
ROS_INFO("Should be of the form:");
|
40 |
ROS_INFO("/path/to/map/map for map image of name:");
|
41 |
ROS_INFO(" /path/to/map/map.bmp");
|
42 |
assert(false);
|
43 |
} |
44 |
|
45 |
if(!file_exist(height_image_path_)) {
|
46 |
ROS_INFO("Incorrect map path passed in");
|
47 |
ROS_INFO("Should be of the form:");
|
48 |
ROS_INFO("/path/to/map/map for map image of name:");
|
49 |
ROS_INFO(" /path/to/map/map_.bmp");
|
50 |
assert(false);
|
51 |
} |
52 |
|
53 |
// Find the correct dimensions
|
54 |
// Check that the image format is correct
|
55 |
map_image_ = new QImage(map_image_path_);
|
56 |
height_image_ = new QImage(height_image_path_);
|
57 |
|
58 |
// Check that images are in bounds
|
59 |
int map_image_height = map_image_->height();
|
60 |
int map_image_width = map_image_->width();
|
61 |
|
62 |
int height_image_height = height_image_->height();
|
63 |
int height_image_width = height_image_->width();
|
64 |
|
65 |
if(height_image_height != map_image_height) {
|
66 |
ROS_INFO("Map image height != height image height");
|
67 |
assert(false);
|
68 |
} |
69 |
|
70 |
if(height_image_width != map_image_width) {
|
71 |
ROS_INFO("Map image width != height image width");
|
72 |
assert(false);
|
73 |
} |
74 |
|
75 |
map_pixel_height_ = map_image_height; |
76 |
map_pixel_width_= map_image_width; |
77 |
|
78 |
ROS_INFO("Loading map with path '%s'",base_path_.toUtf8().constData());
|
79 |
|
80 |
} |
81 |
|
82 |
Map::~Map() |
83 |
{ |
84 |
delete map_image_;
|
85 |
delete height_image_;
|
86 |
} |
87 |
|
88 |
int Map::getMapPixelHeight()
|
89 |
{ |
90 |
return map_pixel_height_;
|
91 |
} |
92 |
|
93 |
int Map::getMapPixelWidth()
|
94 |
{ |
95 |
return map_pixel_width_;
|
96 |
} |
97 |
|
98 |
float Map::getTerrainHeight(float x, float y) { |
99 |
ROS_INFO("Unimplemented");
|
100 |
assert(false);
|
101 |
} |
102 |
|
103 |
float Map::getColor(float x, float y) { |
104 |
ROS_INFO("Unimplemented");
|
105 |
assert(false);
|
106 |
} |
107 |
|
108 |
const QImage Map::getMapImage() {
|
109 |
return *map_image_;
|
110 |
} |
111 |
|
112 |
const QImage Map::getHeightImage() {
|
113 |
return *height_image_;
|
114 |
} |
115 |
|
116 |
float Map::getXOffset() {
|
117 |
return x_offset_;
|
118 |
} |
119 |
|
120 |
float Map::getYOffset() {
|
121 |
return y_offset_;
|
122 |
} |
123 |
|
124 |
} |
125 |
|