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

58
navigations/nav_grid_iterators/CMakeLists.txt Executable file
View File

@@ -0,0 +1,58 @@
cmake_minimum_required(VERSION 3.0.2)
project(nav_grid_iterators)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11;-Wall;-Werror")
find_package(catkin REQUIRED COMPONENTS
nav_grid nav_2d_msgs nav_2d_utils nav_msgs roscpp
)
catkin_package(
CATKIN_DEPENDS nav_grid nav_2d_msgs nav_2d_utils nav_msgs roscpp
INCLUDE_DIRS include
LIBRARIES nav_grid_iterators
)
add_library(nav_grid_iterators
src/whole_grid.cpp
src/sub_grid.cpp
src/circle_fill.cpp
src/circle_outline.cpp
src/spiral.cpp
src/bresenham.cpp
src/ray_trace.cpp
src/line.cpp
src/polygon_outline.cpp
src/polygon_fill.cpp
)
target_link_libraries(
nav_grid_iterators ${catkin_LIBRARIES}
)
add_executable(demo demo/demo.cpp)
add_dependencies(demo ${catkin_EXPORTED_TARGETS})
target_link_libraries(demo nav_grid_iterators ${catkin_LIBRARIES})
include_directories(
include ${catkin_INCLUDE_DIRS}
)
if (CATKIN_ENABLE_TESTING)
find_package(roslint REQUIRED)
roslint_cpp()
roslint_add_test()
catkin_add_gtest(line_tests test/line_tests.cpp)
target_link_libraries(line_tests nav_grid_iterators)
catkin_add_gtest(${PROJECT_NAME}_utest test/utest.cpp)
target_link_libraries(${PROJECT_NAME}_utest nav_grid_iterators)
endif (CATKIN_ENABLE_TESTING)
install(TARGETS nav_grid_iterators
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)

32
navigations/nav_grid_iterators/README.md Executable file
View File

@@ -0,0 +1,32 @@
# nav_grid_iterators
This package provides C++ iterators for iterating over some portion of a `NavGrid`. There are two sets. The first are signed line iterators which are not constrained to valid `NavGrid` indexes. Second, are the general iterators which are constrained to valid `NavGrid` indexes.
## Signed Line Iteration
As a building block for the general iterators, we provide two line iterators that iterate over `int` coordinates. Both take two pairs of coordinates for start and end points, as well as a boolean for whether to include the coordinates of the end point in the iteration.
* [`Bresenham`](include/nav_grid_iterators/line/bresenham.h) takes integer coordinates as input and implements [Bresenham's line algorithm](https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm), which means that there is either only one cell per column or one cell per row.
* [`RayTrace`](include/nav_grid_iterators/line/ray_trace.h) takes double coordinates as input and implements [Ray tracing](https://en.wikipedia.org/wiki/Ray_tracing_(graphics)), which means that it iterates over all cells that the line passes through, even if only briefly.
## General Iterators
* [`WholeGrid`](include/nav_grid_iterators/whole_grid.h) iterates over every cell in the grid in row major order.
* [`SubGrid`](include/nav_grid_iterators/sub_grid.h) iterates over a rectangular subportion of the grid in row major order.
* [`Line`](include/nav_grid_iterators/line.h) iterates over a line, using either of the above algorithms, but the coordinates are constrained to the grid.
* [`PolygonFill`](include/nav_grid_iterators/polygon_fill.h) iterates over all the cells whose centers fall within a `nav_2d_msgs::Polygon2D`
* [`PolygonOutline`](include/nav_grid_iterators/polygon_outline.h) iterates over the outline of a `nav_2d_msgs::Polygon2D` using either of the two above line iterators.
* [`CircleFill`](include/nav_grid_iterators/circle_fill.h) iterates over all the cells whose centers fall within a given circle, iterating in row major order.
* [`Spiral`](include/nav_grid_iterators/spiral.h) iterates over the same cells as `CircleFill` but from the center of the circle outward.
* [`CircleOutline`](include/nav_grid_iterators/circle_outline.h) iterates around the outline of a circle.
# Demo
A demonstration of all the general iterators can be seen by running `roslaunch nav_grid_iterators demo.launch` or by looking at [this video](demo/demo.mp4).
* The purple iterator is `WholeGrid`
* The bottom row, left to right, are
* `SubGrid` (green)
* `PolygonFill` (yellow)
* `PolygonOutline+Bresenham` (blue)
* `PolygonOutline+RayTrace` (blue)
* In the middle are a `Line+Bresenham` (bottom) and a `Line+RayTrace` (top)
* The top row, left to right, are
* `CircleFill` (grey)
* `Spiral` (green)
* `CircleOutline` (cyan)

160
navigations/nav_grid_iterators/demo/demo.cpp Executable file
View File

@@ -0,0 +1,160 @@
/*
* 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 <ros/ros.h>
#include <nav_grid_iterators/iterators.h>
#include <nav_grid/vector_nav_grid.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_2d_utils/conversions.h>
#include <vector>
template<class Iterator>
class InfiniteIterator
{
public:
InfiniteIterator(Iterator it, unsigned char active_value, unsigned char seen_value)
: it_(it), active_(active_value), seen_(seen_value)
{
}
void iterate(nav_grid::NavGrid<unsigned char>& grid)
{
// Note that this demo assumes that each iterator has at least one point on the grid
grid.setValue(*it_, seen_);
++it_;
if (it_ == it_.end())
{
it_ = it_.begin();
}
grid.setValue(*it_, active_);
}
private:
Iterator it_;
unsigned char active_, seen_;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "iterator_demo");
nav_grid::VectorNavGrid<unsigned char> grid;
nav_grid::NavGridInfo info;
sleep(5.0);
info.width = 75;
info.height = 60;
info.resolution = 0.1;
grid.setInfo(info);
ros::NodeHandle nh("~");
ros::Publisher pub = nh.advertise<nav_msgs::OccupancyGrid>("/map", 1);
ros::Rate r(33);
nav_2d_msgs::Polygon2D triangle;
triangle.points.resize(3);
triangle.points[0].x = 1.9;
triangle.points[0].y = 0.5;
triangle.points[1].x = 3.5;
triangle.points[1].y = 1.0;
triangle.points[2].x = 2.0;
triangle.points[2].y = 2.4;
nav_2d_msgs::Polygon2D diamond;
diamond.points.resize(4);
diamond.points[0].x = 4.5;
diamond.points[0].y = 0.0;
diamond.points[1].x = 5.3;
diamond.points[1].y = 1.25;
diamond.points[2].x = 4.5;
diamond.points[2].y = 2.5;
diamond.points[3].x = 3.7;
diamond.points[3].y = 1.25;
nav_2d_msgs::Polygon2D diamond2;
diamond2.points.resize(4);
diamond2.points[0].x = 6.5;
diamond2.points[0].y = 0.0;
diamond2.points[1].x = 7.3;
diamond2.points[1].y = 1.25;
diamond2.points[2].x = 6.5;
diamond2.points[2].y = 2.5;
diamond2.points[3].x = 5.7;
diamond2.points[3].y = 1.25;
InfiniteIterator<nav_grid_iterators::WholeGrid> whole_grid(nav_grid_iterators::WholeGrid(&info), 100, 50);
InfiniteIterator<nav_grid_iterators::SubGrid> sub_grid(nav_grid_iterators::SubGrid(&info, 3, 5, 14, 15), 99, 101);
InfiniteIterator<nav_grid_iterators::Line> line(nav_grid_iterators::Line(&info, 7.3, 2.8, 0.2, 2.5), 126, 200);
InfiniteIterator<nav_grid_iterators::Line> line2(nav_grid_iterators::Line(&info, 7.3, 3.3, 0.2, 3.0, true, false),
126, 200);
InfiniteIterator<nav_grid_iterators::CircleFill> circle_fill(nav_grid_iterators::CircleFill(&info, 1.25, 4.8, 1.0),
254, 255);
InfiniteIterator<nav_grid_iterators::Spiral> spiral(nav_grid_iterators::Spiral(&info, 3.75, 4.8, 1.0), 254, 126);
InfiniteIterator<nav_grid_iterators::CircleOutline> circle_o(nav_grid_iterators::CircleOutline(&info, 6.25, 4.8, 1.0),
254, 99);
InfiniteIterator<nav_grid_iterators::PolygonFill> poly_f(nav_grid_iterators::PolygonFill(&info, triangle), 126, 254);
InfiniteIterator<nav_grid_iterators::PolygonOutline> poly_o(nav_grid_iterators::PolygonOutline(&info, diamond), 0, 1);
InfiniteIterator<nav_grid_iterators::PolygonOutline> poly_r(nav_grid_iterators::PolygonOutline(&info, diamond2, 0),
0, 1);
nav_msgs::OccupancyGrid ogrid;
ogrid.header.frame_id = info.frame_id;
ogrid.info = nav_2d_utils::infoToInfo(info);
ogrid.data.resize(info.width * info.height);
while (ros::ok())
{
whole_grid.iterate(grid);
sub_grid.iterate(grid);
line.iterate(grid);
line2.iterate(grid);
circle_fill.iterate(grid);
spiral.iterate(grid);
circle_o.iterate(grid);
poly_f.iterate(grid);
poly_o.iterate(grid);
poly_r.iterate(grid);
// Manaully creating OccupancyGrid (rather than use nav_grid_pub_sub) to avoid circular dependency
ogrid.header.stamp = ros::Time::now();
unsigned int data_index = 0;
for (const nav_grid::Index& index : nav_grid_iterators::WholeGrid(info))
{
ogrid.data[data_index++] = grid(index);
}
pub.publish(ogrid);
r.sleep();
}
return 0;
}

4
navigations/nav_grid_iterators/demo/demo.launch Executable file
View File

@@ -0,0 +1,4 @@
<launch>
<node name="demo" pkg="nav_grid_iterators" type="demo" output="screen" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find nav_grid_iterators)/demo/demo.rviz" />
</launch>

BIN
navigations/nav_grid_iterators/demo/demo.mp4 Executable file
View File

Binary file not shown.

19
navigations/nav_grid_iterators/demo/demo.rviz Executable file
View File

@@ -0,0 +1,19 @@
Visualization Manager:
Displays:
- Class: rviz/Map
Color Scheme: costmap
Enabled: true
Name: Map
Topic: /map
Views:
Current:
Angle: 0
Class: rviz/TopDownOrtho
Name: Current View
Scale: 100
Value: TopDownOrtho (rviz)
X: 3.5
Y: 2.5
Window Geometry:
Height: 1400
Width: 2500

139
navigations/nav_grid_iterators/include/nav_grid_iterators/base_iterator.h Executable file
View File

@@ -0,0 +1,139 @@
/*
* 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_ITERATORS_BASE_ITERATOR_H
#define NAV_GRID_ITERATORS_BASE_ITERATOR_H
#include <nav_grid/nav_grid_info.h>
#include <nav_grid/index.h>
namespace nav_grid_iterators
{
template<class Derived>
class BaseIterator
{
public:
/**
* @brief Public Constructor. Takes in a pointer to the info and starts at the minimum index
* @param info NavGridInfo for the grid to iterate over
*/
explicit BaseIterator(const nav_grid::NavGridInfo* info) : BaseIterator(info, nav_grid::Index(0, 0)) {}
/**
* @brief Public Constructor. Takes in a reference to the info and starts at the minimum index
* @param info NavGridInfo for the grid to iterate over
*/
explicit BaseIterator(const nav_grid::NavGridInfo& info) : BaseIterator(&info, nav_grid::Index(0, 0)) {}
/**
* @brief Destructor
*/
virtual ~BaseIterator() = default;
/**
* @brief Helper function for range-style iteration
* Equivalent to the above constructor
* @return Iterator representing beginning of the iteration
*/
virtual Derived begin() const = 0;
/**
* @brief Helper function for range-style iteration
* @return Iterator representing end of the iteration, with an invalid index
*/
virtual Derived end() const = 0;
/**
* @brief Test if two iterators are equivalent
*
* Derived classes may want to implement the fieldsEqual function
* for checking if additional fields beyond the index and info are equal.
*/
bool operator==(const Derived& other) { return info_ == other.info_ && index_ == other.index_ && fieldsEqual(other); }
/**
* @brief Test if two iterators are not equivalent - required for testing if iterator is at the end
*/
bool operator!=(const Derived& other) { return !(*this == other); }
/**
* @brief Additional check for whether fields of derived iterators are equal.
*
* Helps make overriding the == operator easy.
*/
virtual bool fieldsEqual(const Derived& other) { return true; }
/**
* @brief Dereference the iterator
* @return the index to which the iterator is pointing.
*/
const nav_grid::Index& operator*() const { return index_; }
/**
* @brief Increase the iterator to the next element.
* @return a reference to the updated iterator.
*/
Derived& operator++()
{
increment();
return *static_cast<Derived*>(this);
}
/**
* @brief Increase the iterator to the next element.
*/
virtual void increment() = 0;
using self_type = Derived;
using value_type = nav_grid::Index;
using reference = nav_grid::Index&;
using pointer = nav_grid::Index*;
using iterator_category = std::input_iterator_tag;
using difference_type = int;
protected:
/**
* @brief Protected constructor that takes in an arbitrary index
* @param info NavGridInfo for the grid to iterate over
* @param index Initial index
*/
BaseIterator(const nav_grid::NavGridInfo* info, const nav_grid::Index& index) : info_(info), index_(index) {}
const nav_grid::NavGridInfo* info_;
nav_grid::Index index_;
};
} // namespace nav_grid_iterators
#endif // NAV_GRID_ITERATORS_BASE_ITERATOR_H

