HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

168
navigations/nav_grid/include/nav_grid/coordinate_conversion.h Executable file
View File

@@ -0,0 +1,168 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_GRID_COORDINATE_CONVERSION_H
#define NAV_GRID_COORDINATE_CONVERSION_H
#include <nav_grid/nav_grid_info.h>
#include <math.h>
namespace nav_grid
{
/**
* @brief Convert from grid coordinates to world coordinates of the center of the cell
*
* The resulting coordinates are for the center of the grid cell.
*
* @param[in] mx The x grid coordinate
* @param[in] my The y grid coordinate
* @param[out] wx Set to the associated x world coordinate
* @param[out] wy Set to the associated y world coordinate
*/
inline void gridToWorld(const NavGridInfo& info, int mx, int my, double& wx, double& wy)
{
wx = info.origin_x + (mx + 0.5) * info.resolution;
wy = info.origin_y + (my + 0.5) * info.resolution;
}
/**
* @brief Convert from world coordinates to the precise (double) grid coordinates
*
* The results are not rounded, so that the values can be used for locating a position within a cell
*
* @param[in] wx The x world coordinate
* @param[in] wy The y world coordinate
* @param[out] mx Set to the associated x grid coordinate
* @param[out] my Set to the associated y grid coordinate
*/
inline void worldToGrid(const NavGridInfo& info, double wx, double wy, double& mx, double& my)
{
mx = (wx - info.origin_x) / info.resolution;
my = (wy - info.origin_y) / info.resolution;
}
/**
* @brief Convert from world coordinates to grid coordinates without checking for legal bounds
* @param[in] wx The x world coordinate
* @param[in] wy The y world coordinate
* @param[out] mx Set to the associated x grid coordinate
* @param[out] my Set to the associated y grid coordinate
* @note The returned grid coordinates <b>are not guaranteed to lie within the grid.</b>
*/
inline void worldToGrid(const NavGridInfo& info, double wx, double wy, int& mx, int& my)
{
double dmx, dmy;
worldToGrid(info, wx, wy, dmx, dmy);
mx = static_cast<int>(floor(dmx));
my = static_cast<int>(floor(dmy));
}
/**
* @brief Convert from world coordinates to grid coordinates
*
* Combined functionality from costmap_2d::worldToMap and costmap_2d::worldToMapEnforceBounds.
* The output parameters are set to grid indexes within the grid, even if the function returns false,
* meaning the coordinates are outside the grid.
*
* @param[in] wx The x world coordinate
* @param[in] wy The y world coordinate
* @param[out] mx Set to the associated (bounds-enforced) x grid coordinate
* @param[out] my Set to the associated (bounds-enforced) y grid coordinate
* @return True if the input coordinates were within the grid
*/
inline bool worldToGridBounded(const NavGridInfo& info, double wx, double wy, unsigned int& mx, unsigned int& my)
{
double dmx, dmy;
worldToGrid(info, wx, wy, dmx, dmy);
bool valid = true;
if (dmx < 0.0)
{
mx = 0;
valid = false;
}
else if (dmx >= info.width)
{
mx = info.width - 1;
valid = false;
}
else
{
mx = static_cast<unsigned int>(dmx);
}
if (dmy < 0.0)
{
my = 0;
valid = false;
}
else if (dmy >= info.height)
{
my = info.height - 1;
valid = false;
}
else
{
my = static_cast<unsigned int>(dmy);
}
return valid;
}
/**
* @brief Check to see if the world coordinates are within the grid.
*
* This should only be used if the caller does not need the associated grid coordinates. Otherwise it would
* be more efficient to call worldToGridBounded.
*
* @param[in] wx The x world coordinate
* @param[in] wy The y world coordinate
* @return True if the input coordinates were within the grid
*/
inline bool isWithinGrid(const NavGridInfo& info, double wx, double wy)
{
wx -= info.origin_x;
wy -= info.origin_y;
return wx >= 0.0 &&
wy >= 0.0 &&
wx < info.width * info.resolution &&
wy < info.height * info.resolution;
}
} // namespace nav_grid
#endif // NAV_GRID_COORDINATE_CONVERSION_H

99
navigations/nav_grid/include/nav_grid/index.h Executable file
View File

