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
00030
00031
00032
00033
00034
00035 #ifndef NAV_GRID_VECTOR_NAV_GRID_H
00036 #define NAV_GRID_VECTOR_NAV_GRID_H
00037
00038 #include <nav_grid/nav_grid.h>
00039 #include <nav_grid/coordinate_conversion.h>
00040 #include <algorithm>
00041 #include <vector>
00042
00043 namespace nav_grid
00044 {
00050 template <typename T> class VectorNavGrid : public NavGrid<T>
00051 {
00052 public:
00053 using NavGrid<T>::NavGrid;
00054
00058 void reset() override
00059 {
00060 data_.assign(this->info_.width * this->info_.height, this->default_value_);
00061 }
00062
00072 void setInfo(const NavGridInfo& new_info) override
00073 {
00074 if (this->info_.width != new_info.width)
00075 {
00076 std::vector<T> new_vector(new_info.width * new_info.height, this->default_value_);
00077 unsigned int cols_to_move = std::min(this->info_.width, new_info.width);
00078 auto old_it = data_.begin();
00079 auto new_it = new_vector.begin();
00080 unsigned int max_row = std::min(this->info_.height, new_info.height);
00081 for (unsigned int row = 0; row < max_row; row++)
00082 {
00083 std::copy(old_it, old_it + cols_to_move, new_it);
00084 old_it += this->info_.width;
00085 new_it += new_info.width;
00086 }
00087 data_.swap(new_vector);
00088 }
00089 else if (this->info_.height != new_info.height)
00090 {
00091 data_.resize(new_info.width * new_info.height, this->default_value_);
00092 }
00093
00094 this->info_ = new_info;
00095 }
00096
00107 void updateInfo(const NavGridInfo& new_info) override
00108 {
00109
00110 if (this->info_ == new_info)
00111 {
00112 return;
00113 }
00114
00115
00116 if (this->info_.resolution != new_info.resolution || this->info_.frame_id != new_info.frame_id)
00117 {
00118 setInfo(new_info);
00119 return;
00120 }
00121
00122
00123 int cell_ox, cell_oy;
00124 worldToGrid(this->info_, new_info.origin_x, new_info.origin_y, cell_ox, cell_oy);
00125
00126
00127 int old_size_x = static_cast<int>(this->info_.width);
00128 int old_size_y = static_cast<int>(this->info_.height);
00129
00130
00131 int lower_left_x = std::min(std::max(cell_ox, 0), old_size_x);
00132 int lower_left_y = std::min(std::max(cell_oy, 0), old_size_y);
00133 int upper_right_x = std::min(std::max(cell_ox + static_cast<int>(new_info.width), 0), old_size_x);
00134 int upper_right_y = std::min(std::max(cell_oy + static_cast<int>(new_info.height), 0), old_size_y);
00135
00136 unsigned int cell_size_x = upper_right_x - lower_left_x;
00137 unsigned int cell_size_y = upper_right_y - lower_left_y;
00138
00139
00140 std::vector<T> new_data(new_info.width * new_info.height, this->default_value_);
00141
00142
00143 int start_x = lower_left_x - cell_ox;
00144 int start_y = lower_left_y - cell_oy;
00145
00146
00147
00148 auto src_index = data_.begin() + (lower_left_y * old_size_x + lower_left_x);
00149 auto dest_index = new_data.begin() + (start_y * new_info.width + start_x);
00150
00151
00152 for (unsigned int i = 0; i < cell_size_y; ++i)
00153 {
00154 std::copy(src_index, src_index + cell_size_x, dest_index);
00155 src_index += this->info_.width;
00156 dest_index += new_info.width;
00157 }
00158
00159 data_.swap(new_data);
00160
00161
00162 this->info_.width = new_info.width;
00163 this->info_.height = new_info.height;
00164
00165
00166
00167 this->info_.origin_x += cell_ox * this->info_.resolution;
00168 this->info_.origin_y += cell_oy * this->info_.resolution;
00169 }
00170
00171 void setValue(const unsigned int x, const unsigned int y, const T& value) override
00172 {
00173 data_[getIndex(x, y)] = value;
00174 }
00175
00176 T getValue(const unsigned int x, const unsigned int y) const override
00177 {
00178 return data_[getIndex(x, y)];
00179 }
00180
00181 using NavGrid<T>::operator();
00182 using NavGrid<T>::getValue;
00183 using NavGrid<T>::setValue;
00184
00188 T operator[] (unsigned int i) const {return data_[i];}
00189 T& operator[] (unsigned int i) {return data_[i];}
00190
00195 unsigned int size() const { return data_.size(); }
00196
00203 inline unsigned int getIndex(unsigned int mx, unsigned int my) const
00204 {
00205 return my * this->info_.width + mx;
00206 }
00207
00214 inline unsigned int getIndex(double x, double y) const
00215 {
00216 unsigned int mx, my;
00217 worldToGridBounded(this->info_, x, y, mx, my);
00218 return getIndex(mx, my);
00219 }
00220
00227 inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
00228 {
00229 unsigned int w = this->info_.width;
00230 my = index / w;
00231 mx = index - my * w;
00232 }
00233
00234 protected:
00235 std::vector<T> data_;
00236 };
00237 }
00238
00239 #endif // NAV_GRID_VECTOR_NAV_GRID_H