113
navigations/nav_grid_iterators/include/nav_grid_iterators/circle_fill.h Executable file
View File

@@ -0,0 +1,113 @@
/*
* 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_ITERATORS_CIRCLE_FILL_H
#define NAV_GRID_ITERATORS_CIRCLE_FILL_H
#include <nav_grid_iterators/base_iterator.h>
#include <nav_grid_iterators/sub_grid.h>
#include <memory>
namespace nav_grid_iterators
{
/**
* @class CircleFill
* @brief Iterates over all of the valid indexes that lie within a circle in row major order
*/
class CircleFill : public BaseIterator<CircleFill>
{
public:
/**
* @brief Public Constructor.
* @param info NavGridInfo for the grid to iterate over
* @param center_x Center of the circle (x coordinate)
* @param center_y Center of the circle (y coordinate)
* @param radius Size of the circle
*/
CircleFill(const nav_grid::NavGridInfo* info, double center_x, double center_y, double radius);
/**
* @brief Copy Constructor
* Required to ensure unique_ptr is set properly
*/
CircleFill(const CircleFill& other);
/**
* @brief Assignment Operator
* Required to ensure unique_ptr is set properly
*/
CircleFill& operator=(const CircleFill& other);
/**@name Standard BaseIterator Interface */
/**@{*/
CircleFill begin() const override;
CircleFill end() const override;
void increment() override;
bool fieldsEqual(const CircleFill& other) override;
/**@}*/
protected:
/**
* @brief Protected constructor that takes in an arbitrary index and other internal parameters
* @param info NavGridInfo for the grid to iterate over
* @param index Initial index
* @param center_x Center of the circle (x coordinate)
* @param center_y Center of the circle (y coordinate)
* @param radius_sq Square of the size of the circle
* @param min_x Minimum valid index that is within the circle (x coordinate)
* @param min_y Minimum valid index that is within the circle (y coordinate)
* @param width Maximum number of valid indexes in a row of the circle
* @param height Maximum number of of valid indexes in a column of the circle
* @param start_index The first valid index in the minimum row
*/
CircleFill(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, double center_x, double center_y,
double radius_sq, unsigned int min_x, unsigned int min_y, unsigned int width, unsigned int height,
const nav_grid::Index& start_index);
/**
* @brief Check if coordinates are inside the circle.
* @return true if inside, false otherwise.
*/
bool isInside(unsigned int x, unsigned int y) const;
double center_x_, center_y_, radius_sq_;
unsigned int min_x_, min_y_, width_, height_;
nav_grid::Index start_index_;
std::unique_ptr<SubGrid> internal_iterator_;
};
} // namespace nav_grid_iterators
#endif // NAV_GRID_ITERATORS_CIRCLE_FILL_H

124
navigations/nav_grid_iterators/include/nav_grid_iterators/circle_outline.h Executable file
View File

@@ -0,0 +1,124 @@
/*
* 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_ITERATORS_CIRCLE_OUTLINE_H
#define NAV_GRID_ITERATORS_CIRCLE_OUTLINE_H
#include <nav_grid_iterators/base_iterator.h>
namespace nav_grid_iterators
{
/**
* @brief returns the sign of a number
* @param val number
* @return 1 if positive, 0 if 0, -1 if negative
*/
inline int signum(const int val)
{
return (0 < val) - (val < 0);
}
/**
* @class CircleOutline
* @brief Iterates over the valid indexes that lie on the outline of a circle
*/
class CircleOutline : public BaseIterator<CircleOutline>
{
public:
/**
* @brief Public Constructor.
* @param info NavGridInfo for the grid to iterate over
* @param center_x Center of the circle (x coordinate)
* @param center_y Center of the circle (y coordinate)
* @param radius Size of the circle
*/
CircleOutline(const nav_grid::NavGridInfo* info, double center_x, double center_y, double radius);
/**
* @brief Public Constructor with integer radius.
* @param info NavGridInfo for the grid to iterate over
* @param center_x Center of the circle (x coordinate)
* @param center_y Center of the circle (y coordinate)
* @param radius Size of the circle
*/
CircleOutline(const nav_grid::NavGridInfo* info, double center_x, double center_y, unsigned int radius);
/**@name Standard BaseIterator Interface */
/**@{*/
CircleOutline begin() const override;
CircleOutline end() const override;
void increment() override;
bool fieldsEqual(const CircleOutline& other) override;
/**@}*/
protected:
/**
* @brief Protected constructor that takes in an arbitrary index and other internal parameters
* @param info NavGridInfo for the grid to iterate over
* @param index Initial index
* @param center_index_x Index of the center of the circle (x coordinate)
* @param center_index_y Index of the center of the circle (y coordinate)
* @param distance The number of cells in the radius of the circle
* @param init Whether the first cell has been visited or not
* @param start_index The first valid index in the minimum row
*/
CircleOutline(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, int center_index_x, int center_index_y,
unsigned int distance, bool init, const nav_grid::Index& start_index);
/**
* @brief Check if arbitrary coordinates are within the grid
* @return true if inside grid, false otherwise.
*/
bool isValidIndex(int x, int y) const;
/**
* @brief Check if a cell with the given distance from the center of the circle is on the outline of the circle
* @return true if the distance to the cell when rounded to an integer is equal to the distance_
*/
bool isOnOutline(int dx, int dy) const;
int center_index_x_, center_index_y_;
unsigned int distance_;
bool init_;
int signed_width_, signed_height_;
int point_x_, point_y_;
nav_grid::Index start_index_;
};
} // namespace nav_grid_iterators
#endif // NAV_GRID_ITERATORS_CIRCLE_OUTLINE_H

47
navigations/nav_grid_iterators/include/nav_grid_iterators/iterators.h Executable file
View File

@@ -0,0 +1,47 @@
/*
* 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_ITERATORS_ITERATORS_H
#define NAV_GRID_ITERATORS_ITERATORS_H
#include <nav_grid_iterators/whole_grid.h>
#include <nav_grid_iterators/sub_grid.h>
#include <nav_grid_iterators/circle_fill.h>
#include <nav_grid_iterators/circle_outline.h>
#include <nav_grid_iterators/spiral.h>
#include <nav_grid_iterators/line.h>
#include <nav_grid_iterators/polygon_outline.h>
#include <nav_grid_iterators/polygon_fill.h>
#endif // NAV_GRID_ITERATORS_ITERATORS_H

115
navigations/nav_grid_iterators/include/nav_grid_iterators/line.h Executable file
View File

@@ -0,0 +1,115 @@
/*
* 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_ITERATORS_LINE_H
#define NAV_GRID_ITERATORS_LINE_H
#include <nav_grid_iterators/base_iterator.h>
#include <nav_grid_iterators/line/abstract_line_iterator.h>
#include <memory>
namespace nav_grid_iterators
{
/**
* @class Line
* @brief Iterates over all of the valid indexes of a line
*/
class Line : public BaseIterator<Line>
{
public:
/**
* @brief Public Constructor.
* @param info NavGridInfo for the grid to iterate over
* @param x0 Start x coordinate
* @param y0 Start y coordinate
* @param x1 End x coordinate
* @param y1 End y coordinate
* @param include_last_index If true, will include the end coordinates.
*/
Line(const nav_grid::NavGridInfo* info, double x0, double y0, double x1, double y1,
bool include_last_index = true, bool bresenham = true);
/**
* @brief Copy Constructor
* Required to ensure unique_ptr is set properly
*/
Line(const Line& other);
/**
* @brief Assignment Operator
* Required to ensure unique_ptr is set properly
*/
Line& operator=(const Line& other);
/**@name Standard BaseIterator Interface */
/**@{*/
Line begin() const override;
Line end() const override;
void increment() override;
bool fieldsEqual(const Line& other) override;
/**@}*/
protected:
/**
* @brief Protected constructor that takes in an arbitrary index and other internal parameters
* @param info NavGridInfo for the grid to iterate over
* @param index Initial index
* @param x0 Start x coordinate
* @param y0 Start y coordinate
* @param x1 End x coordinate
* @param y1 End y coordinate
* @param include_last_index If true, will include the end coordinates.
* @param start_index The first valid index
* @param end_index The first invalid index
*/
Line(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, double x0, double y0, double x1, double y1,
bool include_last_index, bool bresenham, nav_grid::Index start_index, nav_grid::Index end_index);
void constructIterator();
/**
* @brief Check if a SignedIndex is within the bounds of the NavGrid
*/
bool inBounds(const nav_grid::SignedIndex& sindex);
std::unique_ptr<AbstractLineIterator> internal_iterator_;
double x0_, y0_, x1_, y1_;
bool include_last_index_;
bool bresenham_;
int signed_width_, signed_height_;
nav_grid::Index start_index_, end_index_;
};
} // namespace nav_grid_iterators
#endif // NAV_GRID_ITERATORS_LINE_H

89
navigations/nav_grid_iterators/include/nav_grid_iterators/line/abstract_line_iterator.h Executable file
View File

@@ -0,0 +1,89 @@
/*
* 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_ITERATORS_LINE_ABSTRACT_LINE_ITERATOR_H
#define NAV_GRID_ITERATORS_LINE_ABSTRACT_LINE_ITERATOR_H
#include <nav_grid/index.h>
namespace nav_grid_iterators
{
/**
* @class AbstractLineIterator
* @brief Abstract class for iterating over lines.
*
* Not constrained by a bounding box from NavGridInfo, i.e. can include positive and negative indexes
*/
class AbstractLineIterator
{
public:
/**
* @brief Public Constructor
*/
AbstractLineIterator() {}
/**
* @brief Public Destructor
*/
virtual ~AbstractLineIterator() = default;
/**
* @brief Dereference the iterator
* @return the index to which the iterator is pointing.
*/
const nav_grid::SignedIndex& operator*() const { return index_; }
virtual nav_grid::SignedIndex getFinalIndex() const = 0;
bool isFinished()
{
return getFinalIndex() == index_;
}
/**
* @brief Increase the iterator to the next element.
*/
virtual void increment() = 0;
protected:
/**
* @brief Protected Constructor - takes arbitrary index
*/
explicit AbstractLineIterator(nav_grid::SignedIndex index) : index_(index) {}
nav_grid::SignedIndex index_;
};
} // namespace nav_grid_iterators
#endif // NAV_GRID_ITERATORS_LINE_ABSTRACT_LINE_ITERATOR_H

