vector_nav_grid.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2018, Locus Robotics
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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     // If the info is the same, make no changes
00110     if (this->info_ == new_info)
00111     {
00112       return;
00113     }
00114 
00115     // If the resolution or frame changes, reset the whole grid
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     // project the new origin into the grid
00123     int cell_ox, cell_oy;
00124     worldToGrid(this->info_, new_info.origin_x, new_info.origin_y, cell_ox, cell_oy);
00125 
00126     // To save casting from unsigned int to int a bunch of times
00127     int old_size_x = static_cast<int>(this->info_.width);
00128     int old_size_y = static_cast<int>(this->info_.height);
00129 
00130     // we need to compute the overlap of the new and existing windows
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     // we need a vector to store the new contents in the window temporarily
00140     std::vector<T> new_data(new_info.width * new_info.height, this->default_value_);
00141 
00142     // compute the starting cell location for copying data back in
00143     int start_x = lower_left_x - cell_ox;
00144     int start_y = lower_left_y - cell_oy;
00145 
00146     // now we want to copy the overlapping information into the new vector, but in its new location
00147     // we'll first need to compute the starting points for each vector
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     // now, we'll copy the source vector into the destination vector
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     // update the dimensions
00162     this->info_.width = new_info.width;
00163     this->info_.height = new_info.height;
00164 
00165     // update the origin. Recomputed instead of using new_info.origin
00166     // because we want to keep things grid-aligned
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 }  // namespace nav_grid
00238 
00239 #endif  // NAV_GRID_VECTOR_NAV_GRID_H


nav_grid
Author(s):
autogenerated on Wed Jun 26 2019 20:09:30