vector_nav_grid.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #ifndef NAV_GRID_VECTOR_NAV_GRID_H
36 #define NAV_GRID_VECTOR_NAV_GRID_H
37 
38 #include <nav_grid/nav_grid.h>
40 #include <algorithm>
41 #include <vector>
42 
43 namespace nav_grid
44 {
50 template <typename T> class VectorNavGrid : public NavGrid<T>
51 {
52 public:
53  using NavGrid<T>::NavGrid;
54 
58  void reset() override
59  {
60  data_.assign(this->info_.width * this->info_.height, this->default_value_);
61  }
62 
72  void setInfo(const NavGridInfo& new_info) override
73  {
74  if (this->info_.width != new_info.width)
75  {
76  std::vector<T> new_vector(new_info.width * new_info.height, this->default_value_);
77  unsigned int cols_to_move = std::min(this->info_.width, new_info.width);
78  auto old_it = data_.begin();
79  auto new_it = new_vector.begin();
80  unsigned int max_row = std::min(this->info_.height, new_info.height);
81  for (unsigned int row = 0; row < max_row; row++)
82  {
83  std::copy(old_it, old_it + cols_to_move, new_it);
84  old_it += this->info_.width;
85  new_it += new_info.width;
86  }
87  data_.swap(new_vector);
88  }
89  else if (this->info_.height != new_info.height)
90  {
91  data_.resize(new_info.width * new_info.height, this->default_value_);
92  }
93 
94  this->info_ = new_info;
95  }
96 
107  void updateInfo(const NavGridInfo& new_info) override
108  {
109  // If the info is the same, make no changes
110  if (this->info_ == new_info)
111  {
112  return;
113  }
114 
115  // If the resolution or frame changes, reset the whole grid
116  if (this->info_.resolution != new_info.resolution || this->info_.frame_id != new_info.frame_id)
117  {
118  setInfo(new_info);
119  return;
120  }
121 
122  // project the new origin into the grid
123  int cell_ox, cell_oy;
124  worldToGrid(this->info_, new_info.origin_x, new_info.origin_y, cell_ox, cell_oy);
125 
126  // To save casting from unsigned int to int a bunch of times
127  int old_size_x = static_cast<int>(this->info_.width);
128  int old_size_y = static_cast<int>(this->info_.height);
129 
130  // we need to compute the overlap of the new and existing windows
131  int lower_left_x = std::min(std::max(cell_ox, 0), old_size_x);
132  int lower_left_y = std::min(std::max(cell_oy, 0), old_size_y);
133  int upper_right_x = std::min(std::max(cell_ox + static_cast<int>(new_info.width), 0), old_size_x);
134  int upper_right_y = std::min(std::max(cell_oy + static_cast<int>(new_info.height), 0), old_size_y);
135 
136  unsigned int cell_size_x = upper_right_x - lower_left_x;
137  unsigned int cell_size_y = upper_right_y - lower_left_y;
138 
139  // we need a vector to store the new contents in the window temporarily
140  std::vector<T> new_data(new_info.width * new_info.height, this->default_value_);
141 
142  // compute the starting cell location for copying data back in
143  int start_x = lower_left_x - cell_ox;
144  int start_y = lower_left_y - cell_oy;
145 
146  // now we want to copy the overlapping information into the new vector, but in its new location
147  // we'll first need to compute the starting points for each vector
148  auto src_index = data_.begin() + (lower_left_y * old_size_x + lower_left_x);
149  auto dest_index = new_data.begin() + (start_y * new_info.width + start_x);
150 
151  // now, we'll copy the source vector into the destination vector
152  for (unsigned int i = 0; i < cell_size_y; ++i)
153  {
154  std::copy(src_index, src_index + cell_size_x, dest_index);
155  src_index += this->info_.width;
156  dest_index += new_info.width;
157  }
158 
159  data_.swap(new_data);
160 
161  // update the dimensions
162  this->info_.width = new_info.width;
163  this->info_.height = new_info.height;
164 
165  // update the origin. Recomputed instead of using new_info.origin
166  // because we want to keep things grid-aligned
167  this->info_.origin_x += cell_ox * this->info_.resolution;
168  this->info_.origin_y += cell_oy * this->info_.resolution;
169  }
170 
171  void setValue(const unsigned int x, const unsigned int y, const T& value) override
172  {
173  data_[getIndex(x, y)] = value;
174  }
175 
176  T getValue(const unsigned int x, const unsigned int y) const override
177  {
178  return data_[getIndex(x, y)];
179  }
180 
181  using NavGrid<T>::operator();
182  using NavGrid<T>::getValue;
183  using NavGrid<T>::setValue;
184 
188  T operator[] (unsigned int i) const {return data_[i];}
189  T& operator[] (unsigned int i) {return data_[i];}
190 
195  unsigned int size() const { return data_.size(); }
196 
203  inline unsigned int getIndex(unsigned int mx, unsigned int my) const
204  {
205  return my * this->info_.width + mx;
206  }
207 
214  inline unsigned int getIndex(double x, double y) const
215  {
216  unsigned int mx, my;
217  worldToGridBounded(this->info_, x, y, mx, my);
218  return getIndex(mx, my);
219  }
220 
227  inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
228  {
229  unsigned int w = this->info_.width;
230  my = index / w;
231  mx = index - my * w;
232  }
233 
234 protected:
235  std::vector<T> data_;
236 };
237 } // namespace nav_grid
238 
239 #endif // NAV_GRID_VECTOR_NAV_GRID_H
unsigned int getIndex(double x, double y) const
Given two world coordinates... compute the associated index.
T operator[](unsigned int i) const
NavGridInfo info_
Definition: nav_grid.h:151
void indexToCells(unsigned int index, unsigned int &mx, unsigned int &my) const
Given an index... compute the associated grid coordinates.
double origin_x
The origin defines the coordinates of minimum corner of cell (0,0) in the grid.
Definition: nav_grid_info.h:57
unsigned int getIndex(unsigned int mx, unsigned int my) const
Given two grid coordinates... compute the associated index.
void reset() override
Reset the contents of the grid to the default value.
void setInfo(const NavGridInfo &new_info) override
Change the info while attempting to keep the values associated with the grid coordinates.
void setValue(const unsigned int x, const unsigned int y, const T &value) override
set the value of the grid at (x,y)
void updateInfo(const NavGridInfo &new_info) override
Update the info while keeping the data geometrically in tact.
std::vector< T > data_
unsigned int size() const
Return the size of the vector. Equivalent to width * height.
void worldToGrid(const NavGridInfo &info, double wx, double wy, double &mx, double &my)
Convert from world coordinates to the precise (double) grid coordinates.
T getValue(const unsigned int x, const unsigned int y) const override
get the value of the grid at (x,y)
bool worldToGridBounded(const NavGridInfo &info, double wx, double wy, unsigned int &mx, unsigned int &my)
Convert from world coordinates to grid coordinates.


nav_grid
Author(s):
autogenerated on Sun Jan 10 2021 04:08:26