136
navigations/nav_grid_iterators/include/nav_grid_iterators/line/bresenham.h Executable file
View File

@@ -0,0 +1,136 @@
/*
* 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_ITERATORS_LINE_BRESENHAM_H
#define NAV_GRID_ITERATORS_LINE_BRESENHAM_H
#include <nav_grid_iterators/line/abstract_line_iterator.h>
namespace nav_grid_iterators
{
/**
* @class Bresenham
* @brief Line Iterator using Bresenham's algorithm (no subpixel precision)
*/
class Bresenham : public AbstractLineIterator
{
public:
/**
* @brief Public constructor
* @param x0 Start x coordinate
* @param y0 Start y coordinate
* @param x1 End x coordinate
* @param y1 End y coordinate
* @param include_last_index If true, will include the end coordinates.
*/
Bresenham(int x0, int y0, int x1, int y1, bool include_last_index = true);
/**
* @brief Test if two iterators are equivalent
*/
bool operator==(const Bresenham& other)
{
return x0_ == other.x0_ && y0_ == other.y0_ && x1_ == other.x1_ && y1_ == other.y1_ && index_ == other.index_;
}
/**
* @brief Test if two iterators are not equivalent - required for testing if iterator is at the end
*/
bool operator!=(const Bresenham& other) { return !(*this == other); }
/**
* @brief Helper function for range-style iteration
* @return Iterator representing beginning of the iteration
*/
Bresenham begin() const;
/**
* @brief Helper function for range-style iteration
* @return Iterator representing end of the iteration, with an invalid index
*/
Bresenham end() const;
nav_grid::SignedIndex getFinalIndex() const override;
void increment() override;
/**
* @brief Increase the iterator to the next element.
* @return a reference to the updated iterator.
*/
Bresenham& operator++()
{
increment();
return *this;
}
using self_type = Bresenham;
using value_type = nav_grid::SignedIndex;
using reference = nav_grid::SignedIndex&;
using pointer = nav_grid::SignedIndex*;
using iterator_category = std::input_iterator_tag;
using difference_type = int;
protected:
/**
* @brief Protected constructor that takes in an arbitrary index and other internal parameters
* @param index Initial index
* @param x0 Start x coordinate
* @param y0 Start y coordinate
* @param x1 End x coordinate
* @param y1 End y coordinate
* @param include_last_index If true, will include the end coordinates.
* @param error_inc_x
* @param loop_inc_x
* @param error_inc_y
* @param loop_inc_y
* @param denominator
* @param numerator
* @param numerator_inc
*/
Bresenham(const nav_grid::SignedIndex& index,
int x0, int y0, int x1, int y1, bool include_last_index,
int error_inc_x, int loop_inc_x, int error_inc_y, int loop_inc_y,
int denominator, int numerator, int numerator_inc);
int x0_, y0_, x1_, y1_;
bool include_last_index_;
int error_inc_x_, loop_inc_x_, error_inc_y_, loop_inc_y_;
int denominator_, numerator_, numerator_inc_;
};
} // namespace nav_grid_iterators
#endif // NAV_GRID_ITERATORS_LINE_BRESENHAM_H

132
navigations/nav_grid_iterators/include/nav_grid_iterators/line/ray_trace.h Executable file
View File

@@ -0,0 +1,132 @@
/*
* 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_ITERATORS_LINE_RAY_TRACE_H
#define NAV_GRID_ITERATORS_LINE_RAY_TRACE_H
#include <nav_grid_iterators/line/abstract_line_iterator.h>
namespace nav_grid_iterators
{
/**
* @class RayTrace
* @brief Line Iterator with Ray Tracing (subpixel accuracy)
*/
class RayTrace : public AbstractLineIterator
{
public:
/**
* @brief Public constructor
* @param x0 Start x coordinate
* @param y0 Start y coordinate
* @param x1 End x coordinate
* @param y1 End y coordinate
* @param include_last_index If true, will include the end coordinates.
*/
RayTrace(double x0, double y0, double x1, double y1, bool include_last_index = true);
/**
* @brief Test if two iterators are equivalent
*/
bool operator==(const RayTrace& other)
{
return x0_ == other.x0_ && y0_ == other.y0_ && x1_ == other.x1_ && y1_ == other.y1_ && index_ == other.index_;
}
/**
* @brief Test if two iterators are not equivalent - required for testing if iterator is at the end
*/
bool operator!=(const RayTrace& other) { return !(*this == other); }
/**
* @brief Helper function for range-style iteration
* @return Iterator representing beginning of the iteration
*/
RayTrace begin() const;
/**
* @brief Helper function for range-style iteration
* @return Iterator representing end of the iteration, with an invalid index
*/
RayTrace end() const;
nav_grid::SignedIndex getFinalIndex() const override;
void increment() override;
/**
* @brief Increase the iterator to the next element.
* @return a reference to the updated iterator.
*/
RayTrace& operator++()
{
increment();
return *this;
}
using self_type = RayTrace;
using value_type = nav_grid::SignedIndex;
using reference = nav_grid::SignedIndex&;
using pointer = nav_grid::SignedIndex*;
using iterator_category = std::input_iterator_tag;
using difference_type = int;
protected:
/**
* @brief Protected constructor that takes in an arbitrary index and other internal parameters
* @param index Initial index
* @param x0 Start x coordinate
* @param y0 Start y coordinate
* @param x1 End x coordinate
* @param y1 End y coordinate
* @param include_last_index If true, will include the end coordinates.
* @param dx
* @param dy
* @param initial_error
* @param loop_inc_x
* @param loop_inc_y
*/
RayTrace(const nav_grid::SignedIndex& index,
double x0, double y0, double x1, double y1, bool include_last_index,
double dx, double dy, double initial_error, int loop_inc_x, int loop_inc_y);
double x0_, y0_, x1_, y1_;
bool include_last_index_;
double dx_, dy_, error_, initial_error_;
int loop_inc_x_, loop_inc_y_;
};
} // namespace nav_grid_iterators
#endif // NAV_GRID_ITERATORS_LINE_RAY_TRACE_H

109
navigations/nav_grid_iterators/include/nav_grid_iterators/polygon_fill.h Executable file
View File

@@ -0,0 +1,109 @@
/*
* 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_ITERATORS_POLYGON_FILL_H
#define NAV_GRID_ITERATORS_POLYGON_FILL_H
#include <nav_grid_iterators/base_iterator.h>
#include <nav_grid_iterators/sub_grid.h>
#include <nav_2d_msgs/Polygon2D.h>
#include <memory>
namespace nav_grid_iterators
{
/**
* @class PolygonFill
* @brief Iterates over all of the valid indexes that lie within an arbitrary polygon in row major order
*/
class PolygonFill : public BaseIterator<PolygonFill>
{
public:
/**
* @brief Public Constructor.
* @param info NavGridInfo for the grid to iterate over
* @param polygon Polygon to iterate over
*/
PolygonFill(const nav_grid::NavGridInfo* info, nav_2d_msgs::Polygon2D polygon);
/**
* @brief Copy Constructor
* Required to ensure unique_ptr is set properly
*/
PolygonFill(const PolygonFill& other);
/**
* @brief Assignment Operator
* Required to ensure unique_ptr is set properly
*/
PolygonFill& operator=(const PolygonFill& other);
/**@name Standard BaseIterator Interface */
/**@{*/
PolygonFill begin() const override;
PolygonFill end() const override;
void increment() override;
bool fieldsEqual(const PolygonFill& other) override;
/**@}*/
protected:
/**
* @brief Protected constructor that takes in an arbitrary index and other internal parameters
* @param info NavGridInfo for the grid to iterate over
* @param index Initial index
* @param polygon Polygon to iterate over
* @param min_x Minimum valid index that is within the polygon (x coordinate)
* @param min_y Minimum valid index that is within the polygon (y coordinate)
* @param width Maximum number of valid indexes in a row of the polygon
* @param height Maximum number of of valid indexes in a column of the polygon
* @param start_index The first valid index in the minimum row
*/
PolygonFill(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, nav_2d_msgs::Polygon2D polygon,
unsigned int min_x, unsigned int min_y, unsigned int width, unsigned int height,
const nav_grid::Index& start_index);
/**
* @brief Check if given index is inside the polygon.
* @return true if inside, false otherwise.
*/
bool isInside(unsigned int x, unsigned int y) const;
nav_2d_msgs::Polygon2D polygon_;
unsigned int min_x_, min_y_, width_, height_;
nav_grid::Index start_index_;
std::unique_ptr<SubGrid> internal_iterator_;
};
} // namespace nav_grid_iterators
#endif // NAV_GRID_ITERATORS_POLYGON_FILL_H

104
navigations/nav_grid_iterators/include/nav_grid_iterators/polygon_outline.h Executable file
View File

@@ -0,0 +1,104 @@
/*
* 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_ITERATORS_POLYGON_OUTLINE_H
#define NAV_GRID_ITERATORS_POLYGON_OUTLINE_H
#include <nav_grid_iterators/base_iterator.h>
#include <nav_grid_iterators/line.h>
#include <nav_2d_msgs/Polygon2D.h>
#include <memory>
namespace nav_grid_iterators
{
/**
* @class PolygonOutline
* @brief Iterates over all of the valid indexes on the outline of a polygon
*/
class PolygonOutline : public BaseIterator<PolygonOutline>
{
public:
/**
* @brief Public Constructor.
* @param info NavGridInfo for the grid to iterate over
* @param polygon Polygon to iterate over
*/
PolygonOutline(const nav_grid::NavGridInfo* info, nav_2d_msgs::Polygon2D polygon, bool bresenham = true);
/**
* @brief Copy Constructor
* Required to ensure unique_ptr is set properly
*/
PolygonOutline(const PolygonOutline& other);
/**
* @brief Assignment Operator
* Required to ensure unique_ptr is set properly
*/
PolygonOutline& operator=(const PolygonOutline& other);
/**@name Standard BaseIterator Interface */
/**@{*/
PolygonOutline begin() const override;
PolygonOutline end() const override;
void increment() override;
bool fieldsEqual(const PolygonOutline& other) override;
/**@}*/
protected:
/**
* @brief Protected constructor that takes in an arbitrary index and other internal parameters
* @param info NavGridInfo for the grid to iterate over
* @param index Initial index
* @param polygon Polygon to iterate over
* @param side_index Which side we are currently iterating over
*/
PolygonOutline(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, nav_2d_msgs::Polygon2D polygon,
bool bresenham, unsigned int side_index);
/**
* @brief Given a new side index, loads the internal iterator.
* If there are no valid values in the internal iterator, increases the side index.
*/
void loadSide();
std::unique_ptr<Line> internal_iterator_;
nav_2d_msgs::Polygon2D polygon_;
nav_grid::Index start_index_;
bool bresenham_;
unsigned int side_index_;
};
} // namespace nav_grid_iterators
#endif // NAV_GRID_ITERATORS_POLYGON_OUTLINE_H