@@ -0,0 +1,99 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_GRID_INDEX_H
#define NAV_GRID_INDEX_H
#include <string>
namespace nav_grid
{
/**
* @class GenericIndex
* @brief A simple pair of x/y coordinates
*/
template <typename NumericType>
struct GenericIndex
{
NumericType x, y;
explicit GenericIndex(const NumericType& x = 0, const NumericType& y = 0) : x(x), y(y) {}
/**
* @brief comparison operator that requires equal x and y
*/
bool operator == (const GenericIndex& other) const
{
return x == other.x && y == other.y;
}
bool operator != (const GenericIndex& other) const
{
return !operator==(other);
}
/**
* @brief less than operator so object can be used in sets
*/
bool operator < (const GenericIndex& other) const
{
return x < other.x || (x == other.x && y < other.y);
}
// Derived Comparators
bool operator > (const GenericIndex& other) const { return other < *this; }
bool operator <= (const GenericIndex& other) const { return !(*this > other); }
bool operator >= (const GenericIndex& other) const { return !(*this < other); }
/**
* @brief String representation of this object
*/
std::string toString() const
{
return "(" + std::to_string(x) + ", " + std::to_string(y) + ")";
}
};
template <typename NumericType>
inline std::ostream& operator<<(std::ostream& stream, const GenericIndex<NumericType>& index)
{
stream << index.toString();
return stream;
}
using SignedIndex = GenericIndex<int>;
using Index = GenericIndex<unsigned int>;
} // namespace nav_grid
#endif // NAV_GRID_INDEX_H

157
navigations/nav_grid/include/nav_grid/nav_grid.h Executable file
View File

@@ -0,0 +1,157 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_GRID_NAV_GRID_H
#define NAV_GRID_NAV_GRID_H
#include <nav_grid/nav_grid_info.h>
#include <nav_grid/index.h>
#include <string>
namespace nav_grid
{
/**
* @class NavGrid
* This class is a spiritual successor to the costmap_2d::Costmap2D class, with the key differences being that
* the datatype and data storage methods are not specified, and the frame_id is specified.
*
* The templatized nature of the class allows you to store whatever you like at each grid location, including
* unsigned chars if emulating Costmap2D or floating point numbers if emulating the grid_map package, or whatever
* else.
*
* The VectorNavGrid class in this package implements this class with a straight-forward single-dimensional vector
* representing the two dimensional grid. Other classes could implement the data storage differently.
*
* Getting data from the grid can be done either through the getValue methods or the parenthetical operators (which call
* getValue internally). Implementing classes must implement getValue.
* x = grid(0, 0) + grid.getValue(0, 1);
*
* Writing data to the grid must be done through the setValue method (which implementing classes must implement)
* grid.setValue(0, 0, x);
*
* You can also use nav_grid::Index objects
* nav_grid::Index index(0, 0);
* x = grid(index) + grid.getValue(index);
* index.y = 3;
* grid.setCost(index, x);
* The Index methods also internally call setValue/getValue
*
* The geometry of the grid is specified by the NavGridInfo. Borrowing an idea from the grid_map package, two
* separate methods are defined for changing the info. setInfo will change the info without changing the grid values.
* updateInfo will change the info while trying to preserve the contents of the grid.
*
* The final component is a collection of methods inspired by Costmap2D for converting coordinates of different types.
*/
template <typename T> class NavGrid
{
public:
explicit NavGrid(const T default_value = T{}) : default_value_(default_value) {}
/**
* @brief Reset the contents of the grid
*/
virtual void reset() = 0;
/**
* @brief get the value of the grid at (x,y)
* @param x[in] Valid x coordinate
* @param y[in] Valid y coordinate
* @return value at (x,y)
*/
virtual T getValue(const unsigned int x, const unsigned int y) const = 0;
/**
* @brief set the value of the grid at (x,y)
* @param x[in] Valid x coordinate
* @param y[in] Valid y coordinate
* @param value[in] New Value
*/
virtual void setValue(const unsigned int x, const unsigned int y, const T& value) = 0;
/**@name Convenience Aliases */
// Note: You may not be able to use these unless your deriving class declares using NavGrid<T>::operator() or
// using NavGrid<T>::getValue
/**@{*/
T getValue(const Index& index) { return getValue(index.x, index.y); }
T operator() (const unsigned int x, const unsigned int y) const { return getValue(x, y); }
T operator() (const Index& index) const { return getValue(index.x, index.y); }
void setValue(const Index& index, const T& value) { setValue(index.x, index.y, value); }
/**@}*/
/**
* @brief Change the info while attempting to keep the values associated with the grid coordinates
* @param[in] new_info New grid info
*/
virtual void setInfo(const NavGridInfo& new_info) = 0;
/**
* @brief Change the info while attempting to keep the values associated with the world coordinates
*
* For example, if the only change to the info is to the origin's x coordinate (increasing by an amount equal to the
* resolution), then all the values should be shifted one grid cell to the left.
*
* @param[in] new_info New grid info
*/
virtual void updateInfo(const NavGridInfo& new_info) { setInfo(new_info); }
inline NavGridInfo getInfo() const { return info_; }
/**
* @brief Set the default value
* @param[in] new_value New Default Value
*/
void setDefaultValue(const T new_value)
{
default_value_ = new_value;
}
/*****************************************************************************************************
* NavGridInfo accessor methods
*****************************************************************************************************/
inline unsigned int getWidth() const { return info_.width; }
inline unsigned int getHeight() const { return info_.height; }
inline double getResolution() const { return info_.resolution; }
inline std::string getFrameId() const { return info_.frame_id; }
inline double getOriginX() const { return info_.origin_x; }
inline double getOriginY() const { return info_.origin_y; }
protected:
NavGridInfo info_;
T default_value_;
};
} // namespace nav_grid
#endif // NAV_GRID_NAV_GRID_H

