diff --git a/.clang-tidy b/.clang-tidy index 4f03f0e..66f5069 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -4,6 +4,7 @@ Checks: '*, -google-build-using-namespace, -clang-analyzer-alpha.clone.CloneChecker, -google-runtime-int, + -google-readability-todo, -cppcoreguidelines-pro-bounds-array-to-pointer-decay, -cppcoreguidelines-pro-bounds-constant-array-index, -clang-analyzer-alpha.deadcode.UnreachableCode, @@ -20,9 +21,12 @@ Checks: '*, -google-runtime-references, -llvm-include-order, -fuchsia-*, - -misc-unused-parameters' - -WarningsAsErrors: '*' + -misc-unused-parameters, + -hicpp-no-assembler, + -readability-magic-numbers, + -google-readability-namespace-comments, + -llvm-namespace-comment' +#WarningsAsErrors: '*' HeaderFilterRegex: '' AnalyzeTemporaryDtors: false CheckOptions: diff --git a/.gitignore b/.gitignore index e69de29..62c8935 100644 --- a/.gitignore +++ b/.gitignore @@ -0,0 +1 @@ +.idea/ \ No newline at end of file diff --git a/boustrophedon_msgs/msg/StripingPlan.msg b/boustrophedon_msgs/msg/StripingPlan.msg index afca5de..cf313ae 100644 --- a/boustrophedon_msgs/msg/StripingPlan.msg +++ b/boustrophedon_msgs/msg/StripingPlan.msg @@ -1,2 +1,2 @@ Header header -boustrophedon_msgs/StripingPoint[] points +StripingPoint[] points diff --git a/boustrophedon_msgs/msg/StripingPoint.msg b/boustrophedon_msgs/msg/StripingPoint.msg index 8dc90a5..d584ed8 100644 --- a/boustrophedon_msgs/msg/StripingPoint.msg +++ b/boustrophedon_msgs/msg/StripingPoint.msg @@ -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 diff --git a/boustrophedon_server/CMakeLists.txt b/boustrophedon_server/CMakeLists.txt index 6e6ca9b..7be4073 100644 --- a/boustrophedon_server/CMakeLists.txt +++ b/boustrophedon_server/CMakeLists.txt @@ -2,7 +2,6 @@ cmake_minimum_required(VERSION 2.8.3) project(boustrophedon_server) set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_CLANG_TIDY clang-tidy) find_package(catkin REQUIRED COMPONENTS roscpp diff --git a/boustrophedon_server/include/boustrophedon_server/boustrophedon_planner_server.h b/boustrophedon_server/include/boustrophedon_server/boustrophedon_planner_server.h index 9a073e5..859ee06 100644 --- a/boustrophedon_server/include/boustrophedon_server/boustrophedon_planner_server.h +++ b/boustrophedon_server/include/boustrophedon_server/boustrophedon_planner_server.h @@ -4,13 +4,15 @@ #include #include #include +#include #include #include +#include -#include "cgal_utils.h" -#include "striping_planner.h" -#include "outline_planner.h" +#include "boustrophedon_server/cgal_utils.h" +#include "boustrophedon_server/striping_planner.h" +#include "boustrophedon_server/outline_planner.h" #include "cellular_decomposition/polygon_decomposer.h" class BoustrophedonPlannerServer @@ -27,20 +29,41 @@ class BoustrophedonPlannerServer ros::NodeHandle private_node_handle_; Server action_server_; ros::ServiceServer conversion_server_; + ros::Publisher initial_polygon_publisher_; + ros::Publisher preprocessed_polygon_publisher_; + ros::Publisher path_points_publisher_; + ros::Publisher polygon_points_publisher_; StripingPlanner striping_planner_; OutlinePlanner outline_planner_; + bool repeat_boundary_{}; + bool outline_clockwise_{}; + bool skip_outlines_{}; + bool enable_orientation_{}; int outline_layer_count_{}; double stripe_separation_{}; + double intermediary_separation_{}; double stripe_angle_{}; + bool travel_along_boundary_{}; + bool allow_points_outside_boundary_{}; + bool enable_half_y_turns_{}; + int points_per_turn_{}; + double turn_start_offset_{}; tf::TransformListener transform_listener_{}; + bool publish_polygons_{}; + bool publish_path_points_{}; bool convertStripingPlanToPath(boustrophedon_msgs::ConvertPlanToPath::Request& request, boustrophedon_msgs::ConvertPlanToPath::Response& response); boustrophedon_msgs::PlanMowingPathResult toResult(std::vector&& path, const std::string& frame) const; Polygon fromBoundary(const geometry_msgs::PolygonStamped& boundary) const; Point fromPositionWithFrame(const geometry_msgs::PoseStamped& pose, const std::string& target_frame) const; + bool checkPolygonIsValid(const Polygon& poly) const; + double getStripeAngleFromOrientation(const geometry_msgs::PoseStamped& robot_position); + geometry_msgs::PolygonStamped convertCGALPolygonToMsg(const Polygon& poly) const; + void publishPathPoints(const std::vector& path) const; + void publishPolygonPoints(const Polygon& poly) const; }; #endif // SRC_BOUSTROPHEDON_PLANNER_SERVER_H diff --git a/boustrophedon_server/include/boustrophedon_server/boustrophedon_types.h b/boustrophedon_server/include/boustrophedon_server/boustrophedon_types.h index f5188f7..e761e03 100644 --- a/boustrophedon_server/include/boustrophedon_server/boustrophedon_types.h +++ b/boustrophedon_server/include/boustrophedon_server/boustrophedon_types.h @@ -8,7 +8,7 @@ enum class PointType Outline = 0, StripeStart = 1, StripeEnd = 2, - Travel = 3 + StripeIntermediate = 3 }; struct NavPoint diff --git a/boustrophedon_server/include/boustrophedon_server/cellular_decomposition/cell.h b/boustrophedon_server/include/boustrophedon_server/cellular_decomposition/cell.h index 444e79f..f9ecdf0 100644 --- a/boustrophedon_server/include/boustrophedon_server/cellular_decomposition/cell.h +++ b/boustrophedon_server/include/boustrophedon_server/cellular_decomposition/cell.h @@ -1,74 +1,30 @@ #ifndef SRC_CELL_H #define SRC_CELL_H -#include #include -#include "../cgal_types.h" +#include "boustrophedon_server/cgal_utils.h" +#include "boustrophedon_server/cgal_types.h" +#include 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& cells); - std::vector toPoints() const; Polygon toPolygon() const; - Vertex start; - std::vector ceiling_points; - std::vector floor_points; - Vertex end; - + std::vector points; std::vector neighbors; + bool visited = false; }; } // namespace bcd -template <> -struct std::hash -{ - 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 diff --git a/boustrophedon_server/include/boustrophedon_server/cellular_decomposition/polygon_decomposer.h b/boustrophedon_server/include/boustrophedon_server/cellular_decomposition/polygon_decomposer.h index 4a289f2..683335b 100644 --- a/boustrophedon_server/include/boustrophedon_server/cellular_decomposition/polygon_decomposer.h +++ b/boustrophedon_server/include/boustrophedon_server/cellular_decomposition/polygon_decomposer.h @@ -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 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 getSubPolygons(const Point& position); private: - std::vector> cells_; - std::vector vertices_; + std::vector 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 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& 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& 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& 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& 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 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::iterator findWorkingCell(const Point& upper, const Point& lower); - std::vector toPolygons(std::vector 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::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>& current_cells); + std::vector visitCells(std::vector::iterator starting_cell) const; - bool handleFloor(bcd::Vertex& current_vertex, std::vector>& current_cells); - bool handleCeil(bcd::Vertex& current_vertex, std::vector>& current_cells); - bool handleStart(bcd::Vertex& current_vertex, std::vector>& current_cells); - bool handleEnd(bcd::Vertex& current_vertex, std::vector>& current_cells); - bool handleOpen(bcd::Vertex& current_vertex, std::vector>& current_cells); - bool handleClose(bcd::Vertex& current_vertex, std::vector>& 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 generateCellTree(std::vector::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> determineCellsToMerge(std::vector& cells) const; - std::vector>::iterator - getTopCell(bcd::Vertex& current_vertex, std::vector>& current_cells) const; - std::vector>::iterator - getBottomCell(bcd::Vertex& current_vertex, std::vector>& 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 toPolygons(const std::vector& cells); }; #endif // SRC_POLYGON_DECOMPOSER_H diff --git a/boustrophedon_server/include/boustrophedon_server/cgal_types.h b/boustrophedon_server/include/boustrophedon_server/cgal_types.h index c77e416..473b722 100644 --- a/boustrophedon_server/include/boustrophedon_server/cgal_types.h +++ b/boustrophedon_server/include/boustrophedon_server/cgal_types.h @@ -4,13 +4,17 @@ #include #include #include +#include +#include using Kernel = CGAL::Cartesian; using Polygon = CGAL::Polygon_2; using Point = Kernel::Point_2; +using Vector = Kernel::Vector_2; using Traits = CGAL::Arr_linear_traits_2; using Segment = Traits::Segment_2; using Line = Traits::Line_2; using AffineTransform = CGAL::Aff_transformation_2; +using NeighborSearch = CGAL::Orthogonal_k_neighbor_search>; #endif // SRC_CGAL_TYPES_H diff --git a/boustrophedon_server/include/boustrophedon_server/cgal_utils.h b/boustrophedon_server/include/boustrophedon_server/cgal_utils.h index 4b47547..2fd9691 100644 --- a/boustrophedon_server/include/boustrophedon_server/cgal_utils.h +++ b/boustrophedon_server/include/boustrophedon_server/cgal_utils.h @@ -1,10 +1,12 @@ #ifndef SRC_CGAL_UTILS_H #define SRC_CGAL_UTILS_H -#include "cgal_types.h" -#include "boustrophedon_types.h" +#include "boustrophedon_server/cgal_types.h" +#include "boustrophedon_server/boustrophedon_types.h" -AffineTransform preprocessPolygon(Polygon& polygon, const Point& robot_position, const double& striping_angle); +#define EPSILON 0.0001 + +AffineTransform preprocessPolygon(Polygon& polygon, Point& robot_position, const double& striping_angle); void postprocessPolygonAndPath(const AffineTransform& preprocess_transform, Polygon& polygon, std::vector& path); @@ -22,4 +24,25 @@ static void transformPoints(const Iterator_t& first, const Iterator_t& last, void transformNavPoints(const std::vector::iterator& first, const std::vector::iterator& last, const CGAL::Aff_transformation_2& transform); +Polygon::Vertex_const_circulator getLeftVertex(const Polygon& polygon); + +std::vector getIntersectionPoints(const Polygon& polygon, const Line& line); + +std::vector::const_iterator findPointInVector(const Point& point, const std::vector& vector); + +std::vector::const_iterator findPointInVector(const Point& point, const std::vector::iterator& begin, + const std::vector::iterator& end); + +void insertPointAlongEdge(const Point& point, Polygon& polygon); + +Polygon mergePolygons(const Polygon& poly_1, const Polygon& poly_2); + +bool approximatelyEqual(Point a, Point b, double epsilon); + +bool approximatelyEqual(double a, double b, double epsilon); + +bool definitelyGreaterThan(double a, double b, double epsilon); + +bool definitelyLessThan(double a, double b, double epsilon); + #endif // SRC_CGAL_UTILS_H diff --git a/boustrophedon_server/include/boustrophedon_server/outline_planner.h b/boustrophedon_server/include/boustrophedon_server/outline_planner.h index d96bb16..de062c2 100644 --- a/boustrophedon_server/include/boustrophedon_server/outline_planner.h +++ b/boustrophedon_server/include/boustrophedon_server/outline_planner.h @@ -1,15 +1,26 @@ #ifndef SRC_OUTLINE_PLANNER_H #define SRC_OUTLINE_PLANNER_H -#include "cgal_types.h" -#include "boustrophedon_types.h" +#include "boustrophedon_server/cgal_types.h" +#include "boustrophedon_server/boustrophedon_types.h" class OutlinePlanner { public: struct Parameters { - /** The number of passes to make in the outline portion + /** If true, the path will start by retracing the boundary + */ + bool repeat_boundary = false; + /** If true, the path will follow a clockwise outline (including interior outlines). Else, counter-clockwise. + */ + bool outline_clockwise = true; + /** If true, the planner will skip any outline tracing and go straight to striping. + */ + bool skip_outlines = false; + + /** The number of passes to make in the outline portion, where the first interior polygon = 1. + * CAUTION: Attempting to set more outline_layers than possible will cause the server to abort. */ int outline_layers = 3; /** Center-to-center distance (in meters) between passes in the coverage path. @@ -21,12 +32,15 @@ class OutlinePlanner void setParameters(Parameters parameters); - void addToPath(Polygon polygon, const Point& robot_position, std::vector& path, Polygon& innermost_polygon); + bool addToPath(Polygon polygon, const Point& robot_position, std::vector& path, Polygon& innermost_polygon); private: Parameters params_; void shiftPolygonToStartNearRobot(Polygon& polygon, const Point& robot_position); + bool addOutermostOutline(std::vector& path, const Polygon& polygon); + bool addInnerOutline(std::vector& path, const Polygon& polygon, const double& offset, + Polygon& innermost_polygon); }; #endif // SRC_OUTLINE_PLANNER_H diff --git a/boustrophedon_server/include/boustrophedon_server/striping_planner.h b/boustrophedon_server/include/boustrophedon_server/striping_planner.h index 959a3ba..39dee99 100644 --- a/boustrophedon_server/include/boustrophedon_server/striping_planner.h +++ b/boustrophedon_server/include/boustrophedon_server/striping_planner.h @@ -1,35 +1,69 @@ #ifndef SRC_BOUSTROPHEDONPLANNER_H #define SRC_BOUSTROPHEDONPLANNER_H -#include "cgal_types.h" -#include "boustrophedon_types.h" +#include "boustrophedon_server/cgal_types.h" +#include "boustrophedon_server/cgal_utils.h" +#include "boustrophedon_server/boustrophedon_types.h" class StripingPlanner { public: struct Parameters { - /** Center-to-center distance (in meters) between passes in the coverage path. - * - * Applies to both outline and fill portions. - */ - double stripe_separation; + // Center-to-center distance (in meters) between passes in the coverage path. + // Applies to both outline and fill portions. + double stripe_separation = 0.5; + + // Distance between intermediary waypoints along a stripe + // Defaults to max double, so it doesn't stripe unless you have a REALLY big boundary + double intermediary_separation = std::numeric_limits::max(); + + // Determines whether the path will travel along the boundary to get to the start and end points of the striping + // path. Also makes the mower return to the starting point when finished. + bool travel_along_boundary = true; + + // Determines whether a half-y-turn arc will be added to the path between each stripe, to smooth out turning. + bool enable_half_y_turn = true; + + // Determines the amount of points in each arc of a half-y-turn. + int points_per_turn = 20; + + // Determines the offset that a turn starts at -- setting how much room there is for a turn. + // Larger moves the turn closer to the boundary. + double turn_start_offset = 0.5; // meters }; void setParameters(Parameters parameters); - void addToPath(const Polygon& polygon, const Point& robot_position, std::vector& path); + void addToPath(const Polygon& polygon, const Polygon& sub_polygon, Point& robot_position, + std::vector& path); + void addReturnToStart(const Polygon& polygon, const Point& start_position, const Point& robot_position, + std::vector& path); private: + enum class StripingDirection + { + STARTING, + UP, + DOWN + }; + Parameters params_; - /** Constructs the fill portion of the mowing path, which stripes back and - * forth over the inner area left after the outline portion is complete. - * - */ - void fillPolygon(const Polygon& polygon, std::vector& path); + // Constructs the fill portion of the mowing path, which stripes back and + // forth over the inner area left after the outline portion is complete. + void fillPolygon(const Polygon& polygon, std::vector& path, const Point& robot_position); + + void addIntermediaryPoints(std::vector& intersection); + void addBoundaryFollowingPoints(std::vector& path, const Point& next_stripe_start, Polygon polygon); + void addHalfYTurnPoints(std::vector& path, const Point& next_stripe_start, StripingDirection& stripe_dir); + std::vector generateDiscretizedArc(const Point& center_point, const float& radius, const float& start_rad, + const float& end_rad, const int& num_points); - std::vector getIntersectionPoints(const Polygon& polygon, const Line& line); + bool isLeftClosest(const Polygon& polygon, const Point& robot_position, double& min_x, double& max_x); + std::vector getOutlinePathToPoint(const Polygon& polygon, const Point& start_point, const Point& end_point); + static std::vector getPolygonPath(const Polygon& polygon, const Point& start_point, const Point& end_point); + float getPathLength(const std::vector& path); }; #endif // SRC_BOUSTROPHEDONPLANNER_H diff --git a/boustrophedon_server/launch/boustrophedon_server.launch b/boustrophedon_server/launch/boustrophedon_server.launch index ff7d76a..52170f1 100644 --- a/boustrophedon_server/launch/boustrophedon_server.launch +++ b/boustrophedon_server/launch/boustrophedon_server.launch @@ -1,7 +1,20 @@ - - + + + + + + + + + + + + + + + diff --git a/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_node.cpp b/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_node.cpp index fb3b835..62da1ba 100644 --- a/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_node.cpp +++ b/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_node.cpp @@ -1,5 +1,5 @@ #include -#include +#include "boustrophedon_server/boustrophedon_planner_server.h" int main(int argc, char** argv) { diff --git a/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_server.cpp b/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_server.cpp index 2b5e554..5df9105 100644 --- a/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_server.cpp +++ b/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_server.cpp @@ -1,6 +1,6 @@ #include -#include +#include "boustrophedon_server/boustrophedon_planner_server.h" BoustrophedonPlannerServer::BoustrophedonPlannerServer() : private_node_handle_("~") @@ -10,25 +10,103 @@ BoustrophedonPlannerServer::BoustrophedonPlannerServer() &BoustrophedonPlannerServer::convertStripingPlanToPath, this) } { std::size_t error = 0; + error += static_cast( + !rosparam_shortcuts::get("plan_path", private_node_handle_, "repeat_boundary", repeat_boundary_)); + error += static_cast( + !rosparam_shortcuts::get("plan_path", private_node_handle_, "outline_clockwise", outline_clockwise_)); + error += static_cast( + !rosparam_shortcuts::get("plan_path", private_node_handle_, "skip_outlines", skip_outlines_)); error += static_cast( !rosparam_shortcuts::get("plan_path", private_node_handle_, "outline_layer_count", outline_layer_count_)); error += static_cast( !rosparam_shortcuts::get("plan_path", private_node_handle_, "stripe_separation", stripe_separation_)); + error += static_cast( + !rosparam_shortcuts::get("plan_path", private_node_handle_, "intermediary_separation", intermediary_separation_)); error += static_cast( !rosparam_shortcuts::get("plan_path", private_node_handle_, "stripe_angle", stripe_angle_)); + error += static_cast(!rosparam_shortcuts::get("plan_path", private_node_handle_, + "enable_stripe_angle_orientation", enable_orientation_)); + error += static_cast( + !rosparam_shortcuts::get("plan_path", private_node_handle_, "travel_along_boundary", travel_along_boundary_)); + error += static_cast(!rosparam_shortcuts::get( + "plan_path", private_node_handle_, "allow_points_outside_boundary", allow_points_outside_boundary_)); + error += static_cast( + !rosparam_shortcuts::get("plan_path", private_node_handle_, "enable_half_y_turns", enable_half_y_turns_)); + error += static_cast( + !rosparam_shortcuts::get("plan_path", private_node_handle_, "points_per_turn", points_per_turn_)); + error += static_cast( + !rosparam_shortcuts::get("plan_path", private_node_handle_, "turn_start_offset", turn_start_offset_)); + error += static_cast( + !rosparam_shortcuts::get("plan_path", private_node_handle_, "publish_polygons", publish_polygons_)); + error += static_cast( + !rosparam_shortcuts::get("plan_path", private_node_handle_, "publish_path_points", publish_path_points_)); rosparam_shortcuts::shutdownIfError("plan_path", error); - striping_planner_.setParameters({ stripe_separation_ }); - outline_planner_.setParameters({ outline_layer_count_, stripe_separation_ }); + if (intermediary_separation_ <= 0.0) + { + // doesn't make sense, or we don't want intermediaries. set it to double max so we can't make any intermediaries + intermediary_separation_ = std::numeric_limits::max(); + } + + if (enable_half_y_turns_ && outline_layer_count_ < 1) + { + if (allow_points_outside_boundary_) + { + ROS_WARN_STREAM("Current configuration will result in turns that go outside the boundary, but this has been " + "explicitly enabled"); + } + else + { + // we can't do half-y-turns safely without an inner boundary layer, as the arc will stick outside of the boundary + ROS_ERROR_STREAM("Cannot plan using half-y-turns if the outline_layer_count is less than 1! Boustrophedon " + "planner will not start."); + return; + } + } + + striping_planner_.setParameters({ stripe_separation_, intermediary_separation_, travel_along_boundary_, + enable_half_y_turns_, points_per_turn_, turn_start_offset_ }); + outline_planner_.setParameters( + { repeat_boundary_, outline_clockwise_, skip_outlines_, outline_layer_count_, stripe_separation_ }); action_server_.start(); + + if (publish_polygons_) + { + initial_polygon_publisher_ = private_node_handle_.advertise("initial_polygon", 1); + preprocessed_polygon_publisher_ = + private_node_handle_.advertise("preprocessed_polygon", 1); + } + // mainly for use with plotJuggler, which wants the points to be put one at a time on the same topic + if (publish_path_points_) + { + path_points_publisher_ = private_node_handle_.advertise("path_points", 1000); + polygon_points_publisher_ = private_node_handle_.advertise("polygon_points", 1000); + } } void BoustrophedonPlannerServer::executePlanPathAction(const boustrophedon_msgs::PlanMowingPathGoalConstPtr& goal) { std::string boundary_frame = goal->property.header.frame_id; - Polygon polygon = fromBoundary(goal->property); + if (enable_orientation_) + { + stripe_angle_ = getStripeAngleFromOrientation(goal->robot_position); + } + Polygon polygon = fromBoundary(goal->property); + if (!checkPolygonIsValid(polygon)) + { + action_server_.setAborted(Server::Result(), std::string("Boustrophedon planner does not work for polygons of " + "size " + "< 3.")); + return; + } + if (!polygon.is_simple()) + { + action_server_.setAborted(Server::Result(), std::string("Boustrophedon planner only works for simple (non " + "self-intersecting) polygons.")); + return; + } Point robot_position; try { @@ -40,31 +118,56 @@ void BoustrophedonPlannerServer::executePlanPathAction(const boustrophedon_msgs: std::string("Boustrophedon planner failed with a tf exception: ") + ex.what()); return; } - std::vector path; auto preprocess_transform = preprocessPolygon(polygon, robot_position, stripe_angle_); - Polygon fill_polygon; - outline_planner_.addToPath(polygon, robot_position, path, fill_polygon); + if (publish_polygons_) + { + initial_polygon_publisher_.publish(goal->property); + preprocessed_polygon_publisher_.publish(convertCGALPolygonToMsg(polygon)); + } + Polygon fill_polygon; + if (!outline_planner_.addToPath(polygon, robot_position, path, fill_polygon)) + { + action_server_.setAborted(Server::Result(), std::string("Boustrophedon planner failed because " + "outline_layer_count " + "was too large for the boundary.")); + return; + } PolygonDecomposer polygon_decomposer{}; + // A print statement MUST be here, see issue #1586 + ROS_INFO_STREAM("Decomposing boundary polygon into sub-polygons..."); polygon_decomposer.decompose(fill_polygon); std::vector sub_polygons = polygon_decomposer.getSubPolygons(robot_position); - + ROS_INFO_STREAM("Broke the boundary up into " << sub_polygons.size() << " sub-polygons"); + Polygon merged_polygon; + Point start_position = robot_position; for (const auto& subpoly : sub_polygons) { - striping_planner_.addToPath(subpoly, robot_position, path); - } + // combine the new subpoly with the merged_polygon. If merged_polygon is empty, it returns the sub polygon + merged_polygon = mergePolygons(merged_polygon, subpoly); + // add the stripes to the path, using merged_polygon boundary to travel if necessary. + striping_planner_.addToPath(merged_polygon, subpoly, robot_position, path); + } + if (travel_along_boundary_) + { + striping_planner_.addReturnToStart(merged_polygon, start_position, robot_position, path); + } postprocessPolygonAndPath(preprocess_transform, polygon, path); - + if (publish_path_points_) // if we care about visualizing the planned path in plotJuggler + { + publishPathPoints(path); + publishPolygonPoints(polygon); + } auto result = toResult(std::move(path), boundary_frame); action_server_.setSucceeded(result); } boustrophedon_msgs::PlanMowingPathResult BoustrophedonPlannerServer::toResult(std::vector&& path, - const std::string& frame) const + const std::string& frame) const { boustrophedon_msgs::PlanMowingPathResult result; result.plan.points.reserve(path.size()); @@ -121,3 +224,68 @@ bool BoustrophedonPlannerServer::convertStripingPlanToPath(boustrophedon_msgs::C }); return true; } + +geometry_msgs::PolygonStamped BoustrophedonPlannerServer::convertCGALPolygonToMsg(const Polygon& poly) const +{ + geometry_msgs::PolygonStamped stamped_poly; + + for (auto it = poly.vertices_begin(); it != poly.vertices_end(); it++) + { + geometry_msgs::Point32 point; + point.x = float(it->x()); + point.y = float(it->y()); + point.z = float(0.0); + stamped_poly.polygon.points.push_back(point); + } + + stamped_poly.header.frame_id = "map"; + stamped_poly.header.stamp = ros::Time::now(); + return stamped_poly; +} + +bool BoustrophedonPlannerServer::checkPolygonIsValid(const Polygon& poly) const +{ + return !(poly.size() < 3); // expand later if we find more cases of invalid polygons +} + +// get the yaw from the robot_position part of the given goal +double BoustrophedonPlannerServer::getStripeAngleFromOrientation(const geometry_msgs::PoseStamped& robot_position) +{ + tf::Quaternion quat(robot_position.pose.orientation.x, robot_position.pose.orientation.y, + robot_position.pose.orientation.z, robot_position.pose.orientation.w); + tf::Matrix3x3 m(quat); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + ROS_INFO_STREAM("Got Striping Angle from Orientation: " << yaw); + // TODO: Recalibrate the IMU so that we can get rid of this constant below. + return yaw + 1.57079632679; // Adds PI / 2 to account for incorrect IMU calibration / reference vector +} + +// publishes the path points one at a time for visualization in plotJuggler +void BoustrophedonPlannerServer::publishPathPoints(const std::vector& path) const +{ + for (NavPoint point : path) + { + geometry_msgs::PointStamped stripe_point{}; + stripe_point.header.stamp = ros::Time::now(); + stripe_point.point.x = point.point.x(); + stripe_point.point.y = point.point.y(); + path_points_publisher_.publish(stripe_point); + ros::spinOnce(); + } +} + +// publishes the polygon points one at a time for visualization in plotJuggler +void BoustrophedonPlannerServer::publishPolygonPoints(const Polygon& poly) const +{ + for (auto it = poly.vertices_begin(); it != poly.vertices_end(); it++) + { + geometry_msgs::PointStamped point; + point.header.stamp = ros::Time::now(); + point.point.x = float(it->x()); + point.point.y = float(it->y()); + point.point.z = float(0.0); + polygon_points_publisher_.publish(point); + ros::spinOnce(); + } +} \ No newline at end of file diff --git a/boustrophedon_server/src/boustrophedon_server/boustrophedon_types.cpp b/boustrophedon_server/src/boustrophedon_server/boustrophedon_types.cpp index 3886422..69a929a 100644 --- a/boustrophedon_server/src/boustrophedon_server/boustrophedon_types.cpp +++ b/boustrophedon_server/src/boustrophedon_server/boustrophedon_types.cpp @@ -1,4 +1,4 @@ -#include +#include "boustrophedon_server/boustrophedon_types.h" NavPoint::NavPoint(PointType type, Point point) : type{ type }, point{ std::move(point) } { diff --git a/boustrophedon_server/src/boustrophedon_server/cellular_decomposition/cell.cpp b/boustrophedon_server/src/boustrophedon_server/cellular_decomposition/cell.cpp index 1cbdbc8..09fb789 100644 --- a/boustrophedon_server/src/boustrophedon_server/cellular_decomposition/cell.cpp +++ b/boustrophedon_server/src/boustrophedon_server/cellular_decomposition/cell.cpp @@ -1,147 +1,147 @@ -#include +#include "boustrophedon_server/cellular_decomposition/cell.h" using namespace bcd; -Vertex::Vertex(Event type, Circulator point) : type{ type }, point{ point } +Cell::Cell(const Polygon& polygon) { + points.insert(points.begin(), polygon.vertices_begin(), polygon.vertices_end()); } -bool Vertex::isAdjacentTo(const Vertex& other) const +Polygon Cell::toPolygon() const { - return *(point + 1) == *other.point || *(point - 1) == *other.point; + Polygon polygon; + polygon.insert(polygon.vertices_end(), points.begin(), points.end()); + return polygon; } -Circulator Vertex::nextPoint() const +void Cell::insertPointOnEdge(const Point& point) { - return point + 1; -} + // use the function in cgal_utils + Polygon polygon = toPolygon(); + insertPointAlongEdge(point, polygon); -Circulator Vertex::previousPoint() const -{ - return point - 1; + points = std::vector(); + points.insert(points.begin(), polygon.vertices_begin(), polygon.vertices_end()); } -std::ostream& operator<<(std::ostream& out, Event event) +bool Cell::isPointOnEdge(const Point& point) { - std::string string; - switch (event) + Polygon polygon = toPolygon(); + auto edge_iterator = polygon.edges_begin(); + for (; edge_iterator != polygon.edges_end(); edge_iterator++) { - case Event::Start: - string = std::string("Start"); - break; - case Event::End: - string = std::string("End"); - break; - case Event::Open: - string = std::string("Open"); - break; - case Event::Close: - string = std::string("Close"); - break; - case Event::Ceil: - string = std::string("Ceil"); - break; - case Event::Floor: - string = std::string("Floor"); - break; + Segment edge = *edge_iterator; + // check the point's distance to the edge - it might be off by a little bit because of floating point error + if (definitelyLessThan(CGAL::squared_distance(point, edge), EPSILON, EPSILON)) + { + // it's on the edge + return true; + } } - out << string; - return out; -} - -std::ostream& operator<<(std::ostream& out, const Vertex& vertex) -{ - out << *vertex.point << ": " << vertex.type; - return out; -} - -Cell::Cell(Vertex start) : start{ std::move(start) } -{ -} - -bool Cell::connectsToFloor(const Vertex& other) const -{ - bool connects_to_start_floor = floor_points.empty() && start.isAdjacentTo(other); - bool connects_to_end_floor = !floor_points.empty() && floor_points.back().isAdjacentTo(other); - return connects_to_start_floor || connects_to_end_floor; -} - -bool Cell::connectsToCeil(const Vertex& other) const -{ - bool connects_to_start_ceil = ceiling_points.empty() && start.isAdjacentTo(other); - bool connects_to_end_ceil = !ceiling_points.empty() && ceiling_points.back().isAdjacentTo(other); - return connects_to_start_ceil || connects_to_end_ceil; + // it wasn't on any of the edges + return false; } -bool Cell::enclosesVector(const Vertex& other) const +// check if this cell is adjacent to other cell, aka shares two vertices +bool Cell::isAdjacentTo(const bcd::Cell& other) { - auto top_point = currentCeil().point; - auto bottom_point = currentFloor().point; - - return bottom_point->y() < other.point->y() && other.point->y() < top_point->y(); -} - -const Vertex& Cell::currentCeil() const -{ - return ceiling_points.empty() ? start : ceiling_points.back(); -} + int same_points_count = 0; + // loop through every point in this cell + for (auto& point : points) + { + // loop through every point in other cell + for (const auto& other_point : other.points) + { + if (approximatelyEqual(point, other_point, EPSILON)) + { + same_points_count++; + } + } + } -const Vertex& Cell::currentFloor() const -{ - return floor_points.empty() ? start : floor_points.back(); -} + // if there were two point shared between these cells, they're adjacent + if (same_points_count == 2) + { + return true; + } + else if (same_points_count > 2) + { + std::cout << "somehow two cells were adjacent on " << same_points_count << " points!" << std::endl; + } -const Point Cell::nextCeilPoint() const -{ - return *(currentCeil().point - 1); + return false; } -const Point Cell::nextFloorPoint() const +bool Cell::canMergeWith(const std::vector& cells) { - return *(currentFloor().point + 1); -} + // check if this cell can merge with the given list of cells + // we check this by checking if any cell has points >= smallest x of this cell and points <= largest x of this cell + // in addition, the cell has to be adjacent to at least one other cell -std::vector Cell::toPoints() const -{ - std::vector points; - points.reserve(2 + floor_points.size() + ceiling_points.size()); - points.emplace_back(*start.point); - std::transform(floor_points.begin(), floor_points.end(), std::back_inserter(points), - [](const Vertex& vertex) { return *vertex.point; }); - points.emplace_back(*end.point); - std::transform(ceiling_points.rbegin(), ceiling_points.rend(), std::back_inserter(points), - [](const Vertex& vertex) { return *vertex.point; }); - return points; -} + // check for the case where there are no cells in the current list: + if (cells.size() == 0) + { + return true; + } -Polygon Cell::toPolygon() const -{ - Polygon polygon; - auto points = toPoints(); - polygon.insert(polygon.vertices_end(), points.begin(), points.end()); - return polygon; -} + // get the left and right vertex + double min_x = std::numeric_limits::max(); + double max_x = std::numeric_limits::lowest(); // min() is smallest positive value, lowest() is -max() -std::ostream& operator<<(std::ostream& out, const bcd::Cell& cell) -{ - out << cell.start; - for (const auto& vertex : cell.floor_points) + for (const Point& point : points) { - out << ", " << vertex; + if (point.x() < min_x) + { + min_x = point.x(); + } + if (point.x() > max_x) + { + max_x = point.x(); + } } - out << ", " << cell.end; - for (auto vertex = cell.ceiling_points.rbegin(); vertex != cell.ceiling_points.rend(); vertex++) + + bool has_adjacent_cell = false; + // check to see if any points in the given vector violate the above rule + for (const Cell& cell : cells) { - out << ", " << *vertex; + // record to see if there are any points in this cell > min_x and and points < max_x + bool has_larger_point = false; + bool has_smaller_point = false; + + for (const Point& point : cell.points) + { + if (definitelyGreaterThan(point.x(), min_x, EPSILON)) + { + has_larger_point = true; + } + if (definitelyLessThan(point.x(), max_x, EPSILON)) + { + has_smaller_point = true; + } + // if both conditions have been violated, this cell shares the same x space as the given list, so it can't merge + if (has_larger_point && has_smaller_point) + { + return false; + } + } + + // no conflict with this cell, but let's check to see if it's adjacent + if (isAdjacentTo(cell)) + { + has_adjacent_cell = true; + } } - out << std::endl << "neigbours: "; + // if we reached here then this cell should be fine to merge...assuming there was at least one adjacent cell! + return has_adjacent_cell; +} - for (const auto& neighbour : cell.neighbors) +std::ostream& operator<<(std::ostream& out, const bcd::Cell& cell) +{ + for (const auto& vertex : cell.points) { - out << neighbour->start << " -> " << neighbour->end << std::endl; + out << vertex << ", "; } - out << std::endl; return out; } diff --git a/boustrophedon_server/src/boustrophedon_server/cellular_decomposition/polygon_decomposer.cpp b/boustrophedon_server/src/boustrophedon_server/cellular_decomposition/polygon_decomposer.cpp index 7f26091..0822c27 100644 --- a/boustrophedon_server/src/boustrophedon_server/cellular_decomposition/polygon_decomposer.cpp +++ b/boustrophedon_server/src/boustrophedon_server/cellular_decomposition/polygon_decomposer.cpp @@ -1,420 +1,559 @@ -#include -#include +#include +#include "boustrophedon_server/cellular_decomposition/polygon_decomposer.h" using namespace bcd; void PolygonDecomposer::decompose(Polygon& polygon) { - createVertices(polygon); - createCells(); + // first, find all the points in the polygon that could cause us to have to split it into multiple cells + std::vector critical_points = getCriticalInflectionPoints(polygon); + + // sort those critical points for later use + sortVertices(critical_points); + + // use those critical points and the polygon to create a collection of convex cells + createCells(critical_points, polygon); } -std::vector PolygonDecomposer::getSubPolygons(const Point& position) const +std::vector PolygonDecomposer::getSubPolygons(const Point& position) { - Cell* closest_cell = getClosestCell(position); - std::vector cells = visitCells(closest_cell); + // get an iterator to the closest cell to position. This will be the root of our depth-first tree + auto closest_cell = getClosestCell(position); + + // reset the visited state of all the cells, in case we've done this before + for (Cell cell : cells_) + { + cell.visited = false; + } - std::vector polygons = toPolygons(cells); + std::vector visited_cells = visitCells(closest_cell); + + std::vector polygons = toPolygons(visited_cells); return polygons; } -void PolygonDecomposer::createVertices(Polygon& polygon) +/** + * 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. + */ +std::vector PolygonDecomposer::getCriticalInflectionPoints(Polygon& polygon) { - insertOpenCloseVertices(polygon); - - auto left_vertex = CGAL::left_vertex_2(polygon.vertices_circulator(), --polygon.vertices_circulator()); - Event current_event = Event::Start; - - vertices_.emplace_back(current_event, left_vertex); + // ensure that the polygon is counter_clockwise oriented. Later functions require this. + if (polygon.is_clockwise_oriented()) + { + polygon.reverse_orientation(); + } - auto vertex = left_vertex + 1; + std::vector critical_inflection_points; + Polygon::Vertex_const_circulator vertex = polygon.vertices_circulator(); - while (vertex != left_vertex) + // loop through all vertices of the polygon + do { - current_event = getEvent(vertex, current_event); - vertices_.emplace_back(current_event, vertex); + // for each vertex, check its interior angle. If greater than 180 degrees, add it to the list + double interior_angle = getInteriorAngleOfVertex(vertex); + if (interior_angle > 180.0) + { + critical_inflection_points.push_back(*vertex); + } + vertex++; - } + } while (vertex != polygon.vertices_circulator()); + + // return the list + return critical_inflection_points; } -void PolygonDecomposer::insertOpenCloseVertices(Polygon& 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 + */ +double PolygonDecomposer::getInteriorAngleOfVertex(const Polygon::Vertex_const_circulator& vertex) { - auto vertex = CGAL::left_vertex_2(polygon.vertices_circulator(), --polygon.vertices_circulator()); - Event current_event = Event::Start; + // check if vertex is a critical point + // because the polygon is counter_clockwise oriented, interior angle will be counter clockwise from prev to next + auto prev_vertex = vertex - 1; + auto next_vertex = vertex + 1; + + // make vectors from vertex to previous and next points + Vector v1 = *prev_vertex - *vertex; + Vector v2 = *next_vertex - *vertex; - // TODO(Oswin): Use something more efficient. I CBF to write a hash function for now. - std::vector visited_vertices; + // get the atan2 angle of each vector to x-axis vector, radians in range -PI to PI + double alpha = std::atan2(v1.y(), v1.x()); + double beta = std::atan2(v2.y(), v2.x()); + + // transform alpha and beta to the range 0 to 2PI + if (alpha < 0) + { + alpha += (CGAL_PI * 2); + } + if (beta < 0) + { + beta += (CGAL_PI * 2); + } - auto left_vertex = *vertex; - vertex++; - while (*vertex != left_vertex) + // use alpha and beta to calculate the clockwise-angle between alpha and beta + double angle; + if (alpha > beta) + { + angle = alpha - beta; + } + else if (alpha < beta) { - current_event = getEvent(vertex, current_event); - if ((current_event == Event::Open || current_event == Event::Close) && - std::find(visited_vertices.begin(), visited_vertices.end(), *vertex) == visited_vertices.end()) + angle = (CGAL_PI * 2) - beta + alpha; + } + else + { + angle = 0.0; + } + + // scale angle to 0 to 360 in degrees + angle *= 180 / CGAL_PI; + + // return angle + return angle; +} + +/** + * Sorts the given list of vertices by x coordinate. If equal, lower y coordinate is placed first. + * @param vertices + */ +void PolygonDecomposer::sortVertices(std::vector& vertices) +{ + std::sort(vertices.begin(), vertices.end(), [](const Point& lhs, const Point& rhs) { + // sort first using the x coordinate + if (!approximatelyEqual(lhs.x(), rhs.x(), EPSILON)) { - visited_vertices.emplace_back(*vertex); - sliceAndInsertIntoPolygon(polygon, *vertex); - // Start from beginning lmao because iterators are (probably) invalidated. - vertex = CGAL::left_vertex_2(polygon.vertices_circulator(), --polygon.vertices_circulator()); - current_event = Event::Start; + return definitelyLessThan(lhs.x(), rhs.x(), EPSILON); } - vertex++; - } + // if x coordinate is the same, then sort using y coordinate + if (!approximatelyEqual(lhs.y(), rhs.y(), EPSILON)) + { + return definitelyLessThan(lhs.y(), rhs.y(), EPSILON); + } + + // else just return true, the two points are in the same place + return true; + }); } -void PolygonDecomposer::sliceAndInsertIntoPolygon(Polygon& polygon, const Point& point) +/** + * 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 PolygonDecomposer::createCells(const std::vector& critical_vertices, const Polygon& polygon) { - auto vertical_line = Line(Point(point.x(), 0), Point(point.x(), 1)); + cells_.emplace_back(polygon); - for (auto it = polygon.vertices_begin(); it != polygon.vertices_end() - 1; it++) + // create all of the new cells using the vertices in critical_vertices + for (const Point& vertex : critical_vertices) { - auto polygon_edge = Segment(*it, *(it + 1)); + std::vector intersection_points = + getIntersectionPoints(polygon, Line(Point(vertex.x(), 0.0), Point(vertex.x(), 1.0))); - auto result = CGAL::intersection(polygon_edge, vertical_line); + // sort the intersection points from lowest y to highest y. The default behavior could have them mixed up. + // sortVertices() will work because the X coordinate should be the same for all of the intersection points + sortVertices(intersection_points); - if (result) + Point above_point; // the intersection point just above the critical vertex + Point below_point; // the intersection point just below the critical vertex + + // if there is an above point, we make a new cell using above and vertex as endpoints + if (findAbovePoint(above_point, vertex, intersection_points, polygon)) { - // Only one point => didn't intersect a vertical edge on the polygon. - // Insert into the polygon between it and it+1 - if (auto found_point = boost::get(&*result)) - { - if (*found_point != polygon_edge.source() && *found_point != polygon_edge.target()) - { - // Fuck pointer invalidation - it = polygon.insert((it + 1), *found_point) + 1; - } - } + sliceNewCell(above_point, vertex); } - } - auto polygon_edge = Segment(*polygon.vertices_circulator(), *(polygon.vertices_circulator() - 1)); - // Check the last edge (last point -> first point) as well - auto result = CGAL::intersection(polygon_edge, vertical_line); + // if there is a below point, we make a new cell using vertex and below as endpoints + if (findBelowPoint(below_point, vertex, intersection_points, polygon)) + { + sliceNewCell(vertex, below_point); + } + } - if (result) + // link adjacent cells together now that they've all been created + // loop through every cell except the last one + for (auto cell_iterator = cells_.begin(); cell_iterator != cells_.end() - 1; ++cell_iterator) { - // Only one point => didn't intersect a vertical edge on the polygon. - // Insert into the polygon between it and it+1 - if (auto found_point = boost::get(&*result)) + // loop through every cell between cell_iterator and the end. This way we check every combination. + for (auto other_iterator = cell_iterator + 1; other_iterator != cells_.end(); ++other_iterator) { - if (*found_point != polygon_edge.source() && *found_point != polygon_edge.target()) + if (cell_iterator->isAdjacentTo(*other_iterator)) { - polygon.push_back(*found_point); + cell_iterator->neighbors.push_back(&*other_iterator); + other_iterator->neighbors.push_back(&*cell_iterator); } } } } - -void PolygonDecomposer::createCells() +/** + * 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 + * @returns true if above point is found, false if not + */ +bool PolygonDecomposer::findAbovePoint(Point& above, const Point& critical_point, std::vector& points, + const Polygon& polygon) { - sortVertices(); + // first find the critical_point in the list of intersection points. Use the cgal_utils function here. + auto it = findPointInVector(critical_point, points); - std::vector> current_cells; - current_cells.emplace_back(std::make_unique(vertices_.front())); - - for (int i = 1; i < vertices_.size(); i++) + // check if the critical point was found + if (it == points.end()) { - Vertex& current_vertex = vertices_[i]; - - bool done = handleVertex(current_vertex, current_cells); - // TODO(Oswinn): Actually fix this instead of this patch - // Swap the point with the other point that it's connected with, hopefully everything works out. - if (!done) + // we didn't find it...probably because of inexact_constructions kernel. Insert it into the list where it should be + // and continue. + for (it = points.begin(); it != points.end() - 1; it++) { - auto other = [&]() { - if (current_vertex.point->x() == current_vertex.nextPoint()->x()) - { - return std::find_if(vertices_.begin(), vertices_.end(), - [&](const Vertex& v) { return *v.point == *current_vertex.nextPoint(); }); - } - if (current_vertex.point->x() == current_vertex.previousPoint()->x()) - { - return std::find_if(vertices_.begin(), vertices_.end(), - [&](const Vertex& v) { return *v.point == *current_vertex.previousPoint(); }); - } - return vertices_.end(); - }(); - - if (other != vertices_.end() && other - vertices_.begin() > i) + if (it->y() < critical_point.y() && critical_point.y() < (it + 1)->y()) { - std::swap(current_vertex, *other); - i--; - continue; + points.insert(it + 1, critical_point); + break; } } - } -} -bool PolygonDecomposer::handleVertex(Vertex& current_vertex, std::vector>& current_cells) -{ - switch (current_vertex.type) - { - case Event::Start: - return handleStart(current_vertex, current_cells); - case Event::Ceil: - return handleCeil(current_vertex, current_cells); - case Event::Floor: - return handleFloor(current_vertex, current_cells); - case Event::End: - return handleEnd(current_vertex, current_cells); - case Event::Open: - return handleOpen(current_vertex, current_cells); - case Event::Close: - return handleClose(current_vertex, current_cells); + // reset it + it = findPointInVector(critical_point, points); + + // because points is not const, and findAbovePoint happens before findBelowPoint, we don't have to modify + // findBelowPoint() } -} -bool PolygonDecomposer::handleFloor(bcd::Vertex& current_vertex, std::vector>& current_cells) -{ - for (auto& cell : current_cells) + // the critical point was found, but there might not be a next point in the list + auto next = it + 1; + if (next != points.end()) { - if (cell->connectsToFloor(current_vertex)) + // the next point exists, but it could be the duplicate or the above point + if (approximatelyEqual(next->y(), it->y(), EPSILON)) { - cell->floor_points.emplace_back(current_vertex); - return true; + // next point was the duplicate point! increment next so that it points to the above point, if it exists + next++; } } - return false; -} -bool PolygonDecomposer::handleCeil(Vertex& current_vertex, std::vector>& current_cells) -{ - for (auto& cell : current_cells) + // now next should either be pointing to the end or the above point + if (next != points.end() && next->y() > it->y()) { - if (cell->connectsToCeil(current_vertex)) + // do a sanity check to ensure that this line segment lies inside the boundary, use midpoint of it and next + Point midpoint = Point(it->x(), (it->y() + next->y()) / 2); + if (CGAL::bounded_side_2(polygon.vertices_begin(), polygon.vertices_end(), midpoint) == CGAL::ON_BOUNDED_SIDE) { - cell->ceiling_points.emplace_back(current_vertex); + above = *next; return true; } } + + // else return false, we didn't find the above point. It doesn't exist. return false; } -bool PolygonDecomposer::handleStart(Vertex& current_vertex, std::vector>& current_cells) +/** + * 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 + * @returns true if below point is found, false if not + */ +bool PolygonDecomposer::findBelowPoint(Point& below, const Point& critical_point, const std::vector& points, + const Polygon& polygon) { - current_cells.emplace_back(std::make_unique(current_vertex)); - return true; -} + // first find the critical_point in the list of intersection points. Use the cgal_utils function here. + auto it = findPointInVector(critical_point, points); -bool PolygonDecomposer::handleEnd(Vertex& current_vertex, std::vector>& current_cells) -{ - for (auto it = current_cells.begin(); it != current_cells.end(); it++) + // check if the critical point was found + if (it == points.end()) { - Cell* cell = it->get(); - if (cell->connectsToCeil(current_vertex) && cell->connectsToFloor(current_vertex)) - { - cell->end = current_vertex; - cells_.emplace_back(std::move(*it)); - current_cells.erase(it); - return true; - } + return false; } - return false; -} -bool PolygonDecomposer::handleOpen(Vertex& current_vertex, std::vector>& current_cells) -{ - for (auto it = current_cells.begin(); it != current_cells.end(); it++) + // the critical point was found, but there might not be a previous point in the list + auto prev = it - 1; + if (prev != points.begin() - 1 && prev->y() < it->y()) { - Cell* closing_cell = it->get(); - if (closing_cell->enclosesVector(current_vertex)) + // do a sanity check to ensure that this line segment lies inside the boundary, use midpoint of it and next + Point midpoint = Point(it->x(), (it->y() + prev->y()) / 2); + if (CGAL::bounded_side_2(polygon.vertices_begin(), polygon.vertices_end(), midpoint) == CGAL::ON_BOUNDED_SIDE) { - closing_cell->end = current_vertex; - - auto top = std::make_unique(current_vertex); - top->ceiling_points.emplace_back(closing_cell->ceiling_points.back()); - - auto bottom = std::make_unique(current_vertex); - bottom->floor_points.emplace_back(closing_cell->floor_points.back()); - - Cell* closed_cell = it->get(); - // Update neighbors list. Counterclockwise from bottom right corner: bottom, top, rest - closed_cell->neighbors.insert(closed_cell->neighbors.begin(), { bottom.get(), top.get() }); - - top->neighbors.emplace_back(closed_cell); - bottom->neighbors.emplace_back(closed_cell); - - cells_.emplace_back(std::move(*it)); - current_cells.erase(it); - - current_cells.emplace_back(std::move(top)); - current_cells.emplace_back(std::move(bottom)); + below = *prev; return true; } } + + // else return false, we didn't find the below point. It doesn't exist. return false; } -bool PolygonDecomposer::handleClose(Vertex& current_vertex, std::vector>& current_cells) +/** + * 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 PolygonDecomposer::sliceNewCell(const Point& upper, const Point& lower) { - auto top_cell_it = getTopCell(current_vertex, current_cells); - if (top_cell_it == current_cells.end()) - { - return false; - } - top_cell_it->get()->end = current_vertex; + // before we can slice a cell using this intersection, we have to figure out what cell to work on + auto working_cell_ptr = findWorkingCell(upper, lower); + Cell working_cell = *working_cell_ptr; - cells_.emplace_back(std::move(*top_cell_it)); - current_cells.erase(top_cell_it); // Fuck pointer invalidation again - auto top_cell_ptr = cells_.back().get(); + // next, we must insert upper and lower into the working cell, assuming they aren't already vertices. + working_cell.insertPointOnEdge(upper); + working_cell.insertPointOnEdge(lower); - auto bottom_cell_it = getBottomCell(current_vertex, current_cells); + Cell new_cell = Cell(); - if (bottom_cell_it == current_cells.end()) + // first we need to get a circulator of the working cell's vertices, and advance it to upper + Polygon working_polygon = working_cell.toPolygon(); + Polygon::Vertex_const_circulator it = working_polygon.vertices_circulator(); + while (!approximatelyEqual(*it, upper, EPSILON)) { - return false; + it++; } - bottom_cell_it->get()->end = current_vertex; - cells_.emplace_back(std::move(*bottom_cell_it)); - current_cells.erase(bottom_cell_it); - auto bottom_cell_ptr = cells_.back().get(); - - auto new_cell = std::make_unique(current_vertex); - new_cell->ceiling_points.emplace_back(top_cell_ptr->ceiling_points.back()); - new_cell->floor_points.emplace_back(bottom_cell_ptr->floor_points.back()); - - // Update neighbors list. Counterclockwise from bottom right corner (back of vector to front): bottom, top - new_cell->neighbors.insert(new_cell->neighbors.begin(), { top_cell_ptr, bottom_cell_ptr }); - top_cell_ptr->neighbors.emplace_back(new_cell.get()); - bottom_cell_ptr->neighbors.emplace_back(new_cell.get()); - - current_cells.emplace_back(std::move(new_cell)); - - return true; -} - -std::vector>::iterator -PolygonDecomposer::getTopCell(Vertex& current_vertex, std::vector>& current_cells) const -{ - return std::find_if(current_cells.begin(), current_cells.end(), [&](const std::unique_ptr& cell) { - return cell->nextFloorPoint() == *current_vertex.point; - }); -} - -std::vector>::iterator -PolygonDecomposer::getBottomCell(Vertex& current_vertex, std::vector>& current_cells) const -{ - return std::find_if(current_cells.begin(), current_cells.end(), [&](const std::unique_ptr& cell) { - return cell->nextCeilPoint() == *current_vertex.point; - }); -} - -Event PolygonDecomposer::getEvent(const Polygon::Vertex_const_circulator& vertex, Event last_event) const -{ - // Transition out of special events - if (last_event == Event::Start || last_event == Event::Open) + // start from upper and rotate counter-clockwise until we reach lower + // put every point from upper to lower in the working cell into the new cell + while (!approximatelyEqual(*it, lower, EPSILON)) { - last_event = Event::Floor; + new_cell.points.push_back(*it); + it++; } - else if (last_event == Event::End || last_event == Event::Close) + // we also need the lower point, so append it one more time + new_cell.points.push_back(*it); + + // remove the points of new_cell (except upper and lower!) from the working cell + auto new_cell_iterator = new_cell.toPolygon().vertices_begin(); + new_cell_iterator++; // skip upper + for (; new_cell_iterator != new_cell.toPolygon().vertices_end() - 1; new_cell_iterator++) { - last_event = Event::Ceil; + // for each point in the new cell, we need to remove it from the working cell + working_cell.points.erase(findPointInVector(*new_cell_iterator, working_cell.points)); } - auto next_vertex = vertex + 1; - - if (last_event == Event::Floor && next_vertex->x() < vertex->x()) + // In certain situations, the working cell could potentially only be a line. Just remove it if so. + if (working_cell.points.size() <= 2) { - if (next_vertex->y() < vertex->y()) - { - return Event::Close; - } - return Event::End; + // remove the working cell + cells_.erase(working_cell_ptr); } - if (last_event == Event::Ceil && next_vertex->x() > vertex->x()) + else { - if (next_vertex->y() < vertex->y()) - { - return Event::Start; - } - return Event::Open; + // update the current working cell pointer to the new working cell + *working_cell_ptr = working_cell; + } + + // the same thing can happen to the new cell. Only add it to cells_ if it's not a line + if (new_cell.points.size() > 2) + { + // put the new cell into the cells vector + cells_.push_back(new_cell); } - return last_event; } -void PolygonDecomposer::sortVertices() +/** + * 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::iterator PolygonDecomposer::findWorkingCell(const Point& upper, const Point& lower) { - std::sort(vertices_.begin(), vertices_.end(), [](const Vertex& lhs, const Vertex& rhs) { - if (lhs.point->x() != rhs.point->x()) + // there should only be one cell with both upper and lower in it. We need to return that cell + auto cell_iterator = cells_.begin(); + for (; cell_iterator != cells_.end(); cell_iterator++) + { + // we need to check each cell to see if upper and lower are on it + bool found_upper = cell_iterator->isPointOnEdge(upper); + bool found_lower = cell_iterator->isPointOnEdge(lower); + + // if both are true, we found the cell we want to work on! If not, keep looking + if (found_lower && found_upper) { - return lhs.point->x() < rhs.point->x(); + return cell_iterator; } + } - return lhs.type < rhs.type; - }); -} - -std::vector PolygonDecomposer::toPolygons(std::vector cells) const -{ - std::vector polygons; - polygons.reserve(cells.size()); - - std::transform(cells.begin(), cells.end(), std::back_inserter(polygons), - [](const Cell* cell) { return cell->toPolygon(); }); - return polygons; + // if we haven't found the working cell by here, something didn't work properly. + ROS_WARN_STREAM("couldn't find working cell!"); + return cell_iterator; // this will be cells_end(); } -bcd::Cell* PolygonDecomposer::getClosestCell(const Point& position) const +/** + * Finds and returns a reference to the closest cell in cells_ from position + * @param position + * @returns a iterator pointing to the closest cell in cells_ + */ +std::vector::iterator PolygonDecomposer::getClosestCell(const Point& position) { + // A lambda to calculate the distance between a point and an edge auto distance_to_edge = [position](const Polygon::Edge_const_iterator& it) { return CGAL::squared_distance(*it, position); }; - auto distance_to_cell = [&](const std::unique_ptr& cell) { - auto polygon = cell->toPolygon(); + // A lambda to calculate the minimum distance between a point and a cell + auto distance_to_cell = [&](const Cell* cell) { + Polygon polygon = cell->toPolygon(); double smallest_distance = std::numeric_limits::max(); + + // for each edge, check if the distance to that edge is smaller than the previously smallest distance for (auto edge_it = polygon.edges_begin(); edge_it != polygon.edges_end(); edge_it++) { + // use the distance_to_edge lambda to get the smallest distance to the edge smallest_distance = std::min(smallest_distance, distance_to_edge(edge_it)); } return smallest_distance; }; - auto compare_distances = [&](const std::unique_ptr& lhs, const std::unique_ptr& rhs) { - return distance_to_cell(lhs) < distance_to_cell(rhs); + // A lambda to compare the distances from position to each of two cells + auto compare_distances = [&](const Cell lhs, const Cell rhs) { + return distance_to_cell(&lhs) < distance_to_cell(&rhs); }; - auto closest = std::min_element(cells_.begin(), cells_.end(), compare_distances); - return closest->get(); + // get an iterator pointing to the cell closest to position + return std::min_element(cells_.begin(), cells_.end(), compare_distances); } -std::vector PolygonDecomposer::visitCells(const Cell* starting_cell) const +/** + * 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 + */ +std::vector PolygonDecomposer::visitCells(std::vector::iterator starting_cell) const { - std::unordered_set visited; - std::vector cells; - const Cell* current_cell = starting_cell; + // we want to visit the cells in an order such that we have to do the least amount of backtracking possible + // create a rearranged list of cells in the order that we should look to merge them together in + std::vector rearranged_cell_tree = generateCellTree(starting_cell); - auto not_visited = [&](const Cell* cell) { return visited.find(cell) == visited.end(); }; + // get lists of cells to combine together + std::vector> combined_cells_list = determineCellsToMerge(rearranged_cell_tree); - auto get_next_unvisited_cell = [&](const Cell* cell) { - return std::find_if(cell->neighbors.begin(), cell->neighbors.end(), not_visited); - }; + std::vector final_cells; - while (cells.size() < cells_.size()) + // for each list of cells to combine together... + for (const std::vector& sub_polygon : combined_cells_list) { - visited.emplace(current_cell); - cells.emplace_back(current_cell); + // start the new cell with the first one in the list + Cell final_cell = sub_polygon.front(); - auto not_visited_cell = get_next_unvisited_cell(current_cell); - if (not_visited_cell != current_cell->neighbors.end()) + // combine all of the next cells in the list together + for (auto it = sub_polygon.begin() + 1; it != sub_polygon.end(); ++it) { - current_cell = *not_visited_cell; + final_cell = mergeCells(final_cell, *it); } - else + + // add it to the final_cells list + final_cells.push_back(final_cell); + } + + return final_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 PolygonDecomposer::generateCellTree(std::vector::iterator root) const +{ + // we're going to create a new cells vector where the root (starting point) is the first element, and adjacent cells + // children + std::vector new_cells_vector; + std::vector queue; + root->visited = true; + queue.push_back(*root); + while (!queue.empty()) + { + // get the next cell in the queue, put it on the new_cells_vector, and remove it from the queue + Cell front = queue.front(); + new_cells_vector.push_back(front); + queue.erase(queue.begin()); + + // get all the adjacent cells of front + for (Cell* neighbor : front.neighbors) { - for (auto it = cells.rbegin(); it != cells.rend(); it++) + if (!neighbor->visited) { - auto not_visited_cell = get_next_unvisited_cell(*it); - if (not_visited_cell != (*it)->neighbors.end()) - { - current_cell = *not_visited_cell; - break; - } + neighbor->visited = true; + queue.push_back(*neighbor); } } } - return cells; + return new_cells_vector; +} + +/** + * 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> PolygonDecomposer::determineCellsToMerge(std::vector& cells) const +{ + std::vector> combined_cells_list; + std::vector first_cell_list; + combined_cells_list.push_back(first_cell_list); + // traverse through the rearranged cells list + for (Cell cell : cells) + { + // check if we can merge the current combined cell and the current cell together + if (cell.canMergeWith(combined_cells_list.back())) + { + // the cells can merge! add cell to the current combined cell + combined_cells_list.back().push_back(cell); + } + else + { + // the cells can't merge. Create a new combined cell and add current cell to its + std::vector new_cell_list; + new_cell_list.push_back(cell); + combined_cells_list.push_back(new_cell_list); + } + } + return combined_cells_list; +} + +/** + * 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 + * Uses the mergePolygons method of cgal_utils + * @param cell_1 + * @param cell_2 + * @returns the new merged cell + */ +Cell PolygonDecomposer::mergeCells(const Cell& cell_1, const Cell& cell_2) const +{ + // perform all of the logic on polygons in cgal_utils.cpp + // previously logic was on cells in this function, moved for generality + Polygon new_polygon = mergePolygons(cell_1.toPolygon(), cell_2.toPolygon()); + Cell new_cell = Cell(new_polygon); + + // this is the complete new cell! it doesn't have an id or neighbors though. + return new_cell; +} + +/** + * Turns a vector of cells into a vector of polygons + * @param cells + * @returns a vector of polygons of the given cells + */ +std::vector PolygonDecomposer::toPolygons(const std::vector& cells) +{ + // create the vector to put all of the polygons into + std::vector polygons; + polygons.reserve(cells.size()); + + // use each cell's toPolygon() function to place each cell into the polygons vector + std::transform(cells.begin(), cells.end(), std::back_inserter(polygons), + [](const Cell& cell) { return cell.toPolygon(); }); + return polygons; } diff --git a/boustrophedon_server/src/boustrophedon_server/cgal_utils.cpp b/boustrophedon_server/src/boustrophedon_server/cgal_utils.cpp index 7b285be..9c72bf4 100644 --- a/boustrophedon_server/src/boustrophedon_server/cgal_utils.cpp +++ b/boustrophedon_server/src/boustrophedon_server/cgal_utils.cpp @@ -1,25 +1,19 @@ -#include +#include "boustrophedon_server/cgal_utils.h" -AffineTransform preprocessPolygon(Polygon& polygon, const Point& robot_position, const double& striping_angle) +AffineTransform preprocessPolygon(Polygon& polygon, Point& robot_position, const double& striping_angle) { AffineTransform transform(CGAL::IDENTITY); AffineTransform rotation_transform(CGAL::ROTATION, sin(striping_angle), cos(striping_angle)); transform = transform * rotation_transform; - auto max_x = CGAL::right_vertex_2(polygon.container().begin(), polygon.container().end())->x(); - if (robot_position.x() >= max_x) - { - AffineTransform flip_transform(-1, 0, 0, 1); - transform = transform * flip_transform; - } - if (polygon.is_clockwise_oriented()) { polygon.reverse_orientation(); } transformPoints(polygon.vertices_begin(), polygon.vertices_end(), transform); + robot_position = transform(robot_position); return transform; } @@ -42,3 +36,249 @@ void transformNavPoints(const std::vector::iterator& first, const std: current->point = transform(current->point); } } + +// returns a circulator pointing to the leftmost point in the polygon +// Key difference between this and CGAL::left_vertex_2 is that this is inclusive of ALL points +Polygon::Vertex_const_circulator getLeftVertex(const Polygon& polygon) +{ + auto temp_vertex = polygon.vertices_circulator(); + auto left_vertex = temp_vertex; + do + { + temp_vertex++; + // if next vertex is more LEFT than previous left vertex OR + // (if next vertex is equally LEFT as previous left vertex AND next vertex has smaller Y) + if (temp_vertex->x() < left_vertex->x() || + ((temp_vertex->x() == left_vertex->x()) && (temp_vertex->y() < left_vertex->y()))) + { + left_vertex = temp_vertex; + } + } while (temp_vertex != --polygon.vertices_circulator()); + return left_vertex; +} + +std::vector getIntersectionPoints(const Polygon& polygon, const Line& line) +{ + std::vector intersections; + + for (auto edge_iterator = polygon.edges_begin(); edge_iterator != polygon.edges_end(); edge_iterator++) + { + Segment edge = *edge_iterator; + auto result = CGAL::intersection(edge, line); + + if (result) + { + auto edge_y_max = std::max(edge.source().y(), edge.target().y()); + auto edge_y_min = std::min(edge.source().y(), edge.target().y()); + + if (auto seg = boost::get(&*result)) + { + // do a sanity check to make sure the intersections are in the range they should be + if ((definitelyLessThan(seg->source().y(), edge_y_max + 0.1, EPSILON)) && + (definitelyGreaterThan(seg->source().y(), edge_y_min - 0.1, EPSILON)) && + (definitelyLessThan(seg->target().y(), edge_y_max + 0.1, EPSILON)) && + (definitelyGreaterThan(seg->target().y(), edge_y_min - 0.1, EPSILON))) + { + intersections.push_back(seg->source()); + intersections.push_back(seg->target()); + } + else + { + std::cout << "An intersection segment was significantly outside of the polygon! Removing..." << std::endl; + } + } + else + { + auto pt = boost::get(&*result); + + // do a sanity check to make sure the point is in the range it should be + if ((definitelyLessThan(pt->y(), edge_y_max + 0.1, EPSILON)) && + (definitelyGreaterThan(pt->y(), edge_y_min - 0.1, EPSILON))) + { + intersections.push_back(*pt); + } + else + { + std::cout << "An intersection point was significantly outside of the polygon! Removing..." << std::endl; + } + } + } + } + return intersections; +} + +std::vector::const_iterator findPointInVector(const Point& point, const std::vector& vector) +{ + // shouldn't use std::find because we need to use approximatelyEqual() because floating point math + auto it = vector.begin(); + for (; it != vector.end(); it++) + { + // ensure the x and y is within tolerances. If we found it, then break out of the loop. + if (approximatelyEqual(*it, point, EPSILON)) + { + break; + } + } + return it; +} + +// mainly to be used by CGAL Polygons, which don't have accessors to their Point vectors, but rather have iterators. +std::vector::const_iterator findPointInVector(const Point& point, const std::vector::iterator& begin, + const std::vector::iterator& end) +{ + // shouldn't use std::find because we need to use approximatelyEqual() because floating point math + auto it = begin; + for (; it != end; it++) + { + // ensure the x and y is within tolerances. If we found it, then break out of the loop. + if (approximatelyEqual(*it, point, EPSILON)) + { + break; + } + } + return it; +} + +void insertPointAlongEdge(const Point& point, Polygon& polygon) +{ + // check to see if point is a vertex + if (findPointInVector(point, polygon.vertices_begin(), polygon.vertices_end()) != polygon.vertices_end()) + { + // it was a vertex, we don't have to do anything + return; + } + + // the point intersects an edge, we need to find that edge + auto edge_iterator = polygon.edges_begin(); + for (; edge_iterator != polygon.edges_end(); edge_iterator++) + { + Segment edge = *edge_iterator; + // check the point's distance to the edge - it might be off by a little bit because of floating point error + if (definitelyLessThan(CGAL::squared_distance(point, edge), EPSILON, EPSILON)) + { + // we found the edge! + break; + } + } + + // check to see if we found the edge + if (edge_iterator == polygon.edges_end()) + { + std::cout << "couldn't find the edge that point " << point.x() << " , " << point.y() << " lies on!" << std::endl; + } + + // now, we need to modify the polygon such that we have two edges from source -> point and point -> target + // we can do this by simply inserting point after the edge's source + for (auto point_iterator = polygon.vertices_begin(); point_iterator != polygon.vertices_end(); point_iterator++) + { + if (approximatelyEqual(*point_iterator, edge_iterator->source(), EPSILON)) + { + // we found the source point in the points list! + // we'll insert the point -- this will invalidate the iterator but that's ok because we're done with it + point_iterator++; + polygon.insert(point_iterator, point); + return; + } + } + + // weird things happened if we got here + std::cout << "didn't find a point in polygon that matched source: " << edge_iterator->source().x() << " , " + << edge_iterator->source().y() << std::endl; +} + +/** + * Merges the two given polygons together, and returns a new polygon that is the two combined polygons + * Two polygons can be merged together if they share two adjacent vertices + * @param poly_1 + * @param poly_2 + * @returns the new merged polygon, or the other polygon if one polygon is empty + */ +Polygon mergePolygons(const Polygon& poly_1, const Polygon& poly_2) +{ + // given two polygons that are neighbors, merge them together by creating a new polygon that uses both polygon's + // points and neighbors + Polygon new_poly = Polygon(); + + // first, check to se eif either polygon is empty + if (poly_1.size() == 0) + { + new_poly = poly_2; + return new_poly; + } + else if (poly_2.size() == 0) + { + new_poly = poly_1; + return new_poly; + } + + // we need to find the shared vertices + Polygon::Vertex_const_circulator poly_1_circulator = poly_1.vertices_circulator(); + Polygon::Vertex_const_circulator poly_1_ending; + // loop to find a particular shared vertex + do + { + // check if this point in cell 1 is also in cell 2 + if (findPointInVector(*poly_1_circulator, poly_2.vertices_begin(), poly_2.vertices_end()) != poly_2.vertices_end()) + { + // if it is, then it's either the upper or lower intersection. We want whichever comes just after the other, ccw + if (findPointInVector(*(poly_1_circulator + 1), poly_2.vertices_begin(), poly_2.vertices_end()) != + poly_2.vertices_end()) + { + // the next vertex is the shared vertex immediately after the other shared vertex! + poly_1_circulator++; + poly_1_ending = poly_1_circulator; + break; + } + } + poly_1_circulator++; + + } while (poly_1_circulator != poly_1.vertices_circulator()); + + // add all of the points from poly_1_circulator to poly_1_circulator - 1 , inclusive, to new_poly + // this gives us all the points in polygon 1, ending on the leading edge of the boundary between the two polygons + do + { + new_poly.push_back(*poly_1_circulator); + poly_1_circulator++; + } while (!approximatelyEqual(*poly_1_circulator, *poly_1_ending, EPSILON)); + + // we've added all of the points in polygon 1 + + Polygon::Vertex_const_circulator poly_2_circulator = poly_2.vertices_circulator(); + // we need to get poly_2_circulator to point to the same point that we just added to new_poly + while (!approximatelyEqual(*poly_2_circulator, *(poly_1_circulator - 1), EPSILON)) + { + poly_2_circulator++; + } + + // now add all the points from poly_2_circulator to poly_1_circulator + 1, exclusive, to new_poly + poly_2_circulator++; + while (!approximatelyEqual(*poly_2_circulator, *poly_1_circulator, EPSILON)) + { + new_poly.push_back(*poly_2_circulator); + poly_2_circulator++; + } + + // this is the complete new poly! + return new_poly; +} + +bool approximatelyEqual(const Point a, const Point b, const double epsilon) +{ + return approximatelyEqual(a.x(), b.x(), epsilon) && approximatelyEqual(a.y(), b.y(), epsilon); +} + +bool approximatelyEqual(const double a, const double b, const double epsilon) +{ + return abs(a - b) <= ((abs(a) < abs(b) ? abs(b) : abs(a)) * epsilon); +} + +bool definitelyGreaterThan(const double a, const double b, const double epsilon) +{ + return (a - b) > ((abs(a) < abs(b) ? abs(b) : abs(a)) * epsilon); +} + +bool definitelyLessThan(const double a, const double b, const double epsilon) +{ + return (b - a) > ((abs(a) < abs(b) ? abs(b) : abs(a)) * epsilon); +} \ No newline at end of file diff --git a/boustrophedon_server/src/boustrophedon_server/outline_planner.cpp b/boustrophedon_server/src/boustrophedon_server/outline_planner.cpp index 65f9901..b821494 100644 --- a/boustrophedon_server/src/boustrophedon_server/outline_planner.cpp +++ b/boustrophedon_server/src/boustrophedon_server/outline_planner.cpp @@ -1,12 +1,12 @@ #include -#include +#include "boustrophedon_server/outline_planner.h" void OutlinePlanner::setParameters(OutlinePlanner::Parameters parameters) { params_ = parameters; } -void OutlinePlanner::addToPath(Polygon polygon, const Point& robot_position, std::vector& path, +bool OutlinePlanner::addToPath(Polygon polygon, const Point& robot_position, std::vector& path, Polygon& innermost_polygon) { shiftPolygonToStartNearRobot(polygon, robot_position); @@ -15,30 +15,77 @@ void OutlinePlanner::addToPath(Polygon polygon, const Point& robot_position, std { polygon.reverse_orientation(); } + innermost_polygon = polygon; - // Add outermost outline - if (params_.outline_layers >= 1) + if (params_.skip_outlines) { - for (const auto& point : polygon.container()) + std::cout << "Skipping outlining!" << std::endl; + return true; + } + + if (params_.repeat_boundary) + { + if (!addOutermostOutline(path, polygon)) { - path.emplace_back(PointType::Outline, point); + return false; } - path.emplace_back(PointType::Outline, polygon.container().front()); } - innermost_polygon = polygon; - for (auto layer = 1; layer < params_.outline_layers; layer++) + + for (auto layer = 1; layer <= params_.outline_layers; layer++) { auto offset_distance = layer * params_.stripe_separation; - auto inner_polygons = CGAL::create_interior_skeleton_and_offset_polygons_2(offset_distance, polygon); - assert(!inner_polygons.empty()); - for (const Point& p : inner_polygons.front()->container()) + if (!addInnerOutline(path, polygon, offset_distance, innermost_polygon)) { - path.emplace_back(PointType::Outline, p); + return false; } - // Close polygon by repeating first point - path.emplace_back(PointType::Outline, inner_polygons.front()->container().front()); - innermost_polygon = *inner_polygons.front(); } + + return true; +} + +bool OutlinePlanner::addOutermostOutline(std::vector& path, const Polygon& polygon) +{ + // Add outermost outline + for (const auto& point : polygon.container()) + { + path.emplace_back(PointType::Outline, point); + } + + if (params_.outline_clockwise) + { + // reverse the outline path to go clockwise instead, so that + // grass clippings go inside the boundary, not outside + std::reverse(++path.begin(), path.end()); + } + // Close polygon by repeating first point + path.emplace_back(PointType::Outline, polygon.container().front()); + return true; +} + +bool OutlinePlanner::addInnerOutline(std::vector& path, const Polygon& polygon, const double& offset, + Polygon& innermost_polygon) +{ + auto inner_polygons = CGAL::create_interior_skeleton_and_offset_polygons_2(offset, polygon); + if (inner_polygons.empty()) + { + return false; + } + for (const Point& p : inner_polygons.front()->container()) + { + path.emplace_back(PointType::Outline, p); + } + + if (params_.outline_clockwise) + { + // reverse the outline path to go clockwise instead, so that + // grass clippings go inside the boundary, not outside + std::reverse(path.end() - inner_polygons.size(), path.end()); + } + + // Close polygon by repeating first point + path.emplace_back(PointType::Outline, inner_polygons.front()->container().front()); + innermost_polygon = *inner_polygons.front(); + return true; } void OutlinePlanner::shiftPolygonToStartNearRobot(Polygon& polygon, const Point& robot_position) diff --git a/boustrophedon_server/src/boustrophedon_server/striping_planner.cpp b/boustrophedon_server/src/boustrophedon_server/striping_planner.cpp index 7051eaf..96f061e 100644 --- a/boustrophedon_server/src/boustrophedon_server/striping_planner.cpp +++ b/boustrophedon_server/src/boustrophedon_server/striping_planner.cpp @@ -1,97 +1,477 @@ #include #include -#include -#include +#include "boustrophedon_server/striping_planner.h" +#include "boustrophedon_server/cgal_utils.h" void StripingPlanner::setParameters(StripingPlanner::Parameters parameters) { params_ = parameters; } -void StripingPlanner::addToPath(const Polygon& polygon, const Point& robot_position, std::vector& path) +void StripingPlanner::addToPath(const Polygon& polygon, const Polygon& sub_polygon, Point& robot_position, + std::vector& path) { std::vector new_path_section; - const auto& last_point = path.empty() ? robot_position : path.back().point; + fillPolygon(sub_polygon, new_path_section, robot_position); - double left_vertex_dist = std::abs(last_point.x() - polygon.left_vertex()->x()); - double right_vertex_dist = std::abs(last_point.x() - polygon.right_vertex()->x()); - bool left_closer = left_vertex_dist < right_vertex_dist; - - if (!left_closer) + if (params_.travel_along_boundary) { - AffineTransform flip_transform(-1, 0, 0, 1); - transformPoints(polygon.vertices_begin(), polygon.vertices_end(), flip_transform); - } - - fillPolygon(polygon, new_path_section); + std::vector start_to_striping = + getOutlinePathToPoint(polygon, robot_position, new_path_section.front().point); - if (!left_closer) + path.insert(path.end(), start_to_striping.begin(), start_to_striping.end()); + path.insert(path.end(), new_path_section.begin(), new_path_section.end()); + } + else { - AffineTransform flip_transform(-1, 0, 0, 1); - transformNavPoints(new_path_section.begin(), new_path_section.end(), flip_transform); + path.insert(path.end(), new_path_section.begin(), new_path_section.end()); } + robot_position = new_path_section.back().point; +} - path.insert(path.end(), new_path_section.begin(), new_path_section.end()); +void StripingPlanner::addReturnToStart(const Polygon& polygon, const Point& start_position, const Point& robot_position, + std::vector& path) +{ + std::vector striping_to_start = getOutlinePathToPoint(polygon, robot_position, start_position); + path.insert(path.end(), striping_to_start.begin(), striping_to_start.end()); } -void StripingPlanner::fillPolygon(const Polygon& polygon, std::vector& path) +void StripingPlanner::fillPolygon(const Polygon& polygon, std::vector& path, const Point& robot_position) { - auto left_vertex = *CGAL::left_vertex_2(polygon.container().begin(), polygon.container().end()); + auto left_vertex = *getLeftVertex(polygon); auto min_x = left_vertex.x(); auto max_x = CGAL::right_vertex_2(polygon.container().begin(), polygon.container().end())->x(); auto stripe_count = static_cast(std::round(CGAL::to_double(max_x - min_x) / params_.stripe_separation)) + 1; - auto current_point = left_vertex; + StripingDirection stripe_dir = StripingDirection::STARTING; + bool left_closest = isLeftClosest(polygon, robot_position, min_x, max_x); for (auto stripe_num = 0; stripe_num < stripe_count; stripe_num++) { - auto x = (stripe_num * params_.stripe_separation) + min_x; + double x; + if (left_closest) + { + x = min_x + (stripe_num * params_.stripe_separation); + } + else + { + x = max_x - (stripe_num * params_.stripe_separation); + } auto intersections = getIntersectionPoints(polygon, Line(Point(x, 0.0), Point(x, 1.0))); if (intersections.size() > 1) { - auto comp_dist_to_curr_point = [current_point](const auto& a, const auto& b) { - auto comp = CGAL::compare_distance_to_point(current_point, a, b); + // lambda for determining the first striping direction naively + auto compare_current_point_distance = [&robot_position](const auto& a, const auto& b) { + auto comp = CGAL::compare_distance_to_point(robot_position, a, b); return (comp == CGAL::SMALLER); }; - std::sort(intersections.begin(), intersections.end(), comp_dist_to_curr_point); + // lambda for sorting points based on current striping direction + auto compare_striping_direction = [&stripe_dir](const auto& a, const auto& b) { + if (stripe_dir == StripingDirection::UP) + { + return a.y() < b.y(); + } + else if (stripe_dir == StripingDirection::DOWN) + { + return a.y() > b.y(); + } + else + { + std::cout << "Comparing based on STARTING striping direction!" << std::endl; + return true; + } + }; + + if (stripe_dir == StripingDirection::STARTING) + { + // figure out the first striping direction using naive comparison of the two points + std::sort(intersections.begin(), intersections.end(), compare_current_point_distance); + // is the first point lower y than the second? + stripe_dir = + intersections.front().y() < intersections.back().y() ? StripingDirection::UP : StripingDirection::DOWN; + } + // add in intermediaries, not sorted + addIntermediaryPoints(intersections); + + // remove any potential duplicates intersections.erase(std::unique(intersections.begin(), intersections.end()), intersections.end()); - path.emplace_back(PointType::StripeStart, intersections.front()); + // re-sort based on the striping direction + std::sort(intersections.begin(), intersections.end(), compare_striping_direction); + + // here's where we add in the behavior to get from the previous stripe to this stripe + if (params_.enable_half_y_turn) + { + // use the half-y-turn behavior + addHalfYTurnPoints(path, intersections.front(), stripe_dir); + + // the half-y-turn places its own stripe start points, so we don't want to place this one naively, unless there + // isn't anything in the path + if (path.empty()) + { + // we still need to place the start point in this case + path.emplace_back(PointType::StripeStart, intersections.front()); + } + } + else + { + // else use the simply boundary_following behavior + // follow the boundary to get to the next point + addBoundaryFollowingPoints(path, intersections.front(), polygon); + + // place the points into the path + path.emplace_back(PointType::StripeStart, intersections.front()); + } + + for (auto it = intersections.begin() + 1; it < intersections.end() - 1; it++) + { + path.emplace_back(PointType::StripeIntermediate, *it); + } path.emplace_back(PointType::StripeEnd, intersections.back()); - current_point = intersections.back(); + + // reverse the striping direction for the next stripe + stripe_dir = stripe_dir == StripingDirection::UP ? StripingDirection::DOWN : StripingDirection::UP; } } } -std::vector StripingPlanner::getIntersectionPoints(const Polygon& polygon, const Line& line) +// adds in waypoints between the two waypoints provided in the intersections vector +void StripingPlanner::addIntermediaryPoints(std::vector& intersections) +{ + Point lower_point = intersections.front(); + Point upper_point = intersections.back(); + double x = lower_point.x(); // this is the x coordinate of all points on this striping line + double y = lower_point.y(); // this is the base y coordinate + + int intermediary_count = + static_cast(std::trunc((upper_point.y() - lower_point.y()) / params_.intermediary_separation)); + + // start the loop at 1. We don't want to repeat a point on the ends of the line segment. + for (int i = 1; i < intermediary_count; i++) + { + double new_y = i * params_.intermediary_separation + y; + intersections.emplace_back(x, new_y); + } +} + +void StripingPlanner::addBoundaryFollowingPoints(std::vector& path, const Point& next_stripe_start, + Polygon polygon) +{ + // sanity check, don't add a boundary following point if there is no previous stripe + if (path.empty()) + { + return; + } + + // first, insert the last point of the path (the end of last stripe) and the start of the next stripe to the polygon + insertPointAlongEdge(path.back().point, polygon); + insertPointAlongEdge(next_stripe_start, polygon); + + // we now have a polygon with all necessary points to find a path between the stripes + std::vector boundary_path = getOutlinePathToPoint(polygon, path.back().point, next_stripe_start); + + // check if there's actually a new path we have to follow, instead of just going straight from end to start + if (boundary_path.size() > 2) + { + // if so, add it to the path. the end is already in there, and the start will be added later + path.insert(path.end(), boundary_path.begin() + 1, boundary_path.end() - 1); + + std::cout << "inserted an extra boundary following path of size: " << boundary_path.size() - 2 << std::endl; + } +} + +void StripingPlanner::addHalfYTurnPoints(std::vector& path, const Point& next_stripe_start, + StripingDirection& stripe_dir) { - std::vector intersections; + // sanity check, don't add a boundary following point if there is no previous stripe + if (path.empty()) + { + return; + } - for (auto edge_iterator = polygon.edges_begin(); edge_iterator != polygon.edges_end(); edge_iterator++) + Point arc_center_point; + std::vector arc; + + // we'll need to do different behaviors based on which direction we are striping in + // the key differences are whether we are striping left-to-right or right-to-left, and whether we are going up or down + if (stripe_dir == StripingDirection::DOWN && path.back().point.x() < next_stripe_start.x()) { - Segment edge = *edge_iterator; + // we are going left to right + // the current stripe is going down, so we need an arc from an UP stripe to a DOWN stripe - auto result = CGAL::intersection(edge, line); + // figure out which one of path.back() or next_stripe_start is higher y-coordinate + if (definitelyGreaterThan(path.back().point.y(), next_stripe_start.y(), EPSILON)) + { + // the previous stripe's end is higher than the current stripe's start. - if (result) + // the arc is centered around a point in the previous stripe + arc_center_point = + Point(path.back().point.x(), path.back().point.y() + params_.turn_start_offset - params_.stripe_separation); + + // For this case, we'll use a circular arc, starting from PI/2 and ending at 0 (left to right, higher to lower) + arc = generateDiscretizedArc(arc_center_point, params_.stripe_separation, CGAL_PI / 2, 0.0, + params_.points_per_turn); + + // the first point in the arc is the zero-turn point, make it end the stripe + arc.front().type = PointType::StripeEnd; + // copy the first point in the arc to start the next stripe + arc.insert(arc.begin() + 1, NavPoint{ PointType::StripeStart, arc.front().point }); + } + else { - if (auto seg = boost::get(&*result)) - { - intersections.push_back(seg->source()); - intersections.push_back(seg->target()); - } - else - { - auto pt = boost::get(&*result); - intersections.push_back(*pt); - } + // the previous stripe's end is lower or equal(ish) to the current stripe's start. + + // the arc is centered around a point in the current stripe + arc_center_point = + Point(next_stripe_start.x(), next_stripe_start.y() + params_.turn_start_offset - params_.stripe_separation); + + // For this case, we'll use a circular arc, starting from PI and ending at PI/2 (left to right, lower to higher) + arc = generateDiscretizedArc(arc_center_point, params_.stripe_separation, CGAL_PI, CGAL_PI / 2, + params_.points_per_turn); + + // the last point in the arc is the zero-turn point, make it end the stripe + arc.back().type = PointType::StripeEnd; + // copy the last point in the arc to start the next stripe + arc.insert(arc.end(), NavPoint{ PointType::StripeStart, arc.back().point }); + } + } + else if (stripe_dir == StripingDirection::UP && path.back().point.x() < next_stripe_start.x()) + { + // we are going left to right + // the current stripe is going up, so we need an arc from a DOWN stripe to an UP stripe. + + // figure out which one of path.back() or next_stripe_start is higher y-coordinate + if (definitelyLessThan(path.back().point.y(), next_stripe_start.y(), EPSILON)) + { + // the previous stripe's end is lower than the current stripe's start. + + // the arc is centered around a point in the previous stripe + arc_center_point = + Point(path.back().point.x(), path.back().point.y() - params_.turn_start_offset + params_.stripe_separation); + + // For this case, we'll use a circular arc, starting from 3PI/2 and ending at 2PI (left to right, lower to higher) + arc = generateDiscretizedArc(arc_center_point, params_.stripe_separation, 3 * CGAL_PI / 2, CGAL_PI * 2, + params_.points_per_turn); + + // the first point in the arc is the zero-turn point, make it end the stripe + arc.front().type = PointType::StripeEnd; + // copy the first point in the arc to start the next stripe + arc.insert(arc.begin() + 1, NavPoint{ PointType::StripeStart, arc.front().point }); + } + else + { + // the previous stripe's end is higher or equal(ish) to the current stripe's start. + + // the arc is centered around a point in the current stripe + arc_center_point = + Point(next_stripe_start.x(), next_stripe_start.y() - params_.turn_start_offset + params_.stripe_separation); + + // For this case, we'll use a circular arc, starting from PI and ending at 3PI/2 (left to right, higher to lower) + arc = generateDiscretizedArc(arc_center_point, params_.stripe_separation, CGAL_PI, 3 * CGAL_PI / 2, + params_.points_per_turn); + + // the last point in the arc is the zero-turn point, make it end the stripe + arc.back().type = PointType::StripeEnd; + // copy the last point in the arc to start the next stripe + arc.insert(arc.end(), NavPoint{ PointType::StripeStart, arc.back().point }); + } + } + else if (stripe_dir == StripingDirection::DOWN && path.back().point.x() > next_stripe_start.x()) + { + // we are going right to left + // the current stripe is going down, so we need an arc from an UP stripe to a DOWN stripe + + // figure out which one of path.back() or next_stripe_start is higher y-coordinate + if (definitelyGreaterThan(path.back().point.y(), next_stripe_start.y(), EPSILON)) + { + // the previous stripe's end is higher than the current stripe's start. + + // the arc is centered around a point in the previous stripe + arc_center_point = + Point(path.back().point.x(), path.back().point.y() + params_.turn_start_offset - params_.stripe_separation); + + // For this case, we'll use a circular arc, starting from PI/2 and ending at PI (right to left, higher to lower) + arc = generateDiscretizedArc(arc_center_point, params_.stripe_separation, CGAL_PI / 2, CGAL_PI, + params_.points_per_turn); + + // the first point in the arc is the zero-turn point, make it end the stripe + arc.front().type = PointType::StripeEnd; + // copy the first point in the arc to start the next stripe + arc.insert(arc.begin() + 1, NavPoint{ PointType::StripeStart, arc.front().point }); } + else + { + // the previous stripe's end is lower or equal(ish) to the current stripe's start. + + // the arc is centered around a point in the current stripe + arc_center_point = + Point(next_stripe_start.x(), next_stripe_start.y() + params_.turn_start_offset - params_.stripe_separation); + + // For this case, we'll use a circular arc, starting from 0 and ending at PI/2 (right to left, lower to higher) + arc = generateDiscretizedArc(arc_center_point, params_.stripe_separation, 0.0, CGAL_PI / 2, + params_.points_per_turn); + + // the last point in the arc is the zero-turn point, make it end the stripe + arc.back().type = PointType::StripeEnd; + // copy the last point in the arc to start the next stripe + arc.insert(arc.end(), NavPoint{ PointType::StripeStart, arc.back().point }); + } + } + else if (stripe_dir == StripingDirection::UP && path.back().point.x() > next_stripe_start.x()) + { + // we are going right to left + // the current stripe is going up, so we need an arc from a DOWN stripe to an UP stripe. + + // figure out which one of path.back() or next_stripe_start is higher y-coordinate + if (definitelyLessThan(path.back().point.y(), next_stripe_start.y(), EPSILON)) + { + // the previous stripe's end is lower than the current stripe's start. + + // the arc is centered around a point in the previous stripe + arc_center_point = + Point(path.back().point.x(), path.back().point.y() - params_.turn_start_offset + params_.stripe_separation); + + // For this case, we'll use a circular arc, starting from 3PI/2 and ending at PI (right to left, lower to higher) + arc = generateDiscretizedArc(arc_center_point, params_.stripe_separation, 3 * CGAL_PI / 2, CGAL_PI, + params_.points_per_turn); + + // the first point in the arc is the zero-turn point, make it end the stripe + arc.front().type = PointType::StripeEnd; + // copy the first point in the arc to start the next stripe + arc.insert(arc.begin() + 1, NavPoint{ PointType::StripeStart, arc.front().point }); + } + else + { + // the previous stripe's end is higher or equal(ish) to the current stripe's start. + + // the arc is centered around a point in the current stripe + arc_center_point = + Point(next_stripe_start.x(), next_stripe_start.y() - params_.turn_start_offset + params_.stripe_separation); + + // For this case, we'll use a circular arc, starting from 2PI and ending at 3PI/2 (right to left, higher to lower) + arc = generateDiscretizedArc(arc_center_point, params_.stripe_separation, CGAL_PI * 2, 3 * CGAL_PI / 2, + params_.points_per_turn); + + // the last point in the arc is the zero-turn point, make it end the stripe + arc.back().type = PointType::StripeEnd; + // copy the last point in the arc to start the next stripe + arc.insert(arc.end(), NavPoint{ PointType::StripeStart, arc.back().point }); + } + } + else + { + // we shouldn't be able to get here + std::cout << "addHalfYTurnPoints(): stripe_dir was neither DOWN nor UP!" << std::endl; + return; + } + // remove the previous stripe end point + path.erase(path.end()); + + // insert the arc points onto the end of the path + path.insert(path.end(), arc.begin(), arc.end()); +} + +std::vector StripingPlanner::generateDiscretizedArc(const Point& center_point, const float& radius, + const float& start_rad, const float& end_rad, + const int& num_points) +{ + std::vector points; + + float radian_interval = (end_rad - start_rad) / (num_points - 1); + + for (int i = 0; i < num_points; i++) + { + float current_radian = start_rad + (radian_interval * i); + Point point = + Point(center_point.x() + (radius * cos(current_radian)), center_point.y() + (radius * sin(current_radian))); + points.emplace_back(PointType::StripeIntermediate, point); + } + + return points; +} + +bool StripingPlanner::isLeftClosest(const Polygon& polygon, const Point& robot_position, double& min_x, double& max_x) +{ + // lambda for determining the first striping direction naively + auto compare_current_point_distance = [&robot_position](const auto& a, const auto& b) { + auto comp = CGAL::compare_distance_to_point(robot_position, a, b); + return (comp == CGAL::SMALLER); + }; + + std::vector starting_points = getIntersectionPoints(polygon, Line(Point(min_x, 0.0), Point(min_x, 1.0))); + std::vector right_points = getIntersectionPoints(polygon, Line(Point(max_x, 0.0), Point(max_x, 1.0))); + starting_points.insert(starting_points.end(), right_points.begin(), right_points.end()); + std::sort(starting_points.begin(), starting_points.end(), compare_current_point_distance); + + // if the closest potential starting point is on the left, return true. If not, return false. + return starting_points.front().x() == min_x; +} + +std::vector StripingPlanner::getOutlinePathToPoint(const Polygon& polygon, const Point& start_point, + const Point& end_point) +{ + NeighborSearch::Tree tree(polygon.vertices_begin(), polygon.vertices_end()); // Initialize the search tree + + NeighborSearch start_search(tree, start_point, 1); // find the 1 nearest point to start_point + Point polygon_start_point = start_search.begin()->first; + + NeighborSearch end_search(tree, end_point, 1); // find the 1 nearest point to end_point + Point polygon_end_point = end_search.begin()->first; + + // now we need to figure out the shortest path along the polygon from polygon_start_point to polygon_end_point + // we only need to try two paths: one going clockwise and one going counter-clockwise + std::vector clockwise_path = getPolygonPath(polygon, polygon_start_point, polygon_end_point); + Polygon reversed_polygon = polygon; + reversed_polygon.reverse_orientation(); + std::vector counter_clockwise_path = + getPolygonPath(reversed_polygon, polygon_start_point, polygon_end_point); + if (getPathLength(clockwise_path) < getPathLength(counter_clockwise_path)) + { + return clockwise_path; + } + else + { + return counter_clockwise_path; + } +} + +std::vector StripingPlanner::getPolygonPath(const Polygon& polygon, const Point& start_point, + const Point& end_point) +{ + // get a circulator starting at start_point + Polygon::Vertex_const_circulator start_circulator = polygon.vertices_circulator(); + + while (*start_circulator != start_point) // loop until the circulator is pointing at the start + { + start_circulator++; + } + // start with the start_point + std::vector path; + path.emplace_back(PointType::Outline, *start_circulator); + + // now we can add points to the path + while (*start_circulator++ != end_point) + { + path.emplace_back(PointType::Outline, *start_circulator); + } + + return path; +} + +float StripingPlanner::getPathLength(const std::vector& path) +{ + float squared_length = 0.0; + for (auto it = path.begin(); it != --path.end(); it++) + { + Point point_1 = it->point; + Point point_2 = (it + 1)->point; + squared_length += CGAL::sqrt(CGAL::squared_distance(point_1, point_2)); } - return intersections; + return squared_length; }