116
navigations/nav_grid_iterators/include/nav_grid_iterators/spiral.h Executable file
View File

@@ -0,0 +1,116 @@
/*
* 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_ITERATORS_SPIRAL_H
#define NAV_GRID_ITERATORS_SPIRAL_H
#include <nav_grid_iterators/base_iterator.h>
#include <nav_grid_iterators/circle_outline.h>
#include <memory>
namespace nav_grid_iterators
{
/**
* @class Spiral
* @brief Iterates over all of the valid indexes that lie within a circle from the center out
*/
class Spiral : public BaseIterator<Spiral>
{
public:
/**
* @brief Public Constructor.
* @param info NavGridInfo for the grid to iterate over
* @param center_x Center of the circle (x coordinate)
* @param center_y Center of the circle (y coordinate)
* @param radius Size of the circle
*/
Spiral(const nav_grid::NavGridInfo* info, double center_x, double center_y, double radius);
/**
* @brief Copy Constructor
* Required to ensure unique_ptr is set properly
*/
Spiral(const Spiral& other);
/**
* @brief Assignment Operator
* Required to ensure unique_ptr is set properly
*/
Spiral& operator=(const Spiral& other);
/**@name Standard BaseIterator Interface */
/**@{*/
Spiral begin() const override;
Spiral end() const override;
void increment() override;
bool fieldsEqual(const Spiral& other) override;
/**@}*/
protected:
/**
* @brief Protected constructor that takes in an arbitrary index and other internal parameters
* @param info NavGridInfo for the grid to iterate over
* @param index Initial index
* @param center_x Center of the circle (x coordinate)
* @param center_y Center of the circle (y coordinate)
* @param radius_sq Square of the size of the circle
* @param distance Which ring of the spiral to start on
* @param max_distance The maximum valid ring
* @param start_index The first valid index in the spiral
*/
Spiral(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, double center_x, double center_y,
double radius_sq, unsigned int distance, unsigned int max_distance,
const nav_grid::Index& start_index);
/**
* @brief Given a new distance value, loads the internal iterator.
* If there are no valid values in the internal iterator, increases the distance.
*/
void loadRing();
/**
* @brief Check if the center of the given index is within the circle
* @return true if inside
*/
bool isInside(unsigned int x, unsigned int y) const;
double center_x_, center_y_, radius_sq_;
unsigned int distance_, max_distance_;
nav_grid::Index start_index_;
std::unique_ptr<CircleOutline> internal_iterator_;
};
} // namespace nav_grid_iterators
#endif // NAV_GRID_ITERATORS_SPIRAL_H

104
navigations/nav_grid_iterators/include/nav_grid_iterators/sub_grid.h Executable file
View File

@@ -0,0 +1,104 @@
/*
* 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_ITERATORS_SUB_GRID_H
#define NAV_GRID_ITERATORS_SUB_GRID_H
#include <nav_grid_iterators/base_iterator.h>
#include <nav_core2/bounds.h>
namespace nav_grid_iterators
{
/**
* @class SubGrid
* @brief Iterator for looping through every index within an aligned rectangular portion of the grid
*/
class SubGrid : public BaseIterator<SubGrid>
{
public:
/**
* @brief Public Constructor
* @param info NavGridInfo for the grid to iterate over
* @param min_x Minimum index (x coordinate)
* @param min_y Minimum index (y coordinate)
* @param width Number of indexes in the x direction
* @param height Number of indexes in the y direction
*/
SubGrid(const nav_grid::NavGridInfo* info, unsigned int min_x, unsigned int min_y,
unsigned int width, unsigned int height)
: SubGrid(info, nav_grid::Index(min_x, min_y), min_x, min_y, width, height) {}
/**
* @brief Public Constructor using UIntBounds object
* @param info NavGridInfo for the grid to iterate over
* @param bounds UIntBounds
*/
SubGrid(const nav_grid::NavGridInfo* info, const nav_core2::UIntBounds& bounds)
: SubGrid(info, bounds.getMinX(), bounds.getMinY(), bounds.getWidth(), bounds.getHeight()) {}
/**
* @brief Public constructor that takes in an arbitrary index
* @param info NavGridInfo for the grid to iterate over
* @param index Initial index
* @param min_x Minimum index (x coordinate)
* @param min_y Minimum index (y coordinate)
* @param width Number of indexes in the x direction
* @param height Number of indexes in the y direction
*/
SubGrid(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, unsigned int min_x, unsigned int min_y,
unsigned int width, unsigned int height);
/**
* @brief Public constructor using UIntBounds object that takes in an arbitrary index
* @param info NavGridInfo for the grid to iterate over
* @param index Initial index
* @param bounds UIntBounds
*/
SubGrid(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, const nav_core2::UIntBounds& bounds)
: SubGrid(info, index, bounds.getMinX(), bounds.getMinY(), bounds.getWidth(), bounds.getHeight()) {}
/**@name Standard BaseIterator Interface */
/**@{*/
SubGrid begin() const override;
SubGrid end() const override;
void increment() override;
bool fieldsEqual(const SubGrid& other) override;
/**@}*/
protected:
unsigned int min_x_, min_y_, width_, height_;
};
} // namespace nav_grid_iterators
#endif // NAV_GRID_ITERATORS_SUB_GRID_H

60
navigations/nav_grid_iterators/include/nav_grid_iterators/whole_grid.h Executable file
View File

@@ -0,0 +1,60 @@
/*
* 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_ITERATORS_WHOLE_GRID_H
#define NAV_GRID_ITERATORS_WHOLE_GRID_H
#include <nav_grid_iterators/base_iterator.h>
namespace nav_grid_iterators
{
/**
* @class WholeGrid
* An iterator that will iterate through all the cells of a grid in row-major order
*/
class WholeGrid : public BaseIterator<WholeGrid>
{
public:
using BaseIterator<WholeGrid>::BaseIterator;
/**@name Standard BaseIterator Interface */
/**@{*/
WholeGrid begin() const override;
WholeGrid end() const override;
void increment() override;
/**@}*/
};
} // namespace nav_grid_iterators
#endif // NAV_GRID_ITERATORS_WHOLE_GRID_H

21
navigations/nav_grid_iterators/package.xml Executable file
View File

@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<package format="2">
<name>nav_grid_iterators</name>
<version>0.3.0</version>
<description>
Iterator implementations for moving around the cells of a nav_grid in a number of common patterns.
</description>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>nav_2d_msgs</depend>
<depend>nav_2d_utils</depend>
<depend>nav_grid</depend>
<depend>nav_msgs</depend>
<depend>roscpp</depend>
<test_depend>roslint</test_depend>
<test_depend>rosunit</test_depend>
</package>

120
navigations/nav_grid_iterators/src/bresenham.cpp Executable file
View File