92
navigations/nav_grid/include/nav_grid/nav_grid_info.h Executable file
View File

@@ -0,0 +1,92 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_GRID_NAV_GRID_INFO_H
#define NAV_GRID_NAV_GRID_INFO_H
#include <string>
namespace nav_grid
{
/**
* @struct NavGridInfo
* This class defines a way to discretize a finite section of the world into a grid.
* It contains similar information to the ROS msg nav_msgs/MapMetaData (aka the info field of nav_msgs/OccupancyGrid)
* except the map_load_time is removed, the geometry is simplified from a Pose to xy coordinates, and the frame_id
* is added.
*/
struct NavGridInfo
{
public:
/* All data is publically accessible */
unsigned int width = 0;
unsigned int height = 0;
double resolution = 1.0;
std::string frame_id = "map";
double origin_x = 0.0; ///< The origin defines the coordinates of minimum corner of cell (0,0) in the grid
double origin_y = 0.0;
/**
* @brief comparison operator that requires all fields are equal
*/
bool operator == (const NavGridInfo& info) const
{
return width == info.width && height == info.height && resolution == info.resolution &&
origin_x == info.origin_x && origin_y == info.origin_y && frame_id == info.frame_id;
}
bool operator != (const NavGridInfo& info) const
{
return !operator==(info);
}
/**
* @brief String representation of this object
*/
std::string toString() const
{
return std::to_string(width) + "x" + std::to_string(height) + " (" + std::to_string(resolution) + "res) " +
frame_id + " " + std::to_string(origin_x) + " " + std::to_string(origin_y);
}
};
inline std::ostream& operator<<(std::ostream& stream, const NavGridInfo& info)
{
stream << info.toString();
return stream;
}
} // namespace nav_grid
#endif // NAV_GRID_NAV_GRID_INFO_H

239
navigations/nav_grid/include/nav_grid/vector_nav_grid.h Executable file
View File

