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

21
navigations/nav_grid/CMakeLists.txt Executable file
View File

@@ -0,0 +1,21 @@
cmake_minimum_required(VERSION 3.0.2)
project(nav_grid)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11")
find_package(catkin REQUIRED)
catkin_package(
INCLUDE_DIRS include
)
if (CATKIN_ENABLE_TESTING)
find_package(roslint REQUIRED)
include_directories(include ${catkin_INCLUDE_DIRS})
roslint_cpp()
roslint_add_test()
catkin_add_gtest(${PROJECT_NAME}_utest test/utest.cpp)
endif (CATKIN_ENABLE_TESTING)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)

39
navigations/nav_grid/README.md Executable file
View File

@@ -0,0 +1,39 @@
# nav_grid
Many navigation algorithms rely on the concept of a two dimensional grid being overlaid on the world, with a value being assigned to each grid cell. In the original navigation stack, `Costmap2DROS` associated an `unsigned char` with grid cells, global planners cache distance to the goal as `float`s and local planners would cache various metrics in a grid to quickly calculate the strength of different trajectories.
![nav_grid diagram](doc/nav_grid.png)
## `NavGridInfo`
Where the grid exists in the world is defined by six parameters.
* `width` and `height` (which together define the number of cells in the grid)
* `resolution` which is the dimension of each cell in meters (square cells only)
* `frame_id` which is the TF frame the grid is defined relative to.
* `origin_x` and `origin_y` which define the offset in meters from the root of the TF frame to the minimum corner of the (0, 0) cell.
Together, these components make a [`nav_grid::NavGridInfo`](include/nav_grid/nav_grid_info.h) struct. It evolved from the [`nav_msgs::MapMetaData` message](http://docs.ros.org/melodic/api/nav_msgs/html/msg/MapMetaData.html) but without `map_load_time`, simplified origin geometry and the `frame_id` added. Note: for general efficiency of computation (particularly moving the grid while retaining some of the values) there is no rotation component to the origin. Each grid is locked to the orientation of its TF frame.
The default values are `width=0, height=0, resolution=1.0, frame_id="map", origin_x=0.0, origin_y=0.0`.
## Coordinate Conversion
One of the most common operations is to want to convert between the real world coordinates and the grid coordinates. These operations can be done with a `NavGridInfo` object and the methods in [`coordinate_conversion.h`](include/nav_grid/coordinate_conversion.h). They are derived from methods in [`costmap_2d.h`](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/costmap_2d/include/costmap_2d/costmap_2d.h#L126), but with some key differences (beyond replacing `map` with `grid`).
* `gridToWorld` is the same as `mapToWorld`, as both return the world coordinates of the center of the specified cell.
* `worldToGrid` works like `worldToMapNoBounds`, but it results in either `int` or `double` coordinates depending on the output parameter types. As the result are not bounded by the grid, the results are signed.
* `worldToGridBounded` is a combination of `worldToMap` and `worldToMapEnforceBounds`. It returns a bool for whether the input coordinates are within the grid AND the output coordinates are forced to be within the grid. The output coordinates are therefore `unsigned int`.
* There's also `isWithinGrid` that returns whether a given point is within the grid (i.e. will match the return value of `worldToGridBounded` but saves some of the computation associated with calculating the actual values of the coordinates.
![example coordinate conversion](doc/coords.png)
## `NavGrid<T>`
Of course, we also want to associate a value with each cell in the grid. For that, we define the templatized [`nav_grid::NavGrid<T>`](include/nav_grid/nav_grid.h) abstract class. The template allows for storing arbitrary data types associated with each grid cell. The actual storage mechanism for the data is not part of the base class to allow for possibly more efficient methods. A default implementation where the data is simply stored in row-major order in a one-dimensional vector is provided in [`nav_grid::VectorNavGrid<T>`](include/nav_grid/vector_nav_grid.h>)
The constructor for `NavGrid` takes a default value for each cell which is 0 by default. The grid's initial info matches the default info above, so the grid is initially `0x0`.
The `NavGrid` class provides handy methods for accessing values via their grid indexes. You can use `grid(x, y)` or `grid.getValue(x, y)` to access each value, and use `grid.setValue(x, y, value)` to write each value. There is also the helper class [`nav_grid::Index`](include/nav_grid/index.h) that can be used to store the two coordinates and used in accessing the data as well a la `grid(index)` and `grid.setValue(index, value)`.
There are two methods for changing the `info` associated with the grid: `setInfo` and `updateInfo`. `setInfo` changes the `info` while maintaining the data associated with each grid coordinate. `updateInfo` will change the info but instead maintain the data associated with the world coordinates.
For instance, imagine a 5x5 grid with 0.5 meter resolution with the cell (2, 0) set to red which represents a cell at (1.25, 0.25) in the world. If we change the origin to be 0.5 meters to the right, the grids will have different values according to the method we use. With `setInfo`, cell (2, 0) is still red, but it is associated with a cell at (1.75, 0.25) in the world. With `updateInfo`, the cell at (1.25, 0.25) is still red, but it is now associated with cell (1, 0). The exact mechanism for how this data is preserved is left to the implementing class.
![illustration of grid update](doc/change_info.png)

BIN
navigations/nav_grid/doc/change_info.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 9.6 KiB

BIN
navigations/nav_grid/doc/coords.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 55 KiB

BIN
navigations/nav_grid/doc/nav_grid.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 15 KiB

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

13
navigations/nav_grid/package.xml Executable file
View File

@@ -0,0 +1,13 @@
<?xml version="1.0"?>
<package format="2">
<name>nav_grid</name>
<version>0.3.0</version>
<description>
A templatized interface for overlaying a two dimensional grid on the world.
</description>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<test_depend>roslint</test_depend>
<test_depend>rosunit</test_depend>
</package>

522
navigations/nav_grid/test/utest.cpp Executable file
View File

@@ -0,0 +1,522 @@
/*
* 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.
*/
#include <gtest/gtest.h>
#include <nav_grid/vector_nav_grid.h>
#include <nav_grid/coordinate_conversion.h>
#include <algorithm>
TEST(VectorNavGrid, info_equality)
{
nav_grid::NavGridInfo info0;
nav_grid::NavGridInfo info1;
nav_grid::NavGridInfo width_info;
width_info.width = 3;
nav_grid::NavGridInfo height_info;
height_info.height = 3;
nav_grid::NavGridInfo res_info;
res_info.resolution = 3.0;
nav_grid::NavGridInfo frame_info;
frame_info.frame_id = "foobar";
nav_grid::NavGridInfo originx_info;
originx_info.origin_x = 3.0;
nav_grid::NavGridInfo originy_info;
originy_info.origin_y = 3.0;
EXPECT_EQ(info0, info0);
EXPECT_EQ(info0, info1);
EXPECT_NE(info0, width_info);
EXPECT_NE(info0, height_info);
EXPECT_NE(info0, res_info);
EXPECT_NE(info0, frame_info);
EXPECT_NE(info0, originx_info);
EXPECT_NE(info0, originy_info);
}
TEST(VectorNavGrid, basic_test)
{
nav_grid::VectorNavGrid<int> grid(-3);
nav_grid::NavGridInfo info;
info.width = 2;
info.height = 3;
grid.setInfo(info);
EXPECT_EQ(grid(0, 0), -3);
grid.setValue(1, 1, 10);
EXPECT_EQ(grid(0, 0), -3);
EXPECT_EQ(grid(1, 1), 10);
}
TEST(VectorNavGrid, basic_index_test)
{
nav_grid::VectorNavGrid<int> grid(-3);
nav_grid::NavGridInfo info;
info.width = 2;
info.height = 3;
grid.setInfo(info);
nav_grid::Index index0(0, 0), index1(1, 1);
EXPECT_EQ(grid(index0), -3);
grid.setValue(index1, 10);
EXPECT_EQ(grid(index0), -3);
EXPECT_EQ(grid(index1), 10);
EXPECT_EQ(grid(0, 0), -3);
EXPECT_EQ(grid(1, 1), 10);
}
TEST(VectorNavGrid, easy_coordinates_test)
{
nav_grid::NavGridInfo info;
info.width = 2;
info.height = 3;
double wx, wy;
gridToWorld(info, 0, 0, wx, wy);
EXPECT_DOUBLE_EQ(wx, 0.5);
EXPECT_DOUBLE_EQ(wy, 0.5);
gridToWorld(info, 1, 2, wx, wy);
EXPECT_DOUBLE_EQ(wx, 1.5);
EXPECT_DOUBLE_EQ(wy, 2.5);
unsigned int umx, umy;
int mx, my;
double dmx, dmy;
ASSERT_TRUE(worldToGridBounded(info, wx, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_EQ(umy, 2);
worldToGrid(info, wx, wy, mx, my);
EXPECT_EQ(mx, 1);
EXPECT_EQ(my, 2);
worldToGrid(info, wx, wy, dmx, dmy);
EXPECT_DOUBLE_EQ(dmx, wx);
EXPECT_DOUBLE_EQ(dmy, wy);
// Invalid Coordinate
wx = 2.5;
EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_EQ(umy, 2);
worldToGrid(info, wx, wy, mx, my);
EXPECT_EQ(mx, 2);
EXPECT_EQ(my, 2);
// Border Cases
EXPECT_TRUE(worldToGridBounded(info, 0.0, wy, umx, umy));
EXPECT_EQ(umx, 0);
EXPECT_TRUE(worldToGridBounded(info, 0.25, wy, umx, umy));
EXPECT_EQ(umx, 0);
EXPECT_TRUE(worldToGridBounded(info, 0.75, wy, umx, umy));
EXPECT_EQ(umx, 0);
EXPECT_TRUE(worldToGridBounded(info, 0.9999, wy, umx, umy));
EXPECT_EQ(umx, 0);
EXPECT_TRUE(worldToGridBounded(info, 1.0, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_TRUE(worldToGridBounded(info, 1.25, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_TRUE(worldToGridBounded(info, 1.75, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_TRUE(worldToGridBounded(info, 1.9999, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_FALSE(worldToGridBounded(info, 2.0, wy, umx, umy));
EXPECT_EQ(umx, 1);
}
TEST(VectorNavGrid, hard_coordinates_test)
{
nav_grid::NavGridInfo info;
info.width = 2;
info.height = 3;
info.resolution = 0.1;
info.origin_x = -0.2;
info.origin_y = 0.2;
double wx, wy;
gridToWorld(info, 0, 0, wx, wy);
EXPECT_DOUBLE_EQ(wx, -0.15);
EXPECT_DOUBLE_EQ(wy, 0.25);
gridToWorld(info, 1, 2, wx, wy);
EXPECT_DOUBLE_EQ(wx, -0.05);
EXPECT_DOUBLE_EQ(wy, 0.45);
unsigned int umx, umy;
int mx, my;
double dmx, dmy;
EXPECT_TRUE(worldToGridBounded(info, wx, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_EQ(umy, 2);
worldToGrid(info, wx, wy, mx, my);
EXPECT_EQ(mx, 1);
EXPECT_EQ(my, 2);
worldToGrid(info, wx, wy, dmx, dmy);
EXPECT_DOUBLE_EQ(dmx, 1.5);
EXPECT_DOUBLE_EQ(dmy, 2.5);
// Invalid Coordinate
wx = 2.5;
EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_EQ(umy, 2);
worldToGrid(info, wx, wy, mx, my);
EXPECT_EQ(mx, 27);
EXPECT_EQ(my, 2);
}
TEST(VectorNavGrid, speed_test)
{
nav_grid::NavGridInfo info;
const int N = 1000;
const int EXTRA = 300;
info.width = N;
info.height = N;
double wx, wy;
unsigned int umx, umy;
int mx, my;
double dmx, dmy;
for (int x = -EXTRA; x < N + EXTRA; x++)
{
for (int y = -EXTRA; y < N + EXTRA; y++)
{
gridToWorld(info, x, y, wx, wy);
if (x < 0 || y < 0 || x >= N || y >= N)
{
EXPECT_FALSE(isWithinGrid(info, wx, wy));
EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy));
EXPECT_EQ(umx, std::min(std::max(0, x), N - 1));
EXPECT_EQ(umy, std::min(std::max(0, y), N - 1));
}
else
{
EXPECT_TRUE(isWithinGrid(info, wx, wy));
EXPECT_TRUE(worldToGridBounded(info, wx, wy, umx, umy));
EXPECT_EQ(umx, x);
EXPECT_EQ(umy, y);
}
worldToGrid(info, wx, wy, mx, my);
EXPECT_EQ(mx, x);
EXPECT_EQ(my, y);
worldToGrid(info, wx, wy, dmx, dmy);
EXPECT_DOUBLE_EQ(dmx, x + 0.5);
EXPECT_DOUBLE_EQ(dmy, y + 0.5);
}
}
}
int testGridValue(double x, double y)
{
return static_cast<int>(100 * floor(x) + floor(y));
}
/**
* Initialize Grid Values with values based on the grid/world coordinates
* which are initially the same
*
* x -->
* 000 100 200 300 400 500 600 700 800 900 y
* 001 101 201 301 401 501 601 701 801 901 |
* 002 102 202 302 402 502 602 702 802 902 |
* 003 103 203 303 403 503 603 703 803 903 V
* 004 104 204 304 404 504 604 704 804 904
*/
void initializeTestGrid(nav_grid::VectorNavGrid<int>& grid)
{
grid.setDefaultValue(-10);
nav_grid::NavGridInfo info;
info.width = 10;
info.height = 5;
grid.setInfo(info);
double mx, my;
for (unsigned int j = 0; j < info.height; j++)
{
for (unsigned int i = 0; i < info.width; i++)
{
gridToWorld(info, i, j, mx, my);
grid.setValue(i, j, testGridValue(mx, my));
}
}
}
/**
* Check to make sure all the grid values are now the same based on their grid coordinates
*/
void checkSetGridValues(const nav_grid::VectorNavGrid<int>& grid,
unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1)
{
for (unsigned int x = 0; x < grid.getWidth(); x++)
{
for (unsigned int y = 0; y < grid.getHeight(); y++)
{
if (x >= x0 && x < x1 && y >= y0 && y < y1)
{
EXPECT_EQ(grid(x, y), testGridValue(x, y)); // testGridValue based on Grid Coordinates
}
else
{
EXPECT_EQ(grid(x, y), -10);
}
}
}
}
/**
* Check to make sure all the grid values are now the same based on their world coordinates
*/
void checkUpdateGridValues(const nav_grid::VectorNavGrid<int>& grid,
unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1)
{
double mx, my;
for (unsigned int x = 0; x < grid.getWidth(); x++)
{
for (unsigned int y = 0; y < grid.getHeight(); y++)
{
if (x >= x0 && x < x1 && y >= y0 && y < y1)
{
gridToWorld(grid.getInfo(), x, y, mx, my);
EXPECT_EQ(grid(x, y), testGridValue(mx, my)); // testGridValue based on World Coordinates
}
else
{
EXPECT_EQ(grid(x, y), -10);
}
}
}
}
void debugGridValues(const nav_grid::VectorNavGrid<int>& grid)
{
for (unsigned int j = 0; j < grid.getHeight(); j++)
{
for (unsigned int i = 0; i < grid.getWidth(); i++)
{
printf("%d ", grid(i, j));
}
printf("\n");
}
printf("\n");
}
TEST(VectorNavGrid, resizing_grid_with_set)
{
nav_grid::VectorNavGrid<int> grid;
initializeTestGrid(grid);
checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo decreased_width_info = grid.getInfo();
decreased_width_info.width = 5;
grid.setInfo(decreased_width_info);
checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo increased_width_info = grid.getInfo();
increased_width_info.width = 9;
grid.setInfo(increased_width_info);
checkSetGridValues(grid, 0, 5, 0, grid.getHeight());
initializeTestGrid(grid);
checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo increased_height_info = grid.getInfo();
increased_height_info.height = 9;
grid.setInfo(increased_height_info);
checkSetGridValues(grid, 0, grid.getWidth(), 0, 5);
nav_grid::NavGridInfo decreased_height_info = grid.getInfo();
decreased_height_info.height = 4;
grid.setInfo(decreased_height_info);
checkSetGridValues(grid, 0, grid.getWidth(), 0, 4);
}
TEST(VectorNavGrid, resizing_grid_with_update)
{
nav_grid::VectorNavGrid<int> grid;
initializeTestGrid(grid);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo decreased_width_info = grid.getInfo();
decreased_width_info.width = 5;
grid.updateInfo(decreased_width_info);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo increased_width_info = grid.getInfo();
increased_width_info.width = 9;
grid.updateInfo(increased_width_info);
checkUpdateGridValues(grid, 0, 5, 0, grid.getHeight());
initializeTestGrid(grid);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo increased_height_info = grid.getInfo();
increased_height_info.height = 9;
grid.updateInfo(increased_height_info);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, 5);
nav_grid::NavGridInfo decreased_height_info = grid.getInfo();
decreased_height_info.height = 4;
grid.updateInfo(decreased_height_info);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, 4);
}
TEST(VectorNavGrid, change_origin)
{
nav_grid::VectorNavGrid<int> grid;
initializeTestGrid(grid);
nav_grid::NavGridInfo bump_right_info = grid.getInfo();
bump_right_info.origin_x = 3;
grid.updateInfo(bump_right_info);
checkUpdateGridValues(grid, 0, 7, 0, grid.getHeight());
nav_grid::NavGridInfo bump_up_info = grid.getInfo();
bump_up_info.origin_y = 2;
grid.updateInfo(bump_up_info);
checkUpdateGridValues(grid, 0, 7, 0, 3);
nav_grid::NavGridInfo bump_left_info = grid.getInfo();
bump_left_info.origin_x = -1;
grid.updateInfo(bump_left_info);
checkUpdateGridValues(grid, 4, grid.getWidth(), 0, 3);
nav_grid::NavGridInfo bump_down_info = grid.getInfo();
bump_down_info.origin_y = 0;
grid.updateInfo(bump_down_info);
checkUpdateGridValues(grid, 4, grid.getWidth(), 2, grid.getHeight());
initializeTestGrid(grid);
nav_grid::NavGridInfo bump_far_right_info = grid.getInfo();
bump_far_right_info.origin_x = 30;
grid.updateInfo(bump_far_right_info);
checkUpdateGridValues(grid, 0, 0, 0, 0);
}
TEST(VectorNavGrid, combined_changes)
{
// This is not a complete set of possible combined changes, just enough to satisfy my curiousity
nav_grid::VectorNavGrid<int> grid;
initializeTestGrid(grid);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo info1 = grid.getInfo();
info1.width = 15;
info1.origin_x = -5.0;
grid.updateInfo(info1);
checkUpdateGridValues(grid, 5, grid.getWidth(), 0, grid.getHeight());
initializeTestGrid(grid);
nav_grid::NavGridInfo info2 = grid.getInfo();
info2.width = 17;
info2.origin_x = -5.0;
grid.updateInfo(info2);
checkUpdateGridValues(grid, 5, grid.getWidth() - 2, 0, grid.getHeight());
initializeTestGrid(grid);
nav_grid::NavGridInfo info3 = grid.getInfo();
info3.width = 2;
info3.origin_x = 2.0;
grid.updateInfo(info3);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
initializeTestGrid(grid);
nav_grid::NavGridInfo info4 = grid.getInfo();
info4.width = 20;
info4.height = 20;
info4.origin_x = -2.0;
info4.origin_y = -5.0;
grid.updateInfo(info4);
checkUpdateGridValues(grid, 2, 12, 5, 10);
}
TEST(Index, comparison_tests)
{
unsigned int N = 5;
for (unsigned int x0 = 0; x0 < N; ++x0)
{
for (unsigned int y0 = 0; y0 < N; ++y0)
{
nav_grid::Index index0(x0, y0);
for (unsigned int x1 = 0; x1 < N; ++x1)
{
for (unsigned int y1 = 0; y1 < N; ++y1)
{
nav_grid::Index index1(x1, y1);
// Check equality and the test for equality that sets use
// See https://stackoverflow.com/a/1114862
if (x0 == x1 && y0 == y1)
{
EXPECT_EQ(index0, index1);
EXPECT_TRUE(!(index0 < index1) && !(index1 < index0));
EXPECT_GE(index0, index1);
EXPECT_LE(index0, index1);
EXPECT_GE(index1, index0);
EXPECT_LE(index1, index0);
}
else
{
EXPECT_NE(index0, index1);
EXPECT_FALSE(!(index0 < index1) && !(index1 < index0));
if (x0 < x1 || (x0 == x1 && y0 < y1))
{
EXPECT_LT(index0, index1);
EXPECT_GT(index1, index0);
EXPECT_LE(index0, index1);
EXPECT_GE(index1, index0);
}
else
{
EXPECT_GT(index0, index1);
EXPECT_LT(index1, index0);
EXPECT_GE(index0, index1);
EXPECT_LE(index1, index0);
}
}
}
}
}
}
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}