@@ -0,0 +1,120 @@
/*
* 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 <nav_grid_iterators/line/bresenham.h>
#include <cmath>
namespace nav_grid_iterators
{
Bresenham::Bresenham(int x0, int y0, int x1, int y1, bool include_last_index)
: AbstractLineIterator(nav_grid::SignedIndex(x0, y0)), x0_(x0), y0_(y0), x1_(x1), y1_(y1),
include_last_index_(include_last_index)
{
int dx = std::abs(x1_ - x0_);
int dy = std::abs(y1_ - y0_);
int xsign = x1_ >= x0_ ? 1 : -1;
int ysign = y1_ >= y0_ ? 1 : -1;
if (dx >= dy) // There is at least one x-value for every y-value
{
loop_inc_x_ = xsign;
error_inc_x_ = 0; // Don't change the x when numerator >= denominator
loop_inc_y_ = 0; // Don't change the y for every iteration
error_inc_y_ = ysign;
denominator_ = dx;
numerator_ = dx / 2;
numerator_inc_ = dy;
}
else // There is at least one y-value for every x-value
{
loop_inc_x_ = 0; // Don't change the x for every iteration
error_inc_x_ = xsign;
loop_inc_y_ = ysign;
error_inc_y_ = 0; // Don't change the y when numerator >= denominator
denominator_ = dy;
numerator_ = dy / 2;
numerator_inc_ = dx;
}
}
Bresenham::Bresenham(const nav_grid::SignedIndex& index,
int x0, int y0, int x1, int y1, bool include_last_index,
int error_inc_x, int loop_inc_x, int error_inc_y, int loop_inc_y,
int denominator, int numerator, int numerator_inc)
: AbstractLineIterator(index), x0_(x0), y0_(y0), x1_(x1), y1_(y1), include_last_index_(include_last_index),
error_inc_x_(error_inc_x), loop_inc_x_(loop_inc_x), error_inc_y_(error_inc_y), loop_inc_y_(loop_inc_y),
denominator_(denominator), numerator_(numerator), numerator_inc_(numerator_inc)
{
}
Bresenham Bresenham::begin() const
{
return Bresenham(nav_grid::SignedIndex(x0_, y0_), x0_, y0_, x1_, y1_, include_last_index_,
error_inc_x_, loop_inc_x_, error_inc_y_, loop_inc_y_,
denominator_, numerator_, numerator_inc_);
}
Bresenham Bresenham::end() const
{
Bresenham end(nav_grid::SignedIndex(x1_, y1_), x0_, y0_, x1_, y1_, include_last_index_,
error_inc_x_, loop_inc_x_, error_inc_y_, loop_inc_y_,
denominator_, numerator_, numerator_inc_);
// If we want the last_index, return an iterator that is whatever is one-past the end coordinates
if (include_last_index_)
end.increment();
return end;
}
void Bresenham::increment()
{
numerator_ += numerator_inc_; // Increase the numerator by the top of the fraction
if (numerator_ >= denominator_) // Check if numerator >= denominator
{
numerator_ -= denominator_; // Calculate the new numerator value
index_.x += error_inc_x_; // Change the x as appropriate
index_.y += error_inc_y_; // Change the y as appropriate
}
index_.x += loop_inc_x_; // Change the x as appropriate
index_.y += loop_inc_y_; // Change the y as appropriate
}
nav_grid::SignedIndex Bresenham::getFinalIndex() const
{
return end().index_;
}
} // namespace nav_grid_iterators

141
navigations/nav_grid_iterators/src/circle_fill.cpp Executable file
View File

@@ -0,0 +1,141 @@
/*
* 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 <nav_grid_iterators/circle_fill.h>
#include <nav_grid/coordinate_conversion.h>
namespace nav_grid_iterators
{
CircleFill::CircleFill(const nav_grid::NavGridInfo* info, double center_x, double center_y, double radius)
: BaseIterator(info), center_x_(center_x), center_y_(center_y), start_index_(0, 0)
{
radius_sq_ = radius * radius;
double min_x = center_x_ - radius;
double max_x = center_x_ + radius;
double min_y = center_y_ - radius;
double max_y = center_y_ + radius;
// Calculate and save the minimum coordinates
worldToGridBounded(*info_, min_x, min_y, min_x_, min_y_);
// Calculate the max coordinates, and save the width/height
unsigned int max_x_grid, max_y_grid;
worldToGridBounded(*info_, max_x, max_y, max_x_grid, max_y_grid);
width_ = max_x_grid - min_x_ + 1;
height_ = max_y_grid - min_y_ + 1;
// Initialize internal iterator
internal_iterator_.reset(new SubGrid(info_, min_x_, min_y_, width_, height_));
index_.x = min_x_;
index_.y = min_y_;
// Iterate to first valid index
if (!isInside(min_x_, min_y_)) ++(*this);
start_index_ = **internal_iterator_;
index_ = start_index_;
}
CircleFill::CircleFill(const nav_grid_iterators::CircleFill& other)
: CircleFill(other.info_, other.index_, other.center_x_, other.center_y_, other.radius_sq_,
other.min_x_, other.min_y_, other.width_, other.height_, other.start_index_)
{
}
CircleFill::CircleFill(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, double center_x,
double center_y, double radius_sq, unsigned int min_x, unsigned int min_y, unsigned int width,
unsigned int height, const nav_grid::Index& start_index)
: BaseIterator(info, index), center_x_(center_x), center_y_(center_y), radius_sq_(radius_sq),
min_x_(min_x), min_y_(min_y), width_(width), height_(height), start_index_(start_index)
{
internal_iterator_.reset(new SubGrid(info_, index_, min_x_, min_y_, width_, height_));
}
CircleFill& CircleFill::operator=(const CircleFill& other)
{
info_ = other.info_;
index_ = other.index_;
center_x_ = other.center_x_;
center_y_ = other.center_y_;
radius_sq_ = other.radius_sq_;
min_x_ = other.min_x_;
min_y_ = other.min_y_;
width_ = other.width_;
height_ = other.height_;
start_index_ = other.start_index_;
internal_iterator_.reset(new SubGrid(info_, index_, min_x_, min_y_, width_, height_));
return *this;
}
bool CircleFill::isInside(unsigned int x, unsigned int y) const
{
double wx, wy;
gridToWorld(*info_, x, y, wx, wy);
double dx = wx - center_x_;
double dy = wy - center_y_;
return (dx * dx + dy * dy) < radius_sq_;
}
CircleFill CircleFill::begin() const
{
return CircleFill(info_, start_index_, center_x_, center_y_, radius_sq_, min_x_, min_y_, width_, height_,
start_index_);
}
CircleFill CircleFill::end() const
{
return CircleFill(info_, *internal_iterator_->end(), center_x_, center_y_, radius_sq_, min_x_, min_y_,
width_, height_, start_index_);
}
void CircleFill::increment()
{
++(*internal_iterator_);
index_ = **internal_iterator_;
while (*internal_iterator_ != internal_iterator_->end())
{
if (isInside(index_.x, index_.y))
break;
++(*internal_iterator_);
index_ = **internal_iterator_;
}
}
bool CircleFill::fieldsEqual(const CircleFill& other)
{
return center_x_ == other.center_x_ && center_y_ == other.center_y_ && radius_sq_ == other.radius_sq_;
}
} // namespace nav_grid_iterators

143
navigations/nav_grid_iterators/src/circle_outline.cpp Executable file
View File

@@ -0,0 +1,143 @@
/*
* 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 <nav_grid_iterators/circle_outline.h>
#include <nav_grid/coordinate_conversion.h>
namespace nav_grid_iterators
{
CircleOutline::CircleOutline(const nav_grid::NavGridInfo* info, double center_x, double center_y, double radius)
: CircleOutline(info, center_x, center_y, static_cast<unsigned int>(ceil(radius / info->resolution)))
{
}
CircleOutline::CircleOutline(const nav_grid::NavGridInfo* info, double center_x, double center_y, unsigned int radius)
: BaseIterator(info), distance_(radius), init_(false)
{
signed_width_ = static_cast<int>(info->width);
signed_height_ = static_cast<int>(info->height);
// Calculate and save the center coordinates
worldToGrid(*info_, center_x, center_y, center_index_x_, center_index_y_);
point_x_ = distance_;
point_y_ = 0;
if (!isValidIndex(center_index_x_ + point_x_, center_index_y_ + point_y_))
{
increment();
init_ = !isValidIndex(center_index_x_ + point_x_, center_index_y_ + point_y_);
}
index_.x = center_index_x_ + point_x_;
index_.y = center_index_y_ + point_y_;
start_index_ = index_;
}
CircleOutline::CircleOutline(const nav_grid::NavGridInfo* info, const nav_grid::Index& index,
int center_index_x, int center_index_y, unsigned int distance,
bool init, const nav_grid::Index& start_index)
: BaseIterator(info, index),
center_index_x_(center_index_x), center_index_y_(center_index_y), distance_(distance), init_(init),
start_index_(start_index)
{
signed_width_ = static_cast<int>(info->width);
signed_height_ = static_cast<int>(info->height);
point_x_ = distance_;
point_y_ = 0;
}
CircleOutline CircleOutline::begin() const
{
return CircleOutline(info_, start_index_, center_index_x_, center_index_y_,
distance_, false, start_index_);
}
CircleOutline CircleOutline::end() const
{
return CircleOutline(info_, start_index_, center_index_x_, center_index_y_,
distance_, true, start_index_);
}
void CircleOutline::increment()
{
init_ = true;
while (true)
{
int nx = -signum(point_y_);
int ny = signum(point_x_);
if (nx != 0 && isOnOutline(point_x_ + nx, point_y_))
{
point_x_ += nx;
}
else if (ny != 0 && isOnOutline(point_x_, point_y_ + ny))
{
point_y_ += ny;
}
else
{
point_x_ += nx;
point_y_ += ny;
}
if (isValidIndex(center_index_x_ + point_x_, center_index_y_ + point_y_))
{
break;
}
if (point_x_ == static_cast<int>(distance_) && point_y_ == 0)
{
index_ = start_index_;
return;
}
}
index_.x = center_index_x_ + point_x_;
index_.y = center_index_y_ + point_y_;
}
bool CircleOutline::fieldsEqual(const CircleOutline& other)
{
return center_index_x_ == other.center_index_x_ && center_index_y_ == other.center_index_y_ &&
distance_ == other.distance_ && init_ == other.init_;
}
bool CircleOutline::isValidIndex(int x, int y) const
{
return x >= 0 && y >= 0 && x < signed_width_ && y < signed_height_;
}
bool CircleOutline::isOnOutline(int dx, int dy) const
{
return static_cast<unsigned int>(hypot(dx, dy)) == distance_;
}
} // namespace nav_grid_iterators

167
navigations/nav_grid_iterators/src/line.cpp Executable file
View File

@@ -0,0 +1,167 @@
/*
* 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 <nav_grid_iterators/line.h>
#include <nav_grid_iterators/line/bresenham.h>
#include <nav_grid_iterators/line/ray_trace.h>
#include <nav_grid/coordinate_conversion.h>
namespace nav_grid_iterators
{
Line::Line(const nav_grid::NavGridInfo* info, double x0, double y0, double x1, double y1,
bool include_last_index, bool bresenham)
: BaseIterator(info), x0_(x0), y0_(y0), x1_(x1), y1_(y1), include_last_index_(include_last_index),
bresenham_(bresenham), start_index_(0, 0), end_index_(0, 0)
{
constructIterator();
// Convenience variables to avoid mismatched comparisons
signed_width_ = static_cast<int>(info->width);
signed_height_ = static_cast<int>(info->height);
// Cache the end index
nav_grid::SignedIndex end = internal_iterator_->getFinalIndex();
end_index_.x = end.x;
end_index_.y = end.y;
// Iterate to first valid index
nav_grid::SignedIndex sindex = **internal_iterator_;
while (!internal_iterator_->isFinished() && !inBounds(sindex))
{
internal_iterator_->increment();
sindex = **internal_iterator_;
}
// If all the indices are invalid, explicitly set the start index to be invalid
if (internal_iterator_->isFinished())
{
start_index_ = end_index_;
}
else
{
start_index_.x = sindex.x;
start_index_.y = sindex.y;
}
index_ = start_index_;
}
Line::Line(const Line& other)
: Line(other.info_, other.index_, other.x0_, other.y0_, other.x1_, other.y1_, other.include_last_index_,
other.bresenham_, other.start_index_, other.end_index_)
{
}
Line::Line(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, double x0, double y0, double x1, double y1,
bool include_last_index, bool bresenham, nav_grid::Index start_index, nav_grid::Index end_index)
: BaseIterator(info, index), x0_(x0), y0_(y0), x1_(x1), y1_(y1), include_last_index_(include_last_index),
bresenham_(bresenham), start_index_(start_index), end_index_(end_index)
{
constructIterator();
signed_width_ = static_cast<int>(info->width);
signed_height_ = static_cast<int>(info->height);
}
Line& Line::operator=(const Line& other)
{
info_ = other.info_;
index_ = other.index_;
x0_ = other.x0_;
y0_ = other.y0_;
x1_ = other.x1_;
y1_ = other.y1_;
include_last_index_ = other.include_last_index_;
bresenham_ = other.bresenham_;
start_index_ = other.start_index_;
end_index_ = other.end_index_;
signed_width_ = other.signed_width_;
signed_height_ = other.signed_height_;
constructIterator();
return *this;
}
void Line::constructIterator()
{
// translate coordinates into grid coordinates
double local_x0, local_y0, local_x1, local_y1;
worldToGrid(*info_, x0_, y0_, local_x0, local_y0);
worldToGrid(*info_, x1_, y1_, local_x1, local_y1);
if (bresenham_)
{
internal_iterator_.reset(new Bresenham(local_x0, local_y0, local_x1, local_y1, include_last_index_));
}
else
{
internal_iterator_.reset(new RayTrace(local_x0, local_y0, local_x1, local_y1, include_last_index_));
}
}
bool Line::inBounds(const nav_grid::SignedIndex& sindex)
{
return sindex.x >= 0 && sindex.y >= 0 && sindex.x < signed_width_ && sindex.y < signed_height_;
}
Line Line::begin() const
{
return Line(info_, start_index_, x0_, y0_, x1_, y1_, include_last_index_, bresenham_, start_index_, end_index_);
}
Line Line::end() const
{
return Line(info_, end_index_, x0_, y0_, x1_, y1_, include_last_index_, bresenham_, start_index_, end_index_);
}
void Line::increment()
{
internal_iterator_->increment();
nav_grid::SignedIndex sindex = **internal_iterator_;
if (!internal_iterator_->isFinished() && !inBounds(sindex))
{
index_ = end_index_;
}
else
{
index_.x = sindex.x;
index_.y = sindex.y;
}
}
bool Line::fieldsEqual(const Line& other)
{
return x0_ == other.x0_ && y0_ == other.y0_ && x1_ == other.x1_ && y1_ == other.y1_ &&
include_last_index_ == other.include_last_index_ && bresenham_ == other.bresenham_;
}
} // namespace nav_grid_iterators

153
navigations/nav_grid_iterators/src/polygon_fill.cpp Executable file
View File

@@ -0,0 +1,153 @@
/*
* 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 <nav_grid_iterators/polygon_fill.h>
#include <nav_grid/coordinate_conversion.h>
#include <nav_2d_utils/polygons.h>
#include <algorithm>
namespace nav_grid_iterators
{
PolygonFill::PolygonFill(const nav_grid::NavGridInfo* info, nav_2d_msgs::Polygon2D polygon)
: BaseIterator(info), polygon_(polygon), start_index_(0, 0)
{
if (polygon.points.size() == 0)
{
internal_iterator_.reset(new SubGrid(info_, 0, 0, 0, 0));
start_index_ = **internal_iterator_;
index_ = start_index_;
return;
}
// Find the minimum and maximum coordinates of the vertices
double min_x = polygon_.points[0].x;
double max_x = min_x;
double min_y = polygon_.points[0].y;
double max_y = min_y;
for (const auto& vertex : polygon_.points)
{
min_x = std::min(min_x, vertex.x);
min_y = std::min(min_y, vertex.y);
max_x = std::max(max_x, vertex.x);
max_y = std::max(max_y, vertex.y);
}
// Save the minimum in grid coordinates
worldToGridBounded(*info_, min_x, min_y, min_x_, min_y_);
// Calculate the maximum in grid coordinates and then save the width/height
unsigned int max_x_grid, max_y_grid;
worldToGridBounded(*info_, max_x, max_y, max_x_grid, max_y_grid);
width_ = max_x_grid - min_x_ + 1;
height_ = max_y_grid - min_y_ + 1;
// Initialize internal iterator
internal_iterator_.reset(new SubGrid(info_, min_x_, min_y_, width_, height_));
index_.x = min_x_;
index_.y = min_y_;
// Iterate to first valid index
if (!isInside(index_.x, index_.y)) ++(*this);
start_index_ = **internal_iterator_;
index_ = start_index_;
}
PolygonFill::PolygonFill(const PolygonFill& other)
: PolygonFill(other.info_, other.index_, other.polygon_, other.min_x_, other.min_y_, other.width_, other.height_,
other.start_index_)
{
}
PolygonFill::PolygonFill(const nav_grid::NavGridInfo* info, const nav_grid::Index& index,
nav_2d_msgs::Polygon2D polygon,
unsigned int min_x, unsigned int min_y, unsigned int width, unsigned int height,
const nav_grid::Index& start_index)
: BaseIterator(info, index), polygon_(polygon),
min_x_(min_x), min_y_(min_y), width_(width), height_(height), start_index_(start_index)
{
internal_iterator_.reset(new SubGrid(info_, min_x_, min_y_, width_, height_));
}
PolygonFill& PolygonFill::operator=(const PolygonFill& other)
{
info_ = other.info_;
index_ = other.index_;
polygon_ = other.polygon_;
min_x_ = other.min_x_;
min_y_ = other.min_y_;
width_ = other.width_;
height_ = other.height_;
start_index_ = other.start_index_;
internal_iterator_.reset(new SubGrid(info_, index_, min_x_, min_y_, width_, height_));
return *this;
}
bool PolygonFill::isInside(unsigned int x, unsigned int y) const
{
// Determine if the current index is inside the polygon using the number of crossings method
double wx, wy;
gridToWorld(*info_, x, y, wx, wy);
return nav_2d_utils::isInside(polygon_, wx, wy);
}
PolygonFill PolygonFill::begin() const
{
return PolygonFill(info_, start_index_, polygon_, min_x_, min_y_, width_, height_, start_index_);
}
PolygonFill PolygonFill::end() const
{
return PolygonFill(info_, *internal_iterator_->end(), polygon_, min_x_, min_y_, width_, height_,
start_index_);
}
void PolygonFill::increment()
{
++(*internal_iterator_);
index_ = **internal_iterator_;
while (*internal_iterator_ != internal_iterator_->end())
{
if (isInside(index_.x, index_.y))
break;
++(*internal_iterator_);
index_ = **internal_iterator_;
}
}
bool PolygonFill::fieldsEqual(const PolygonFill& other)
{
return nav_2d_utils::equals(polygon_, other.polygon_);
}
} // namespace nav_grid_iterators

135
navigations/nav_grid_iterators/src/polygon_outline.cpp Executable file
View File

@@ -0,0 +1,135 @@
/*
* 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 <nav_grid_iterators/polygon_outline.h>
#include <nav_grid/coordinate_conversion.h>
#include <nav_2d_utils/polygons.h>
namespace nav_grid_iterators
{
PolygonOutline::PolygonOutline(const nav_grid::NavGridInfo* info, nav_2d_msgs::Polygon2D polygon, bool bresenham)
: BaseIterator(info), polygon_(polygon), start_index_(0, 0), bresenham_(bresenham), side_index_(0)
{
if (polygon.points.size() == 0)
{
internal_iterator_.reset(new Line(info_, 0.0, 0.0, 0.0, 0.0, false, bresenham_));
index_ = **internal_iterator_;
start_index_ = index_;
return;
}
loadSide();
index_ = **internal_iterator_;
start_index_ = index_;
}
PolygonOutline::PolygonOutline(const PolygonOutline& other)
: PolygonOutline(other.info_, other.index_, other.polygon_, other.bresenham_, other.side_index_)
{
}
PolygonOutline::PolygonOutline(const nav_grid::NavGridInfo* info, const nav_grid::Index& index,
nav_2d_msgs::Polygon2D polygon, bool bresenham, unsigned int side_index)
: BaseIterator(info, index), polygon_(polygon), start_index_(index), bresenham_(bresenham), side_index_(side_index)
{
loadSide();
}
PolygonOutline& PolygonOutline::operator=(const PolygonOutline& other)
{
info_ = other.info_;
index_ = other.index_;
polygon_ = other.polygon_;
bresenham_ = other.bresenham_;
side_index_ = other.side_index_;
loadSide();
return *this;
}
void PolygonOutline::loadSide()
{
while (side_index_ < polygon_.points.size())
{
// The next index loops around to the first index
unsigned int next_index = side_index_ + 1;
if (next_index == polygon_.points.size())
{
next_index = 0;
}
internal_iterator_.reset(new Line(info_,
polygon_.points[side_index_].x, polygon_.points[side_index_].y,
polygon_.points[next_index].x, polygon_.points[next_index].y,
false, bresenham_));
if (*internal_iterator_ != internal_iterator_->end())
break;
++side_index_;
}
}
PolygonOutline PolygonOutline::begin() const
{
return PolygonOutline(info_, start_index_, polygon_, bresenham_, 0);
}
PolygonOutline PolygonOutline::end() const
{
// Since the polygon outline loops around to the original index when it is complete
// the end iterator is represented by having the current side to be the invalid
return PolygonOutline(info_, start_index_, polygon_, bresenham_, polygon_.points.size());
}
void PolygonOutline::increment()
{
++(*internal_iterator_);
if (*internal_iterator_ == internal_iterator_->end())
{
++side_index_;
if (side_index_ == polygon_.points.size())
{
index_ = start_index_;
return;
}
loadSide();
}
index_ = **internal_iterator_;
}
bool PolygonOutline::fieldsEqual(const PolygonOutline& other)
{
return side_index_ == other.side_index_ && nav_2d_utils::equals(polygon_, other.polygon_) &&
bresenham_ == other.bresenham_;
}
} // namespace nav_grid_iterators

146
navigations/nav_grid_iterators/src/ray_trace.cpp Executable file
View File

@@ -0,0 +1,146 @@
/*
* 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 <nav_grid_iterators/line/ray_trace.h>
#include <cmath>
#include <limits>
namespace nav_grid_iterators
{
RayTrace::RayTrace(double x0, double y0, double x1, double y1, bool include_last_index)
: AbstractLineIterator(), x0_(x0), y0_(y0), x1_(x1), y1_(y1), include_last_index_(include_last_index)
{
dx_ = std::abs(x1 - x0);
dy_ = std::abs(y1 - y0);
index_.x = static_cast<int>(floor(x0));
index_.y = static_cast<int>(floor(y0));
if (dx_ == 0)
{
loop_inc_x_ = 0;
error_ = std::numeric_limits<double>::max();
}
else if (x1 > x0)
{
loop_inc_x_ = 1;
error_ = (floor(x0) + 1 - x0) * dy_;
}
else
{
loop_inc_x_ = -1;
error_ = (x0 - floor(x0)) * dy_;
}
if (dy_ == 0)
{
loop_inc_y_ = 0;
error_ -= std::numeric_limits<double>::max();
}
else if (y1 > y0)
{
loop_inc_y_ = 1;
error_ -= (floor(y0) + 1 - y0) * dx_;
}
else
{
loop_inc_y_ = -1;
error_ -= (y0 - floor(y0)) * dx_;
}
/* Since we check if the index is equal to the second point in the line,
* we have to check for this one edge case to ensure we don't get into a rounding
* problem, resulting in an off-by-one error.
*/
if (!include_last_index && x1 < x0 && y1 - floor(y1) == 0.0)
{
error_ += 1e-10;
}
initial_error_ = error_;
// Special use case when start and end point are the same AND we want to include that point
if (include_last_index && loop_inc_x_ == 0 && loop_inc_y_ == 0)
{
loop_inc_x_ = 1;
}
}
RayTrace::RayTrace(const nav_grid::SignedIndex& index,
double x0, double y0, double x1, double y1, bool include_last_index,
double dx, double dy, double initial_error, int loop_inc_x, int loop_inc_y)
: AbstractLineIterator(index), x0_(x0), y0_(y0), x1_(x1), y1_(y1), include_last_index_(include_last_index),
dx_(dx), dy_(dy), error_(initial_error), initial_error_(initial_error),
loop_inc_x_(loop_inc_x), loop_inc_y_(loop_inc_y)
{
}
RayTrace RayTrace::begin() const
{
return RayTrace(nav_grid::SignedIndex(x0_, y0_), x0_, y0_, x1_, y1_, include_last_index_,
dx_, dy_, initial_error_, loop_inc_x_, loop_inc_y_);
}
RayTrace RayTrace::end() const
{
int x_diff = abs(static_cast<int>(x0_) - static_cast<int>(x1_));
int y_diff = abs(static_cast<int>(y0_) - static_cast<int>(y1_));
double final_error = initial_error_ - dx_ * y_diff + dy_ * x_diff;
RayTrace end(nav_grid::SignedIndex(x1_, y1_), x0_, y0_, x1_, y1_, include_last_index_,
dx_, dy_, final_error, loop_inc_x_, loop_inc_y_);
// If we want the last_index, return an iterator that is whatever is one-past the end coordinates
if (include_last_index_)
end.increment();
return end;
}
void RayTrace::increment()
{
if (error_ > 0.0)
{
index_.y += loop_inc_y_;
error_ -= dx_;
}
else
{
index_.x += loop_inc_x_;
error_ += dy_;
}
}
nav_grid::SignedIndex RayTrace::getFinalIndex() const
{
return end().index_;
}
} // namespace nav_grid_iterators

