forked from Greenzie/boustrophedon_planner
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
update boustrophedon planner from private repo
- Loading branch information
Showing
22 changed files
with
1,717 additions
and
606 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1 @@ | ||
| .idea/ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,2 +1,2 @@ | ||
| Header header | ||
| boustrophedon_msgs/StripingPoint[] points | ||
| StripingPoint[] points |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,6 +1,7 @@ | ||
| geometry_msgs/Point point | ||
| uint8 type | ||
|
|
||
| uint8 OUTLINE=0 | ||
| uint8 STRIPE_START=1 | ||
| uint8 STRIPE_END=2 | ||
| uint8 OUTLINE = 0 | ||
| uint8 STRIPE_START = 1 | ||
| uint8 STRIPE_END = 2 | ||
| uint8 STRIPE_INTERMEDIATE = 3 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
64 changes: 10 additions & 54 deletions
64
boustrophedon_server/include/boustrophedon_server/cellular_decomposition/cell.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,74 +1,30 @@ | ||
| #ifndef SRC_CELL_H | ||
| #define SRC_CELL_H | ||
|
|
||
| #include <boost/functional/hash.hpp> | ||
| #include <vector> | ||
| #include "../cgal_types.h" | ||
| #include "boustrophedon_server/cgal_utils.h" | ||
| #include "boustrophedon_server/cgal_types.h" | ||
| #include <CGAL/squared_distance_2.h> | ||
|
|
||
| namespace bcd | ||
| { | ||
| using Circulator = Polygon::Vertex_const_circulator; | ||
| enum class Event | ||
| { | ||
| Start, | ||
| Ceil, | ||
| Floor, | ||
| End, | ||
| Open, | ||
| Close, | ||
| }; | ||
|
|
||
| struct Vertex | ||
| { | ||
| Vertex() = default; | ||
| Vertex(Event type, Circulator point); | ||
| bool isAdjacentTo(const Vertex& other) const; | ||
| Circulator nextPoint() const; | ||
| Circulator previousPoint() const; | ||
|
|
||
| Event type; | ||
| Circulator point; | ||
| }; | ||
|
|
||
| struct Cell | ||
| { | ||
| Cell() = default; | ||
| Cell(Vertex start); | ||
| bool connectsToFloor(const Vertex& other) const; | ||
| bool connectsToCeil(const Vertex& other) const; | ||
| bool enclosesVector(const Vertex& other) const; | ||
|
|
||
| const Vertex& currentCeil() const; | ||
| const Vertex& currentFloor() const; | ||
| explicit Cell(const Polygon& polygon); | ||
|
|
||
| const Point nextCeilPoint() const; | ||
| const Point nextFloorPoint() const; | ||
| void insertPointOnEdge(const Point& point); | ||
| bool isPointOnEdge(const Point& point); | ||
| bool isAdjacentTo(const Cell& other); | ||
| bool canMergeWith(const std::vector<Cell>& cells); | ||
|
|
||
| std::vector<Point> toPoints() const; | ||
| Polygon toPolygon() const; | ||
|
|
||
| Vertex start; | ||
| std::vector<Vertex> ceiling_points; | ||
| std::vector<Vertex> floor_points; | ||
| Vertex end; | ||
|
|
||
| std::vector<Point> points; | ||
| std::vector<Cell*> neighbors; | ||
| bool visited = false; | ||
| }; | ||
| } // namespace bcd | ||
|
|
||
| template <> | ||
| struct std::hash<const bcd::Cell*> | ||
| { | ||
| std::size_t operator()(const bcd::Cell* const& cell) const | ||
| { | ||
| std::size_t seed = 17; | ||
| boost::hash_combine(seed, std::make_pair(cell->start.point->x(), cell->start.point->y())); | ||
| boost::hash_combine(seed, std::make_pair(cell->end.point->x(), cell->end.point->y())); | ||
| return seed; | ||
| } | ||
| }; | ||
|
|
||
| std::ostream& operator<<(std::ostream& out, bcd::Event event); | ||
| std::ostream& operator<<(std::ostream& out, const bcd::Vertex& vertex); | ||
| std::ostream& operator<<(std::ostream& out, const bcd::Cell& cell); | ||
| #endif // SRC_CELL_H |
145 changes: 105 additions & 40 deletions
145
...trophedon_server/include/boustrophedon_server/cellular_decomposition/polygon_decomposer.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,79 +1,144 @@ | ||
| #ifndef SRC_POLYGON_DECOMPOSER_H | ||
| #define SRC_POLYGON_DECOMPOSER_H | ||
|
|
||
| #include "../cgal_types.h" | ||
| #include "cell.h" | ||
| #include "boustrophedon_server/cgal_types.h" | ||
| #include "boustrophedon_server/cgal_utils.h" | ||
| #include "boustrophedon_server/cellular_decomposition/cell.h" | ||
|
|
||
| class PolygonDecomposer | ||
| { | ||
| public: | ||
| /** | ||
| * Decompose the polygon into convex cells, and place them in cells_. | ||
| * @param polygon | ||
| */ | ||
| void decompose(Polygon& polygon); | ||
| std::vector<Polygon> getSubPolygons(const Point& position) const; | ||
|
|
||
| /** | ||
| * Use the given position to construct a sequence of cells to travel to, combining cells where possible | ||
| * @param polygon | ||
| * @returns a vector of Polygons that can each be striped by the striping_planner in order | ||
| */ | ||
| std::vector<Polygon> getSubPolygons(const Point& position); | ||
|
|
||
| private: | ||
| std::vector<std::unique_ptr<bcd::Cell>> cells_; | ||
| std::vector<bcd::Vertex> vertices_; | ||
| std::vector<bcd::Cell> cells_; | ||
|
|
||
| /** | ||
| * Creates vertices and assigns each vertex an event from the Polygon. | ||
| * During the first pass, it finds Open and Close events, and then inserts | ||
| * those intersections of those slices into the polygon. | ||
| * During the second pass, it creates Vertex objects and places them into the | ||
| * vertices_ vector | ||
| * Finds all vertices in the given polygon with an interior angle greater than 180 degrees. | ||
| * @param polygon | ||
| * @returns a list of vertices on the polygon that have greater than 180 degree interior angles. | ||
| */ | ||
| void createVertices(Polygon& polygon); | ||
| static std::vector<Point> getCriticalInflectionPoints(Polygon& polygon); | ||
|
|
||
| /** | ||
| * Finds Open and close events. For each event, it finds the intersection vertices of the | ||
| * slice and inserts them into the polygon. | ||
| * Calculates the interior angle of the given vertex, and returns that angle | ||
| * @param vertex | ||
| * @returns the interior angle in degrees , in the range 0.0 - 360.0 | ||
| */ | ||
| static double getInteriorAngleOfVertex(const Polygon::Vertex_const_circulator& vertex); | ||
|
|
||
| /** | ||
| * Sorts the given list of vertices by x coordinate. If equal, lower y coordinate is placed first. | ||
| * @param vertices | ||
| */ | ||
| static void sortVertices(std::vector<Point>& vertices); | ||
|
|
||
| /** | ||
| * Uses the given list of critical vertices to split the polygon boundary into multiple convex cells. | ||
| * These cells are used by getSubPolygons() to create polygons that can be striped without doubling back | ||
| * @param critical_vertices | ||
| * @param polygon | ||
| */ | ||
| void insertOpenCloseVertices(Polygon& polygon); | ||
| void createCells(const std::vector<Point>& critical_vertices, const Polygon& polygon); | ||
|
|
||
| /** | ||
| * Inserts all intersections of the slice with the polygon into the polygon. | ||
| * Uses the given sorted list of points to find the first unique point above (higher y) the critical_point. | ||
| * If found, places the above point in the above parameter. | ||
| * @param above | ||
| * @param critical_point | ||
| * @param points | ||
| * @param polygon | ||
| * @param point | ||
| * @returns true if above point is found, false if not | ||
| */ | ||
| static void sliceAndInsertIntoPolygon(Polygon& polygon, const Point& point); | ||
| static bool findAbovePoint(Point& above, const Point& critical_point, std::vector<Point>& points, | ||
| const Polygon& polygon); | ||
|
|
||
| /** | ||
| * | ||
| * Uses the given sorted list of points to find the first unique point below (lower y) the critical_point. | ||
| * If found, places the below point in the below parameter. | ||
| * @param below | ||
| * @param critical_point | ||
| * @param points | ||
| * @param polygon | ||
| * @returns true if below point is found, false if not | ||
| */ | ||
| void createCells(); | ||
| static bool findBelowPoint(Point& below, const Point& critical_point, const std::vector<Point>& points, | ||
| const Polygon& polygon); | ||
|
|
||
| bcd::Cell* getClosestCell(const Point& position) const; | ||
| /** | ||
| * Uses the given upper and lower intersection points to split an existing cell into two cells | ||
| * Uses and modifies the cells_ vector. Both upper and lower must intersection exactly one existing cell | ||
| * @param upper | ||
| * @param lower | ||
| */ | ||
| void sliceNewCell(const Point& upper, const Point& lower); | ||
|
|
||
| std::vector<const bcd::Cell*> visitCells(const bcd::Cell* starting_cell) const; | ||
| /** | ||
| * Finds the first (there should only be one) cell in cells_ that both upper and lower exist on. | ||
| * Used by sliceNewCell() | ||
| * @param upper | ||
| * @param lower | ||
| * @returns an iterator of cells_ that points to the cell to be sliced | ||
| */ | ||
| std::vector<bcd::Cell>::iterator findWorkingCell(const Point& upper, const Point& lower); | ||
|
|
||
| std::vector<Polygon> toPolygons(std::vector<const bcd::Cell*> cells) const; | ||
| /** | ||
| * Finds and returns a reference to the closest cell in cells_ from position | ||
| * @param position | ||
| * @returns a const_iterator pointing to the closest cell in cells_ | ||
| */ | ||
| std::vector<bcd::Cell>::iterator getClosestCell(const Point& position); | ||
|
|
||
| /** | ||
| * Dispatcher for the individual event types. | ||
| * @param current_vertex | ||
| * @param current_cells | ||
| * @return | ||
| * Traverses along all the cells in cells_, starting from starting_cell, and finds cells that can be merged together | ||
| * Returns a list of sub-polygons of the original boundary polygon that can be striped | ||
| * @param starting_cell | ||
| * @returns a vector of cells representing sub-polygons to stripe, in order | ||
| */ | ||
| bool handleVertex(bcd::Vertex& current_vertex, std::vector<std::unique_ptr<bcd::Cell>>& current_cells); | ||
| std::vector<bcd::Cell> visitCells(std::vector<bcd::Cell>::iterator starting_cell) const; | ||
|
|
||
| bool handleFloor(bcd::Vertex& current_vertex, std::vector<std::unique_ptr<bcd::Cell>>& current_cells); | ||
| bool handleCeil(bcd::Vertex& current_vertex, std::vector<std::unique_ptr<bcd::Cell>>& current_cells); | ||
| bool handleStart(bcd::Vertex& current_vertex, std::vector<std::unique_ptr<bcd::Cell>>& current_cells); | ||
| bool handleEnd(bcd::Vertex& current_vertex, std::vector<std::unique_ptr<bcd::Cell>>& current_cells); | ||
| bool handleOpen(bcd::Vertex& current_vertex, std::vector<std::unique_ptr<bcd::Cell>>& current_cells); | ||
| bool handleClose(bcd::Vertex& current_vertex, std::vector<std::unique_ptr<bcd::Cell>>& current_cells); | ||
| /** | ||
| * Uses a breadth-first search to traverse the neighbors graph of the cells in cells_, using root as the first node | ||
| * Returns a list of cells that is the original cells_ list, but reordered based on the breadth-first search | ||
| * @param root | ||
| * @returns a vector of cells representing the order in which the cells should be traveled to | ||
| */ | ||
| std::vector<bcd::Cell> generateCellTree(std::vector<bcd::Cell>::iterator root) const; | ||
|
|
||
| /** | ||
| * Uses the given cells list to create a list of list of cells, where each list of cells can be merged together | ||
| * Used to determine cells that can be combined together, to save time striping | ||
| * @param cells | ||
| * @returns a vector of vectors of cells, where all cells in a vector of cells can be merged together safely | ||
| */ | ||
| std::vector<std::vector<bcd::Cell>> determineCellsToMerge(std::vector<bcd::Cell>& cells) const; | ||
|
|
||
| std::vector<std::unique_ptr<bcd::Cell>>::iterator | ||
| getTopCell(bcd::Vertex& current_vertex, std::vector<std::unique_ptr<bcd::Cell>>& current_cells) const; | ||
| std::vector<std::unique_ptr<bcd::Cell>>::iterator | ||
| getBottomCell(bcd::Vertex& current_vertex, std::vector<std::unique_ptr<bcd::Cell>>& current_cells) const; | ||
| /** | ||
| * Merges the two given cells together, and returns a new cell that is the two combined cells | ||
| * Two cells can be merged together if they share two adjacent vertices | ||
| * @param cell_1 | ||
| * @param cell_2 | ||
| * @returns the new merged cell | ||
| */ | ||
| bcd::Cell mergeCells(const bcd::Cell& cell_1, const bcd::Cell& cell_2) const; | ||
|
|
||
| /** | ||
| * Sorts the vertices first by x coordinate, then by type. | ||
| * Turns a vector of cells into a vector of polygons | ||
| * @param cells | ||
| * @returns a vector of polygons of the given cells | ||
| */ | ||
| void sortVertices(); | ||
| bcd::Event getEvent(const bcd::Circulator& vertex, bcd::Event last_event) const; | ||
| static std::vector<Polygon> toPolygons(const std::vector<bcd::Cell>& cells); | ||
| }; | ||
|
|
||
| #endif // SRC_POLYGON_DECOMPOSER_H |
Oops, something went wrong.