@@ -0,0 +1,239 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_GRID_VECTOR_NAV_GRID_H
#define NAV_GRID_VECTOR_NAV_GRID_H
#include <nav_grid/nav_grid.h>
#include <nav_grid/coordinate_conversion.h>
#include <algorithm>
#include <vector>
namespace nav_grid
{
/**
* @class VectorNavGrid
* A straight-forward implementation of the NavGrid class where the data for cell (x, y) is stored in a std::vector
* with index (y * info.width + x).
*/
template <typename T> class VectorNavGrid : public NavGrid<T>
{
public:
using NavGrid<T>::NavGrid;
/**
* @brief Reset the contents of the grid to the default value
*/
void reset() override
{
data_.assign(this->info_.width * this->info_.height, this->default_value_);
}
/**
* @brief Change the info while attempting to keep the values associated with the grid coordinates
*
* If the width changes, we need to move each row to its new location
*
* If just the height changes, then we can resize the vector without having to move elements
*
* We just overwrite the rest of the grid info
*/
void setInfo(const NavGridInfo& new_info) override
{
if (this->info_.width != new_info.width)
{
std::vector<T> new_vector(new_info.width * new_info.height, this->default_value_);
unsigned int cols_to_move = std::min(this->info_.width, new_info.width);
auto old_it = data_.begin();
auto new_it = new_vector.begin();
unsigned int max_row = std::min(this->info_.height, new_info.height);
for (unsigned int row = 0; row < max_row; row++)
{
std::copy(old_it, old_it + cols_to_move, new_it);
old_it += this->info_.width;
new_it += new_info.width;
}
data_.swap(new_vector);
}
else if (this->info_.height != new_info.height)
{
data_.resize(new_info.width * new_info.height, this->default_value_);
}
this->info_ = new_info;
}
/**
* @brief Update the info while keeping the data geometrically in tact
*
* If the resolution or frame_id changes, reset all the data.
*
* Otherwise, adjust the new_info so the grid stays aligned (The grid's new info will be within a
* resolution-length of the original new_info). Then copy the common values into the new grid.
*
* @param[in] new_info New information to update the grid with
*/
void updateInfo(const NavGridInfo& new_info) override
{
// If the info is the same, make no changes
if (this->info_ == new_info)
{
return;
}
// If the resolution or frame changes, reset the whole grid
if (this->info_.resolution != new_info.resolution || this->info_.frame_id != new_info.frame_id)
{
setInfo(new_info);
return;
}
// project the new origin into the grid
int cell_ox, cell_oy;
worldToGrid(this->info_, new_info.origin_x, new_info.origin_y, cell_ox, cell_oy);
// To save casting from unsigned int to int a bunch of times
int old_size_x = static_cast<int>(this->info_.width);
int old_size_y = static_cast<int>(this->info_.height);
// we need to compute the overlap of the new and existing windows
int lower_left_x = std::min(std::max(cell_ox, 0), old_size_x);
int lower_left_y = std::min(std::max(cell_oy, 0), old_size_y);
int upper_right_x = std::min(std::max(cell_ox + static_cast<int>(new_info.width), 0), old_size_x);
int upper_right_y = std::min(std::max(cell_oy + static_cast<int>(new_info.height), 0), old_size_y);
unsigned int cell_size_x = upper_right_x - lower_left_x;
unsigned int cell_size_y = upper_right_y - lower_left_y;
// we need a vector to store the new contents in the window temporarily
std::vector<T> new_data(new_info.width * new_info.height, this->default_value_);
// compute the starting cell location for copying data back in
int start_x = lower_left_x - cell_ox;
int start_y = lower_left_y - cell_oy;
// now we want to copy the overlapping information into the new vector, but in its new location
// we'll first need to compute the starting points for each vector
auto src_index = data_.begin() + (lower_left_y * old_size_x + lower_left_x);
auto dest_index = new_data.begin() + (start_y * new_info.width + start_x);
// now, we'll copy the source vector into the destination vector
for (unsigned int i = 0; i < cell_size_y; ++i)
{
std::copy(src_index, src_index + cell_size_x, dest_index);
src_index += this->info_.width;
dest_index += new_info.width;
}
data_.swap(new_data);
// update the dimensions
this->info_.width = new_info.width;
this->info_.height = new_info.height;
// update the origin. Recomputed instead of using new_info.origin
// because we want to keep things grid-aligned
this->info_.origin_x += cell_ox * this->info_.resolution;
this->info_.origin_y += cell_oy * this->info_.resolution;
}
void setValue(const unsigned int x, const unsigned int y, const T& value) override
{
data_[getIndex(x, y)] = value;
}
T getValue(const unsigned int x, const unsigned int y) const override
{
return data_[getIndex(x, y)];
}
using NavGrid<T>::operator();
using NavGrid<T>::getValue;
using NavGrid<T>::setValue;
/**
* Overloading the [] operator so that the data can be accessed directly with vector_nav_grid[i]
*/
T operator[] (unsigned int i) const {return data_[i];}
T& operator[] (unsigned int i) {return data_[i];}
/**
* @brief Return the size of the vector. Equivalent to width * height.
* @return size of the vector
*/
unsigned int size() const { return data_.size(); }
/**
* @brief Given two grid coordinates... compute the associated index
* @param[in] mx The x coordinate
* @param[in] my The y coordinate
* @return The associated index
*/
inline unsigned int getIndex(unsigned int mx, unsigned int my) const
{
return my * this->info_.width + mx;
}
/**
* @brief Given two world coordinates... compute the associated index
* @param[in] mx The x coordinate
* @param[in] my The y coordinate
* @return The associated index
*/
inline unsigned int getIndex(double x, double y) const
{
unsigned int mx, my;
worldToGridBounded(this->info_, x, y, mx, my);
return getIndex(mx, my);
}
/**
* @brief Given an index... compute the associated grid coordinates
* @param[in] index The index
* @param[out] mx Set to the associated x grid coordinate
* @param[out] my Set to the associated y grid coordinate
*/
inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
{
unsigned int w = this->info_.width;
my = index / w;
mx = index - my * w;
}
protected:
std::vector<T> data_;
};
} // namespace nav_grid
#endif // NAV_GRID_VECTOR_NAV_GRID_H