154
navigations/nav_grid_iterators/src/spiral.cpp Executable file
View File

@@ -0,0 +1,154 @@
/*
* 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 <nav_grid_iterators/spiral.h>
#include <nav_grid/coordinate_conversion.h>
namespace nav_grid_iterators
{
Spiral::Spiral(const nav_grid::NavGridInfo* info, double center_x, double center_y, double radius)
: BaseIterator(info), center_x_(center_x), center_y_(center_y), distance_(0), start_index_(0, 0)
{
radius_sq_ = radius * radius;
max_distance_ = ceil(radius / info->resolution);
loadRing();
index_ = **internal_iterator_;
start_index_ = index_;
}
Spiral::Spiral(const Spiral& other)
: Spiral(other.info_, other.index_, other.center_x_, other.center_y_, other.radius_sq_,
other.distance_, other.max_distance_, other.start_index_)
{
}
Spiral::Spiral(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, double center_x, double center_y,
double radius_sq, unsigned int distance, unsigned int max_distance,
const nav_grid::Index& start_index)
: BaseIterator(info, index), center_x_(center_x), center_y_(center_y), radius_sq_(radius_sq),
distance_(distance), max_distance_(max_distance), start_index_(start_index)
{
loadRing();
if (distance_ < max_distance_)
{
index_ = **internal_iterator_;
start_index_ = index_;
}
}
Spiral& Spiral::operator=(const Spiral& other)
{
info_ = other.info_;
index_ = other.index_;
center_x_ = other.center_x_;
center_y_ = other.center_y_;
radius_sq_ = other.radius_sq_;
distance_ = other.distance_;
max_distance_ = other.max_distance_;
start_index_ = other.start_index_;
loadRing();
if (distance_ < max_distance_)
{
index_ = **internal_iterator_;
start_index_ = index_;
}
return *this;
}
Spiral Spiral::begin() const
{
return Spiral(info_, start_index_, center_x_, center_y_, radius_sq_, 0, max_distance_, start_index_);
}
Spiral Spiral::end() const
{
return Spiral(info_, start_index_, center_x_, center_y_, radius_sq_, max_distance_ + 1, max_distance_,
start_index_);
}
void Spiral::increment()
{
while (distance_ <= max_distance_)
{
++(*internal_iterator_);
if (*internal_iterator_ == internal_iterator_->end())
{
++distance_;
if (distance_ > max_distance_)
{
index_ = start_index_;
return;
}
loadRing();
}
index_ = **internal_iterator_;
if (isInside(index_.x, index_.y))
{
break;
}
}
if (distance_ > max_distance_)
{
index_ = start_index_;
}
}
bool Spiral::fieldsEqual(const Spiral& other)
{
return center_x_ == other.center_x_ && center_y_ == other.center_y_ &&
radius_sq_ == other.radius_sq_ && distance_ == other.distance_;
}
void Spiral::loadRing()
{
while (distance_ <= max_distance_)
{
internal_iterator_.reset(new CircleOutline(info_, center_x_, center_y_, distance_));
if (*internal_iterator_ != internal_iterator_->end())
break;
++distance_;
}
}
bool Spiral::isInside(unsigned int x, unsigned int y) const
{
double wx, wy;
gridToWorld(*info_, x, y, wx, wy);
double dx = wx - center_x_;
double dy = wy - center_y_;
return (dx * dx + dy * dy) < radius_sq_;
}
} // namespace nav_grid_iterators

93
navigations/nav_grid_iterators/src/sub_grid.cpp Executable file
View File

@@ -0,0 +1,93 @@
/*
* 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 <nav_grid_iterators/sub_grid.h>
namespace nav_grid_iterators
{
SubGrid::SubGrid(const nav_grid::NavGridInfo* info, const nav_grid::Index& index,
unsigned int min_x, unsigned int min_y,
unsigned int width, unsigned int height)
: BaseIterator(info, index), min_x_(min_x), min_y_(min_y), width_(width), height_(height)
{
// If the start coordinate is entirely off the grid or the size is 0
// we invalidate the entire iterator and give up immediately
if (min_x_ >= info->width || min_y_ >= info->height || width_ == 0 || height_ == 0)
{
index_ = nav_grid::Index(0, 0);
width_ = 0;
height_ = 0;
min_x_ = 0;
min_y_ = 0;
return;
}
// If the end coordinate is off the grid, we shorten the dimensions to
// cover the on-grid potion
if (min_x_ + width_ > info->width)
{
width_ = info->width - min_x_;
}
if (min_y_ + height_ > info->height)
{
height_ = info->height - min_y_;
}
}
SubGrid SubGrid::begin() const
{
return SubGrid(info_, min_x_, min_y_, width_, height_);
}
SubGrid SubGrid::end() const
{
return SubGrid(info_, nav_grid::Index(min_x_, min_y_ + height_), min_x_, min_y_, width_, height_);
}
void SubGrid::increment()
{
++index_.x;
if (index_.x >= min_x_ + width_)
{
index_.x = min_x_;
++index_.y;
}
}
bool SubGrid::fieldsEqual(const SubGrid& other)
{
return min_x_ == other.min_x_ && min_y_ == other.min_y_ && width_ == other.width_ && height_ == other.height_;
}
} // namespace nav_grid_iterators

60
navigations/nav_grid_iterators/src/whole_grid.cpp Executable file
View File

@@ -0,0 +1,60 @@
/*
* 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 <nav_grid_iterators/whole_grid.h>
namespace nav_grid_iterators
{
WholeGrid WholeGrid::begin() const
{
return WholeGrid(info_);
}
WholeGrid WholeGrid::end() const
{
return WholeGrid(info_, nav_grid::Index(0, info_->height));
}
void WholeGrid::increment()
{
++index_.x;
if (index_.x >= info_->width)
{
index_.x = 0;
++index_.y;
}
}
} // namespace nav_grid_iterators

138
navigations/nav_grid_iterators/test/line_tests.cpp Executable file
View File

@@ -0,0 +1,138 @@
/*
* 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_iterators/line/bresenham.h>
#include <nav_grid_iterators/line/ray_trace.h>
template<class iterator_type>
int countIterations(iterator_type it, int max_iterations = 1000)
{
int count = 0;
iterator_type end = it.end();
for ( ; it != end; ++it)
{
++count;
if (count >= max_iterations) break;
}
return count;
}
TEST(Lines, line)
{
for (int i = 0; i <= 1; i++)
{
bool include_last_point = i == 0;
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(0, 0, 0, 0, include_last_point)), 1 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(0, 0, 3, 0, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(0, 0, -3, 0, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(3, 0, 0, 0, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(-3, 0, 0, 0, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(0, 0, 0, 3, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(0, 0, 0, -3, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(0, 3, 0, 0, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(0, -3, 0, 0, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(0, 0, 1, 9, include_last_point)), 10 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(0, 0, -1, -9, include_last_point)), 10 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(1, 9, 0, 0, include_last_point)), 10 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(-1, -9, 0, 0, include_last_point)), 10 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(9, 1, 0, 0, include_last_point)), 10 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(0, 0, 9, 1, include_last_point)), 10 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(-5, -5, 5, 5, include_last_point)), 11 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(-5, 5, 15, 5, include_last_point)), 21 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(15, 5, -5, 5, include_last_point)), 21 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::Bresenham(15, 5, 15, 7, include_last_point)), 3 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(0, 0, 0, 0, include_last_point)), 1 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(0, 0, 3, 0, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(0, 0, -3, 0, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(3, 0, 0, 0, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(-3, 0, 0, 0, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(0, 0, 0, 3, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(0, 0, 0, -3, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(0, 3, 0, 0, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(0, -3, 0, 0, include_last_point)), 4 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(0, 0, 1, 9, include_last_point)), 11 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(0, 0, -1, -9, include_last_point)), 11 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(1, 9, 0, 0, include_last_point)), 11 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(-1, -9, 0, 0, include_last_point)), 11 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(9, 1, 0, 0, include_last_point)), 11 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(0, 0, 9, 1, include_last_point)), 11 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(-5, -5, 5, 5, include_last_point)), 21 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(-5, 5, 15, 5, include_last_point)), 21 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(15, 5, -5, 5, include_last_point)), 21 - i);
EXPECT_EQ(countIterations(nav_grid_iterators::RayTrace(15, 5, 15, 7, include_last_point)), 3 - i);
}
}
TEST(Lines, border_conditions)
{
int N = 100;
int N2 = N / 2;
for (int include_last_point = 0; include_last_point <= 1; ++include_last_point)
{
for (int i=0; i < N; ++i)
{
EXPECT_LT(countIterations(nav_grid_iterators::Bresenham(0, 0, N2, i - N2, include_last_point)), N*3);
EXPECT_LT(countIterations(nav_grid_iterators::Bresenham(0, 0, -N2, i - N2, include_last_point)), N*3);
EXPECT_LT(countIterations(nav_grid_iterators::Bresenham(0, 0, i - N2, N2, include_last_point)), N*3);
EXPECT_LT(countIterations(nav_grid_iterators::Bresenham(0, 0, i - N2, -N2, include_last_point)), N*3);
EXPECT_LT(countIterations(nav_grid_iterators::RayTrace(0, 0, N2, i - N2, include_last_point)), N*3);
EXPECT_LT(countIterations(nav_grid_iterators::RayTrace(0, 0, -N2, i - N2, include_last_point)), N*3);
EXPECT_LT(countIterations(nav_grid_iterators::RayTrace(0, 0, i - N2, N2, include_last_point)), N*3);
EXPECT_LT(countIterations(nav_grid_iterators::RayTrace(0, 0, i - N2, -N2, include_last_point)), N*3);
}
}
}
TEST(Lines, equality)
{
nav_grid_iterators::Bresenham it1(0, 0, 5, 5);
nav_grid_iterators::Bresenham it2(0, 0, 1, 1);
ASSERT_FALSE(it1 == it2);
}
TEST(Lines, test_copy)
{
// This test will fail to compile if you cannot use the copy operator
nav_grid_iterators::Bresenham bres(0, 0, 0, 0);
bres = bres.begin();
nav_grid_iterators::RayTrace rt(0, 0, 0, 0);
rt = rt.begin();
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

466
navigations/nav_grid_iterators/test/utest.cpp Executable file
View File

@@ -0,0 +1,466 @@
/*
* 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_iterators/iterators.h>
#include <algorithm>
#include <vector>
using nav_grid::Index;
template<class iterator_type>
int countIterations(iterator_type it, int max_iterations = 1000)
{
int count = 0;
iterator_type end = it.end();
for ( ; it != end; ++it)
{
++count;
if (count >= max_iterations) break;
}
return count;
}
TEST(WholeGrid, whole_grid)
{
nav_grid::NavGridInfo info;
info.width = 5;
info.height = 8;
int count = 0;
for (nav_grid_iterators::WholeGrid it(info); it != it.end(); ++it)
{
Index i = *it;
ASSERT_EQ(i.x, count % info.width);
ASSERT_EQ(i.y, count / info.width);
++count;
}
ASSERT_EQ(count, 40);
}
TEST(WholeGrid, whole_grid_range)
{
nav_grid::NavGridInfo info;
info.width = 3;
info.height = 6;
int count = 0;
for (Index i : nav_grid_iterators::WholeGrid(info))
{
ASSERT_EQ(i.x, count % info.width);
ASSERT_EQ(i.y, count / info.width);
++count;
}
ASSERT_EQ(count, 18);
}
TEST(WholeGrid, std_stuff)
{
nav_grid::NavGridInfo info;
info.width = 8;
info.height = 2;
nav_grid_iterators::WholeGrid wg(info);
std::vector<Index> vec;
std::copy(wg.begin(), wg.end(), std::back_inserter(vec));
for (int count = 0; count < 16; ++count)
{
Index& i = vec[count];
ASSERT_EQ(i.x, count % info.width);
ASSERT_EQ(i.y, count / info.width);
}
}
TEST(SubGrid, sub_grid)
{
nav_grid::NavGridInfo info;
info.width = 5;
info.height = 8;
int count = 0;
for (Index i : nav_grid_iterators::SubGrid(&info, 1, 2, 2, 3))
{
ASSERT_EQ(i.x, static_cast<unsigned int>(1 + count % 2));
ASSERT_EQ(i.y, static_cast<unsigned int>(2 + count / 2));
++count;
}
ASSERT_EQ(count, 6);
ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, 1, 3, 4, 1)), 4);
ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, 1, 3, 2, 2)), 4);
nav_core2::UIntBounds bounds(1, 3, 4, 3);
ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, bounds)), 4);
// Empty Bounds
bounds.reset();
ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, bounds)), 0);
// Partially Overlapping Bounds
bounds.touch(3, 2);
bounds.touch(6, 3);
ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, bounds)), 4);
bounds.reset();
bounds.touch(1, 6);
bounds.touch(3, 9);
ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, bounds)), 6);
// Different empty bounds
nav_core2::UIntBounds empty(1, 0, 0, 0);
ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, empty)), 0);
}
TEST(SubGrid, equality)
{
nav_grid::NavGridInfo info;
info.width = 5;
info.height = 8;
nav_grid_iterators::SubGrid it1(&info, 1, 2, 2, 3);
nav_grid_iterators::SubGrid it2(&info, 1, 2, 1, 1);
ASSERT_FALSE(it1 == it2);
}
TEST(CircleFill, circle)
{
nav_grid::NavGridInfo info;
info.width = 8;
info.height = 8;
info.resolution = 1.0;
ASSERT_EQ(countIterations(nav_grid_iterators::CircleFill(&info, 4.0, 4.0, 3.0)), 32);
ASSERT_EQ(countIterations(nav_grid_iterators::CircleFill(&info, 4.3, 4.0, 3.0)), 28);
ASSERT_EQ(countIterations(nav_grid_iterators::CircleFill(&info, 4.0, 4.0, 8.0)), 64);
ASSERT_EQ(countIterations(nav_grid_iterators::CircleFill(&info, 14.0, 4.0, 1.0)), 0);
ASSERT_EQ(countIterations(nav_grid_iterators::CircleFill(&info, 0.0, 4.0, 4.0)), 26);
ASSERT_EQ(countIterations(nav_grid_iterators::CircleFill(&info, 4.0, 4.0, 3.0).begin()), 32);
}
TEST(CircleFill, equality)
{
nav_grid::NavGridInfo info;
info.width = 5;
info.height = 8;
nav_grid_iterators::CircleFill it1(&info, 4.0, 4.0, 8.0);
nav_grid_iterators::CircleFill it2(&info, 1.0, 1.0, 100.0);
ASSERT_FALSE(it1 == it2);
}
TEST(CircleOutline, circle_outline)
{
nav_grid::NavGridInfo info;
info.width = 8;
info.height = 8;
info.resolution = 1.0;
unsigned int size = 0;
EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 1);
EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 8);
EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 16);
EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 20);
EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 14);
EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 5);
EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 0);
EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 0);
}
TEST(CircleOutline, equality)
{
nav_grid::NavGridInfo info;
info.width = 5;
info.height = 8;
nav_grid_iterators::CircleOutline it1(&info, 3.0, 1.0, 1.0);
nav_grid_iterators::CircleOutline it2(&info, 1.0, 1.0, 3.0);
ASSERT_FALSE(it1 == it2);
}
TEST(Spiral, spiral)
{
nav_grid::NavGridInfo info;
info.width = 8;
info.height = 8;
info.resolution = 1.0;
info.origin_x = 0.0;
info.origin_y = 0.0;
ASSERT_EQ(countIterations(nav_grid_iterators::Spiral(&info, 4.0, 4.0, 3.0)), 32);
ASSERT_EQ(countIterations(nav_grid_iterators::Spiral(&info, 4.3, 4.0, 3.0)), 28);
ASSERT_EQ(countIterations(nav_grid_iterators::Spiral(&info, 4.0, 4.0, 8.0)), 64);
ASSERT_EQ(countIterations(nav_grid_iterators::Spiral(&info, 14.0, 4.0, 1.0)), 0);
ASSERT_EQ(countIterations(nav_grid_iterators::Spiral(&info, 0.0, 4.0, 4.0)), 26);
}
TEST(Spiral, equality)
{
nav_grid::NavGridInfo info;
info.width = 5;
info.height = 8;
nav_grid_iterators::Spiral it1(&info, 1.0, 1.0, 1.0);
nav_grid_iterators::Spiral it2(&info, 1.0, 1.0, 3.0);
ASSERT_FALSE(it1 == it2);
}
TEST(Line, signed_line)
{
nav_grid::NavGridInfo info;
info.width = 10;
info.height = 10;
info.resolution = 1.0;
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0, 0)), 1);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 3, 0)), 4);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, -3, 0)), 1);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 3, 0, 0, 0)), 4);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -3, 0, 0, 0)), 1);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -3, 0, 0, 0, false)), 0);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0, 3)), 4);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0, -3)), 1);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 3, 0, 0)), 4);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, -3, 0, 0)), 1);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 1, 9)), 10);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 1, 9, 0, 0)), 10);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 9, 1, 0, 0)), 10);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 9, 1)), 10);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -5, 5, 15, 5)), 10);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 15, 5, -5, 5)), 10);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 15, 5, 15, 7)), 0);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -1, -5, -1, -7)), 0);
}
TEST(Line, signed_line_diff_res)
{
// This is the same test as above with a reduced resolution. Most of the coordinates are divided by 10
// (with an additional 0.05 in some places to avoid floating point errors)
nav_grid::NavGridInfo info;
info.width = 10;
info.height = 10;
info.resolution = 0.1;
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0, 0)), 1);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0.35, 0)), 4);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, -0.35, 0)), 1);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0.35, 0, 0, 0)), 4);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -0.35, 0, 0, 0)), 1);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -0.35, 0, 0, 0, false)), 0);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0, 0.35)), 4);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0, -0.35)), 1);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0.35, 0, 0)), 4);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, -0.35, 0, 0)), 1);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0.15, 0.95)), 10);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0.15, 0.95, 0, 0)), 10);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0.95, 0.15, 0, 0)), 10);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0.95, 0.15)), 10);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -0.5, 0.5, 1.5, 0.5)), 10);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 1.5, 0.5, -0.55, 0.55)), 10);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 1.5, 0.5, 1.5, 0.75)), 0);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -0.1, -0.5, -0.1, -0.7)), 0);
}
TEST(Line, random_test_case)
{
nav_grid::NavGridInfo info;
info.width = 795;
info.height = 925;
info.resolution = 0.05;
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 6.2402, 30.651832, 2.805347, 22.8941, false, false)), 224);
EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 6.2402, 30.651832, 2.805347, 22.8941, true, false)), 225);
}
TEST(Line, equality)
{
nav_grid::NavGridInfo info;
info.width = 5;
info.height = 8;
nav_grid_iterators::Line it1(&info, 0, 0, 5, 5);
nav_grid_iterators::Line it2(&info, 0, 0, 1, 1);
ASSERT_FALSE(it1 == it2);
}
nav_2d_msgs::Point2D make_point(double x, double y)
{
nav_2d_msgs::Point2D pt;
pt.x = x;
pt.y = y;
return pt;
}
TEST(Polygon, polygon)
{
nav_grid::NavGridInfo info;
// First check to make sure it works when the polygon is completely on the grid
info.width = 10;
info.height = 10;
info.resolution = 1.0;
nav_2d_msgs::Polygon2D simple_square;
simple_square.points.push_back(make_point(1.4, 1.4));
simple_square.points.push_back(make_point(1.4, 3.6));
simple_square.points.push_back(make_point(3.6, 3.6));
simple_square.points.push_back(make_point(3.6, 1.4));
EXPECT_EQ(countIterations(nav_grid_iterators::PolygonOutline(&info, simple_square)), 8);
EXPECT_EQ(countIterations(nav_grid_iterators::PolygonFill(&info, simple_square)), 9);
// Then do it again when it is only partially on the grid
info.height = 3;
EXPECT_EQ(countIterations(nav_grid_iterators::PolygonOutline(&info, simple_square)), 5);
EXPECT_EQ(countIterations(nav_grid_iterators::PolygonFill(&info, simple_square)), 6);
// Then check when it is completely off the grid
info.resolution = 0.1;
EXPECT_EQ(countIterations(nav_grid_iterators::PolygonOutline(&info, simple_square)), 0);
EXPECT_EQ(countIterations(nav_grid_iterators::PolygonFill(&info, simple_square)), 0);
}
TEST(Polygon, empty_polygon)
{
nav_grid::NavGridInfo info;
info.width = 10;
info.height = 10;
info.resolution = 1.0;
nav_2d_msgs::Polygon2D empty_polygon;
EXPECT_EQ(countIterations(nav_grid_iterators::PolygonOutline(&info, empty_polygon)), 0);
EXPECT_EQ(countIterations(nav_grid_iterators::PolygonFill(&info, empty_polygon)), 0);
}
TEST(Polygon, equality)
{
nav_grid::NavGridInfo info;
info.width = 10;
info.height = 10;
info.resolution = 1.0;
nav_2d_msgs::Polygon2D simple_square;
simple_square.points.push_back(make_point(1.4, 1.4));
simple_square.points.push_back(make_point(1.4, 3.6));
simple_square.points.push_back(make_point(3.6, 3.6));
simple_square.points.push_back(make_point(3.6, 1.4));
nav_2d_msgs::Polygon2D triangle;
triangle.points.push_back(make_point(1.4, 1.4));
triangle.points.push_back(make_point(1.4, 3.6));
triangle.points.push_back(make_point(3.6, 3.6));
nav_grid_iterators::PolygonOutline it1(&info, simple_square);
nav_grid_iterators::PolygonOutline it2(&info, triangle);
ASSERT_FALSE(it1 == it2);
nav_grid_iterators::PolygonFill it3(&info, simple_square);
nav_grid_iterators::PolygonFill it4(&info, triangle);
ASSERT_FALSE(it3 == it4);
}
TEST(Iterators, test_copy)
{
// This test will fail to compile if you cannot use the copy operator
nav_grid::NavGridInfo info;
info.width = 10;
info.height = 10;
info.resolution = 1.0;
nav_2d_msgs::Polygon2D simple_square;
simple_square.points.push_back(make_point(1.4, 1.4));
simple_square.points.push_back(make_point(1.4, 3.6));
simple_square.points.push_back(make_point(3.6, 3.6));
simple_square.points.push_back(make_point(3.6, 1.4));
nav_grid_iterators::WholeGrid whole_grid(info);
whole_grid = whole_grid.begin();
nav_grid_iterators::SubGrid sub_grid(&info, 1, 2, 2, 3);
sub_grid = sub_grid.begin();
nav_grid_iterators::CircleFill cf(&info, 4.0, 4.0, 3.0);
cf = cf.begin();
nav_grid_iterators::CircleOutline co(&info, 4.0, 4.0, 3.0);
co = co.begin();
nav_grid_iterators::Spiral spiral(&info, 4.0, 4.0, 3.0);
spiral = spiral.begin();
nav_grid_iterators::Line line(&info, 0, 0, 0, 0);
line = line.begin();
nav_grid_iterators::PolygonOutline po(&info, simple_square);
po = po.begin();
nav_grid_iterators::PolygonFill pf(&info, simple_square);
pf = pf.begin();
}
TEST(Iterators, test_assignment)
{
nav_grid::NavGridInfo info;
info.width = 10;
info.height = 10;
info.resolution = 1.0;
nav_grid_iterators::CircleFill iter1(&info, 3.0, 3.0, 1.0);
// Sequence should be (2, 2) (3, 2) (2, 3) (3, 3)
EXPECT_EQ((*iter1).x, 2U);
EXPECT_EQ((*iter1).y, 2U);
nav_grid_iterators::CircleFill iter2 = iter1;
// Effective Copy
EXPECT_EQ((*iter1).x, 2U);
EXPECT_EQ((*iter1).y, 2U);
EXPECT_EQ((*iter2).x, 2U);
EXPECT_EQ((*iter2).y, 2U);
// Increment only iter2
++iter2;
EXPECT_EQ((*iter1).x, 2U);
EXPECT_EQ((*iter1).y, 2U);
EXPECT_EQ((*iter2).x, 3U);
EXPECT_EQ((*iter2).y, 2U);
// Increment first to match
++iter1;
EXPECT_EQ((*iter1).x, 3U);
EXPECT_EQ((*iter1).y, 2U);
EXPECT_EQ((*iter2).x, 3U);
EXPECT_EQ((*iter2).y, 2U);
// Increment only iter2
++iter2;
EXPECT_EQ((*iter1).x, 3U);
EXPECT_EQ((*iter1).y, 2U);
EXPECT_EQ((*iter2).x, 2U);
EXPECT_EQ((*iter2).y, 3U);
// Check copy when not at the start
nav_grid_iterators::CircleFill iter3 = iter1;
EXPECT_EQ((*iter1).x, 3U);
EXPECT_EQ((*iter1).y, 2U);
EXPECT_EQ((*iter2).x, 2U);
EXPECT_EQ((*iter2).y, 3U);
EXPECT_EQ((*iter3).x, 3U);
EXPECT_EQ((*iter3).y, 2U);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}