diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..3069f94 --- /dev/null +++ b/.clang-format @@ -0,0 +1,65 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} +... diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..4f03f0e --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,60 @@ +--- +Checks: '*, + -llvm-header-guard, + -google-build-using-namespace, + -clang-analyzer-alpha.clone.CloneChecker, + -google-runtime-int, + -cppcoreguidelines-pro-bounds-array-to-pointer-decay, + -cppcoreguidelines-pro-bounds-constant-array-index, + -clang-analyzer-alpha.deadcode.UnreachableCode, + -misc-use-after-move, + -cppcoreguidelines-pro-type-vararg, + -modernize-use-emplace, + -android-*, + -absel-*, + -cert-*, + -objc-*, + -zircon-*, + -hicpp-vararg, + -hicpp-no-array-decay, + -google-runtime-references, + -llvm-include-order, + -fuchsia-*, + -misc-unused-parameters' + +WarningsAsErrors: '*' +HeaderFilterRegex: '' +AnalyzeTemporaryDtors: false +CheckOptions: + - key: llvm-namespace-comment.ShortNamespaceLines + value: '10' + - key: llvm-namespace-comment.SpacesBeforeComments + value: '2' + - key: readability-braces-around-statements.ShortStatementLines + value: '2' + # type names + - key: readability-identifier-naming.ClassCase + value: CamelCase + - key: readability-identifier-naming.EnumCase + value: CamelCase + - key: readability-identifier-naming.UnionCase + value: CamelCase + # method names are camelCase + - key: readability-identifier-naming.MethodCase + value: camelBack + # variable names are snake_case + - key: readability-identifier-naming.VariableCase + value: lower_case + - key: readability-identifier-naming.ParameterCase + value: lower_case + - key: readability-identifier-naming.ClassMemberSuffix + value: '_' + # global variables are g_snake_case + - key: readability-identifier-naming.GlobalVariablePrefix + value: g_ + - key: readability-identifier-naming.GlobalVariableCase + value: lower_case + # namespaces are snake_case + - key: readability-identifier-naming.NamespaceCase + value: lower_case +... diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e69de29 diff --git a/README.md b/README.md new file mode 100644 index 0000000..64435ab --- /dev/null +++ b/README.md @@ -0,0 +1,6 @@ +# Boustrophedon Planner +Boustrophedon Planner is a coverage path planner that implements the [Boustrophedon Cell Decomposition](https://en.wikipedia.org/wiki/Boustrophedon_cell_decomposition) algorithm. + +## Overview +The path planner is an actionlib server that takes in a `geometry_msgs/PolygonStamped` and a `geometry_msgs/PoseStamped`, +and returns a `StripingPlan` message which contains a list of waypoints to stripe the passed in polygon. diff --git a/boustrophedon_msgs/CMakeLists.txt b/boustrophedon_msgs/CMakeLists.txt new file mode 100644 index 0000000..1617b5f --- /dev/null +++ b/boustrophedon_msgs/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 2.8.3) +project(boustrophedon_msgs) + +set(CMAKE_CXX_STANDARD 14) + +find_package(catkin REQUIRED + genmsg + std_msgs + geometry_msgs + actionlib_msgs + nav_msgs + ) + +add_message_files( + DIRECTORY msg + FILES + StripingPoint.msg + StripingPlan.msg +) + +add_service_files( + FILES + ConvertPlanToPath.srv +) + +add_action_files( + DIRECTORY action + FILES + PlanMowingPath.action +) + +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs + actionlib_msgs + nav_msgs +) + +catkin_package() diff --git a/boustrophedon_msgs/action/PlanMowingPath.action b/boustrophedon_msgs/action/PlanMowingPath.action new file mode 100644 index 0000000..2f17b89 --- /dev/null +++ b/boustrophedon_msgs/action/PlanMowingPath.action @@ -0,0 +1,5 @@ +geometry_msgs/PolygonStamped property +geometry_msgs/PoseStamped robot_position +--- +boustrophedon_msgs/StripingPlan plan +--- diff --git a/boustrophedon_msgs/msg/StripingPlan.msg b/boustrophedon_msgs/msg/StripingPlan.msg new file mode 100644 index 0000000..afca5de --- /dev/null +++ b/boustrophedon_msgs/msg/StripingPlan.msg @@ -0,0 +1,2 @@ +Header header +boustrophedon_msgs/StripingPoint[] points diff --git a/boustrophedon_msgs/msg/StripingPoint.msg b/boustrophedon_msgs/msg/StripingPoint.msg new file mode 100644 index 0000000..8dc90a5 --- /dev/null +++ b/boustrophedon_msgs/msg/StripingPoint.msg @@ -0,0 +1,6 @@ +geometry_msgs/Point point +uint8 type + +uint8 OUTLINE=0 +uint8 STRIPE_START=1 +uint8 STRIPE_END=2 diff --git a/boustrophedon_msgs/package.xml b/boustrophedon_msgs/package.xml new file mode 100644 index 0000000..6ecd03f --- /dev/null +++ b/boustrophedon_msgs/package.xml @@ -0,0 +1,25 @@ + + + boustrophedon_msgs + 0.0.1 + Custom messages for the Boustrophedon Planner + + Charles Brian Quinn + Oswin So + + TODO + + Charles Brian Quinn + Oswin So + + actionlib_msgs + nav_msgs + geometry_msgs + + message_generation + + catkin + + + + diff --git a/boustrophedon_msgs/srv/ConvertPlanToPath.srv b/boustrophedon_msgs/srv/ConvertPlanToPath.srv new file mode 100644 index 0000000..2ae8192 --- /dev/null +++ b/boustrophedon_msgs/srv/ConvertPlanToPath.srv @@ -0,0 +1,3 @@ +boustrophedon_msgs/StripingPlan plan +--- +nav_msgs/Path path diff --git a/boustrophedon_server/CMakeLists.txt b/boustrophedon_server/CMakeLists.txt new file mode 100644 index 0000000..6e6ca9b --- /dev/null +++ b/boustrophedon_server/CMakeLists.txt @@ -0,0 +1,61 @@ +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 + boustrophedon_msgs + actionlib + tf + geometry_msgs + nav_msgs + rosparam_shortcuts + ) + +find_package(CGAL REQUIRED COMPONENTS Core) +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS + roscpp + boustrophedon_msgs + actionlib + tf + geometry_msgs + nav_msgs + rosparam_shortcuts +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_executable(boustrophedon_planner_server + src/boustrophedon_server/boustrophedon_planner_node.cpp + src/boustrophedon_server/boustrophedon_planner_server.cpp + src/boustrophedon_server/striping_planner.cpp + src/boustrophedon_server/outline_planner.cpp + src/boustrophedon_server/cgal_utils.cpp + src/boustrophedon_server/boustrophedon_types.cpp + src/boustrophedon_server/cellular_decomposition/polygon_decomposer.cpp + src/boustrophedon_server/cellular_decomposition/cell.cpp + ) +target_link_libraries(boustrophedon_planner_server + ${catkin_LIBRARIES} + CGAL::CGAL + CGAL::CGAL_Core) +add_dependencies(boustrophedon_planner_server ${catkin_EXPORTED_TARGETS}) + +install( + TARGETS + boustrophedon_planner_server + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + ) diff --git a/boustrophedon_server/include/boustrophedon_server/boustrophedon_planner_server.h b/boustrophedon_server/include/boustrophedon_server/boustrophedon_planner_server.h new file mode 100644 index 0000000..9a073e5 --- /dev/null +++ b/boustrophedon_server/include/boustrophedon_server/boustrophedon_planner_server.h @@ -0,0 +1,46 @@ +#ifndef SRC_BOUSTROPHEDON_PLANNER_SERVER_H +#define SRC_BOUSTROPHEDON_PLANNER_SERVER_H + +#include +#include +#include + +#include +#include + +#include "cgal_utils.h" +#include "striping_planner.h" +#include "outline_planner.h" +#include "cellular_decomposition/polygon_decomposer.h" + +class BoustrophedonPlannerServer +{ +public: + BoustrophedonPlannerServer(); + + void executePlanPathAction(const boustrophedon_msgs::PlanMowingPathGoalConstPtr& goal); + +private: + using Server = actionlib::SimpleActionServer; + + ros::NodeHandle node_handle_; + ros::NodeHandle private_node_handle_; + Server action_server_; + ros::ServiceServer conversion_server_; + + StripingPlanner striping_planner_; + OutlinePlanner outline_planner_; + + int outline_layer_count_{}; + double stripe_separation_{}; + double stripe_angle_{}; + tf::TransformListener transform_listener_{}; + + 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; +}; + +#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 new file mode 100644 index 0000000..f5188f7 --- /dev/null +++ b/boustrophedon_server/include/boustrophedon_server/boustrophedon_types.h @@ -0,0 +1,22 @@ +#ifndef SRC_BOUSTROPHEDON_TYPES_H +#define SRC_BOUSTROPHEDON_TYPES_H + +#include "cgal_types.h" + +enum class PointType +{ + Outline = 0, + StripeStart = 1, + StripeEnd = 2, + Travel = 3 +}; + +struct NavPoint +{ + PointType type{}; + Point point{}; + + NavPoint(PointType type, Point point); +}; + +#endif // SRC_BOUSTROPHEDON_TYPES_H diff --git a/boustrophedon_server/include/boustrophedon_server/cellular_decomposition/cell.h b/boustrophedon_server/include/boustrophedon_server/cellular_decomposition/cell.h new file mode 100644 index 0000000..444e79f --- /dev/null +++ b/boustrophedon_server/include/boustrophedon_server/cellular_decomposition/cell.h @@ -0,0 +1,74 @@ +#ifndef SRC_CELL_H +#define SRC_CELL_H + +#include +#include +#include "../cgal_types.h" + +namespace bcd +{ +using Circulator = Polygon::Vertex_const_circulator; +enum class Event +{ + Start, + Ceil, + Floor, + End, + Open, + Close, +}; + +struct Vertex +{ + Vertex() = default; + Vertex(Event type, Circulator point); + bool isAdjacentTo(const Vertex& other) const; + Circulator nextPoint() const; + Circulator previousPoint() const; + + Event type; + Circulator point; +}; + +struct Cell +{ + Cell() = default; + Cell(Vertex start); + bool connectsToFloor(const Vertex& other) const; + bool connectsToCeil(const Vertex& other) const; + bool enclosesVector(const Vertex& other) const; + + const Vertex& currentCeil() const; + const Vertex& currentFloor() const; + + const Point nextCeilPoint() const; + const Point nextFloorPoint() const; + + std::vector toPoints() const; + Polygon toPolygon() const; + + Vertex start; + std::vector ceiling_points; + std::vector floor_points; + Vertex end; + + std::vector neighbors; +}; +} // 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 new file mode 100644 index 0000000..4a289f2 --- /dev/null +++ b/boustrophedon_server/include/boustrophedon_server/cellular_decomposition/polygon_decomposer.h @@ -0,0 +1,79 @@ +#ifndef SRC_POLYGON_DECOMPOSER_H +#define SRC_POLYGON_DECOMPOSER_H + +#include "../cgal_types.h" +#include "cell.h" + +class PolygonDecomposer +{ +public: + void decompose(Polygon& polygon); + std::vector getSubPolygons(const Point& position) const; + +private: + std::vector> cells_; + std::vector vertices_; + + /** + * 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 + * @param polygon + */ + void createVertices(Polygon& polygon); + + /** + * Finds Open and close events. For each event, it finds the intersection vertices of the + * slice and inserts them into the polygon. + * @param polygon + */ + void insertOpenCloseVertices(Polygon& polygon); + + /** + * Inserts all intersections of the slice with the polygon into the polygon. + * @param polygon + * @param point + */ + static void sliceAndInsertIntoPolygon(Polygon& polygon, const Point& point); + + /** + * + */ + void createCells(); + + bcd::Cell* getClosestCell(const Point& position) const; + + std::vector visitCells(const bcd::Cell* starting_cell) const; + + std::vector toPolygons(std::vector cells) const; + + /** + * Dispatcher for the individual event types. + * @param current_vertex + * @param current_cells + * @return + */ + bool handleVertex(bcd::Vertex& current_vertex, std::vector>& current_cells); + + 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); + + 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; + + /** + * Sorts the vertices first by x coordinate, then by type. + */ + void sortVertices(); + bcd::Event getEvent(const bcd::Circulator& vertex, bcd::Event last_event) const; +}; + +#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 new file mode 100644 index 0000000..c77e416 --- /dev/null +++ b/boustrophedon_server/include/boustrophedon_server/cgal_types.h @@ -0,0 +1,16 @@ +#ifndef SRC_CGAL_TYPES_H +#define SRC_CGAL_TYPES_H + +#include +#include +#include + +using Kernel = CGAL::Cartesian; +using Polygon = CGAL::Polygon_2; +using Point = Kernel::Point_2; +using Traits = CGAL::Arr_linear_traits_2; +using Segment = Traits::Segment_2; +using Line = Traits::Line_2; +using AffineTransform = CGAL::Aff_transformation_2; + +#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 new file mode 100644 index 0000000..4b47547 --- /dev/null +++ b/boustrophedon_server/include/boustrophedon_server/cgal_utils.h @@ -0,0 +1,25 @@ +#ifndef SRC_CGAL_UTILS_H +#define SRC_CGAL_UTILS_H + +#include "cgal_types.h" +#include "boustrophedon_types.h" + +AffineTransform preprocessPolygon(Polygon& polygon, const Point& robot_position, const double& striping_angle); + +void postprocessPolygonAndPath(const AffineTransform& preprocess_transform, Polygon& polygon, + std::vector& path); + +template +static void transformPoints(const Iterator_t& first, const Iterator_t& last, + const CGAL::Aff_transformation_2& transform) +{ + for (auto current = first; current != last; current++) + { + *current = transform(*current); + } +} + +void transformNavPoints(const std::vector::iterator& first, const std::vector::iterator& last, + const CGAL::Aff_transformation_2& transform); + +#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 new file mode 100644 index 0000000..d96bb16 --- /dev/null +++ b/boustrophedon_server/include/boustrophedon_server/outline_planner.h @@ -0,0 +1,32 @@ +#ifndef SRC_OUTLINE_PLANNER_H +#define SRC_OUTLINE_PLANNER_H + +#include "cgal_types.h" +#include "boustrophedon_types.h" + +class OutlinePlanner +{ +public: + struct Parameters + { + /** The number of passes to make in the outline portion + */ + int outline_layers = 3; + /** Center-to-center distance (in meters) between passes in the coverage path. + * + * Applies to both outline and fill portions. + */ + double stripe_separation = 0.5; + }; + + void setParameters(Parameters parameters); + + void addToPath(Polygon polygon, const Point& robot_position, std::vector& path, Polygon& innermost_polygon); + +private: + Parameters params_; + + void shiftPolygonToStartNearRobot(Polygon& polygon, const Point& robot_position); +}; + +#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 new file mode 100644 index 0000000..959a3ba --- /dev/null +++ b/boustrophedon_server/include/boustrophedon_server/striping_planner.h @@ -0,0 +1,35 @@ +#ifndef SRC_BOUSTROPHEDONPLANNER_H +#define SRC_BOUSTROPHEDONPLANNER_H + +#include "cgal_types.h" +#include "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; + }; + + void setParameters(Parameters parameters); + + void addToPath(const Polygon& polygon, const Point& robot_position, std::vector& path); + +private: + 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); + + std::vector getIntersectionPoints(const Polygon& polygon, const Line& line); +}; + +#endif // SRC_BOUSTROPHEDONPLANNER_H diff --git a/boustrophedon_server/launch/boustrophedon_server.launch b/boustrophedon_server/launch/boustrophedon_server.launch new file mode 100644 index 0000000..ff7d76a --- /dev/null +++ b/boustrophedon_server/launch/boustrophedon_server.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/boustrophedon_server/package.xml b/boustrophedon_server/package.xml new file mode 100644 index 0000000..0003717 --- /dev/null +++ b/boustrophedon_server/package.xml @@ -0,0 +1,32 @@ + + + boustrophedon_server + 0.0.1 + Actionlib server for the Boustrophedon Planner + + Charles Brian Quinn + Oswin So + + + TODO + + + Charles Brian Quinn + Oswin So + + roscpp + boustrophedon_msgs + geometry_msgs + nav_msgs + actionlib + tf + rosparam_shortcuts + + cgal + + rosunit + + catkin + + + diff --git a/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_node.cpp b/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_node.cpp new file mode 100644 index 0000000..fb3b835 --- /dev/null +++ b/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_node.cpp @@ -0,0 +1,13 @@ +#include +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "boustrophedon_planner_node"); + + BoustrophedonPlannerServer server; + + ros::spin(); + + return 0; +} diff --git a/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_server.cpp b/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_server.cpp new file mode 100644 index 0000000..2b5e554 --- /dev/null +++ b/boustrophedon_server/src/boustrophedon_server/boustrophedon_planner_server.cpp @@ -0,0 +1,123 @@ +#include + +#include + +BoustrophedonPlannerServer::BoustrophedonPlannerServer() + : private_node_handle_("~") + , action_server_(node_handle_, "plan_path", boost::bind(&BoustrophedonPlannerServer::executePlanPathAction, this, _1), + false) + , conversion_server_{ node_handle_.advertiseService("convert_striping_plan_to_path", + &BoustrophedonPlannerServer::convertStripingPlanToPath, this) } +{ + std::size_t error = 0; + 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_, "stripe_angle", stripe_angle_)); + rosparam_shortcuts::shutdownIfError("plan_path", error); + + striping_planner_.setParameters({ stripe_separation_ }); + outline_planner_.setParameters({ outline_layer_count_, stripe_separation_ }); + + action_server_.start(); +} + +void BoustrophedonPlannerServer::executePlanPathAction(const boustrophedon_msgs::PlanMowingPathGoalConstPtr& goal) +{ + std::string boundary_frame = goal->property.header.frame_id; + Polygon polygon = fromBoundary(goal->property); + + Point robot_position; + try + { + robot_position = fromPositionWithFrame(goal->robot_position, goal->property.header.frame_id); + } + catch (const tf::TransformException& ex) + { + action_server_.setAborted(Server::Result(), + 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); + + PolygonDecomposer polygon_decomposer{}; + polygon_decomposer.decompose(fill_polygon); + std::vector sub_polygons = polygon_decomposer.getSubPolygons(robot_position); + + for (const auto& subpoly : sub_polygons) + { + striping_planner_.addToPath(subpoly, robot_position, path); + } + + postprocessPolygonAndPath(preprocess_transform, polygon, path); + + 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 +{ + boustrophedon_msgs::PlanMowingPathResult result; + result.plan.points.reserve(path.size()); + result.plan.header.stamp = ros::Time::now(); + result.plan.header.frame_id = frame; + + for (const auto& point : path) + { + boustrophedon_msgs::StripingPoint stripe_point{}; + stripe_point.point.x = point.point.x(); + stripe_point.point.y = point.point.y(); + stripe_point.point.z = 0; + stripe_point.type = static_cast(point.type); + result.plan.points.emplace_back(stripe_point); + } + return result; +} + +Polygon BoustrophedonPlannerServer::fromBoundary(const geometry_msgs::PolygonStamped& boundary) const +{ + Polygon polygon; + for (const auto& point : boundary.polygon.points) + { + polygon.push_back(Point(point.x, point.y)); + } + return polygon; +} + +Point BoustrophedonPlannerServer::fromPositionWithFrame(const geometry_msgs::PoseStamped& pose, + const std::string& target_frame) const +{ + geometry_msgs::PoseStamped transformed_pose; + transform_listener_.transformPose(target_frame, pose, transformed_pose); + return { transformed_pose.pose.position.x, transformed_pose.pose.position.y }; +} + +bool BoustrophedonPlannerServer::convertStripingPlanToPath(boustrophedon_msgs::ConvertPlanToPath::Request& request, + boustrophedon_msgs::ConvertPlanToPath::Response& response) +{ + response.path.header.frame_id = request.plan.header.frame_id; + response.path.header.stamp = request.plan.header.stamp; + + std::transform(request.plan.points.begin(), request.plan.points.end(), response.path.poses.begin(), + [&](const boustrophedon_msgs::StripingPoint& point) { + geometry_msgs::PoseStamped pose; + pose.header.frame_id = request.plan.header.frame_id; + pose.header.stamp = request.plan.header.stamp; + pose.pose.position = point.point; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + return pose; + }); + return true; +} diff --git a/boustrophedon_server/src/boustrophedon_server/boustrophedon_types.cpp b/boustrophedon_server/src/boustrophedon_server/boustrophedon_types.cpp new file mode 100644 index 0000000..3886422 --- /dev/null +++ b/boustrophedon_server/src/boustrophedon_server/boustrophedon_types.cpp @@ -0,0 +1,5 @@ +#include + +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 new file mode 100644 index 0000000..1cbdbc8 --- /dev/null +++ b/boustrophedon_server/src/boustrophedon_server/cellular_decomposition/cell.cpp @@ -0,0 +1,147 @@ +#include + +using namespace bcd; + +Vertex::Vertex(Event type, Circulator point) : type{ type }, point{ point } +{ +} + +bool Vertex::isAdjacentTo(const Vertex& other) const +{ + return *(point + 1) == *other.point || *(point - 1) == *other.point; +} + +Circulator Vertex::nextPoint() const +{ + return point + 1; +} + +Circulator Vertex::previousPoint() const +{ + return point - 1; +} + +std::ostream& operator<<(std::ostream& out, Event event) +{ + std::string string; + switch (event) + { + 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; + } + 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; +} + +bool Cell::enclosesVector(const Vertex& other) const +{ + 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(); +} + +const Vertex& Cell::currentFloor() const +{ + return floor_points.empty() ? start : floor_points.back(); +} + +const Point Cell::nextCeilPoint() const +{ + return *(currentCeil().point - 1); +} + +const Point Cell::nextFloorPoint() const +{ + return *(currentFloor().point + 1); +} + +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; +} + +Polygon Cell::toPolygon() const +{ + Polygon polygon; + auto points = toPoints(); + polygon.insert(polygon.vertices_end(), points.begin(), points.end()); + return polygon; +} + +std::ostream& operator<<(std::ostream& out, const bcd::Cell& cell) +{ + out << cell.start; + for (const auto& vertex : cell.floor_points) + { + out << ", " << vertex; + } + out << ", " << cell.end; + for (auto vertex = cell.ceiling_points.rbegin(); vertex != cell.ceiling_points.rend(); vertex++) + { + out << ", " << *vertex; + } + + out << std::endl << "neigbours: "; + + for (const auto& neighbour : cell.neighbors) + { + out << neighbour->start << " -> " << neighbour->end << std::endl; + } + 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 new file mode 100644 index 0000000..7f26091 --- /dev/null +++ b/boustrophedon_server/src/boustrophedon_server/cellular_decomposition/polygon_decomposer.cpp @@ -0,0 +1,420 @@ +#include +#include + +using namespace bcd; + +void PolygonDecomposer::decompose(Polygon& polygon) +{ + createVertices(polygon); + createCells(); +} + +std::vector PolygonDecomposer::getSubPolygons(const Point& position) const +{ + Cell* closest_cell = getClosestCell(position); + std::vector cells = visitCells(closest_cell); + + std::vector polygons = toPolygons(cells); + return polygons; +} + +void PolygonDecomposer::createVertices(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); + + auto vertex = left_vertex + 1; + + while (vertex != left_vertex) + { + current_event = getEvent(vertex, current_event); + vertices_.emplace_back(current_event, vertex); + vertex++; + } +} + +void PolygonDecomposer::insertOpenCloseVertices(Polygon& polygon) +{ + auto vertex = CGAL::left_vertex_2(polygon.vertices_circulator(), --polygon.vertices_circulator()); + Event current_event = Event::Start; + + // TODO(Oswin): Use something more efficient. I CBF to write a hash function for now. + std::vector visited_vertices; + + auto left_vertex = *vertex; + vertex++; + while (*vertex != left_vertex) + { + 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()) + { + 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; + } + + vertex++; + } +} + +void PolygonDecomposer::sliceAndInsertIntoPolygon(Polygon& polygon, const Point& point) +{ + auto vertical_line = Line(Point(point.x(), 0), Point(point.x(), 1)); + + for (auto it = polygon.vertices_begin(); it != polygon.vertices_end() - 1; it++) + { + auto polygon_edge = Segment(*it, *(it + 1)); + + auto result = CGAL::intersection(polygon_edge, vertical_line); + + if (result) + { + // 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; + } + } + } + } + + 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 (result) + { + // 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()) + { + polygon.push_back(*found_point); + } + } + } +} + +void PolygonDecomposer::createCells() +{ + sortVertices(); + + std::vector> current_cells; + current_cells.emplace_back(std::make_unique(vertices_.front())); + + for (int i = 1; i < vertices_.size(); i++) + { + 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) + { + 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) + { + std::swap(current_vertex, *other); + i--; + continue; + } + } + } +} + +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); + } +} + +bool PolygonDecomposer::handleFloor(bcd::Vertex& current_vertex, std::vector>& current_cells) +{ + for (auto& cell : current_cells) + { + if (cell->connectsToFloor(current_vertex)) + { + cell->floor_points.emplace_back(current_vertex); + return true; + } + } + return false; +} + +bool PolygonDecomposer::handleCeil(Vertex& current_vertex, std::vector>& current_cells) +{ + for (auto& cell : current_cells) + { + if (cell->connectsToCeil(current_vertex)) + { + cell->ceiling_points.emplace_back(current_vertex); + return true; + } + } + return false; +} + +bool PolygonDecomposer::handleStart(Vertex& current_vertex, std::vector>& current_cells) +{ + current_cells.emplace_back(std::make_unique(current_vertex)); + return true; +} + +bool PolygonDecomposer::handleEnd(Vertex& current_vertex, std::vector>& current_cells) +{ + for (auto it = current_cells.begin(); it != current_cells.end(); it++) + { + 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; +} + +bool PolygonDecomposer::handleOpen(Vertex& current_vertex, std::vector>& current_cells) +{ + for (auto it = current_cells.begin(); it != current_cells.end(); it++) + { + Cell* closing_cell = it->get(); + if (closing_cell->enclosesVector(current_vertex)) + { + 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)); + return true; + } + } + return false; +} + +bool PolygonDecomposer::handleClose(Vertex& current_vertex, std::vector>& current_cells) +{ + 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; + + 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(); + + auto bottom_cell_it = getBottomCell(current_vertex, current_cells); + + if (bottom_cell_it == current_cells.end()) + { + return false; + } + + 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) + { + last_event = Event::Floor; + } + else if (last_event == Event::End || last_event == Event::Close) + { + last_event = Event::Ceil; + } + + auto next_vertex = vertex + 1; + + if (last_event == Event::Floor && next_vertex->x() < vertex->x()) + { + if (next_vertex->y() < vertex->y()) + { + return Event::Close; + } + return Event::End; + } + if (last_event == Event::Ceil && next_vertex->x() > vertex->x()) + { + if (next_vertex->y() < vertex->y()) + { + return Event::Start; + } + return Event::Open; + } + return last_event; +} + +void PolygonDecomposer::sortVertices() +{ + std::sort(vertices_.begin(), vertices_.end(), [](const Vertex& lhs, const Vertex& rhs) { + if (lhs.point->x() != rhs.point->x()) + { + return lhs.point->x() < rhs.point->x(); + } + + 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; +} + +bcd::Cell* PolygonDecomposer::getClosestCell(const Point& position) const +{ + 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(); + double smallest_distance = std::numeric_limits::max(); + for (auto edge_it = polygon.edges_begin(); edge_it != polygon.edges_end(); edge_it++) + { + 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); + }; + + auto closest = std::min_element(cells_.begin(), cells_.end(), compare_distances); + return closest->get(); +} + +std::vector PolygonDecomposer::visitCells(const Cell* starting_cell) const +{ + std::unordered_set visited; + std::vector cells; + const Cell* current_cell = starting_cell; + + auto not_visited = [&](const Cell* cell) { return visited.find(cell) == visited.end(); }; + + auto get_next_unvisited_cell = [&](const Cell* cell) { + return std::find_if(cell->neighbors.begin(), cell->neighbors.end(), not_visited); + }; + + while (cells.size() < cells_.size()) + { + visited.emplace(current_cell); + cells.emplace_back(current_cell); + + auto not_visited_cell = get_next_unvisited_cell(current_cell); + if (not_visited_cell != current_cell->neighbors.end()) + { + current_cell = *not_visited_cell; + } + else + { + for (auto it = cells.rbegin(); it != cells.rend(); it++) + { + auto not_visited_cell = get_next_unvisited_cell(*it); + if (not_visited_cell != (*it)->neighbors.end()) + { + current_cell = *not_visited_cell; + break; + } + } + } + } + return cells; +} diff --git a/boustrophedon_server/src/boustrophedon_server/cgal_utils.cpp b/boustrophedon_server/src/boustrophedon_server/cgal_utils.cpp new file mode 100644 index 0000000..7b285be --- /dev/null +++ b/boustrophedon_server/src/boustrophedon_server/cgal_utils.cpp @@ -0,0 +1,44 @@ +#include + +AffineTransform preprocessPolygon(Polygon& polygon, const 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); + + return transform; +} + +void postprocessPolygonAndPath(const AffineTransform& preprocess_transform, Polygon& polygon, + std::vector& path) +{ + auto transform = preprocess_transform.inverse(); + + transformPoints(polygon.vertices_begin(), polygon.vertices_end(), transform); + + transformNavPoints(path.begin(), path.end(), transform); +} + +void transformNavPoints(const std::vector::iterator& first, const std::vector::iterator& last, + const CGAL::Aff_transformation_2& transform) +{ + for (auto current = first; current != last; current++) + { + current->point = transform(current->point); + } +} diff --git a/boustrophedon_server/src/boustrophedon_server/outline_planner.cpp b/boustrophedon_server/src/boustrophedon_server/outline_planner.cpp new file mode 100644 index 0000000..65f9901 --- /dev/null +++ b/boustrophedon_server/src/boustrophedon_server/outline_planner.cpp @@ -0,0 +1,60 @@ +#include +#include + +void OutlinePlanner::setParameters(OutlinePlanner::Parameters parameters) +{ + params_ = parameters; +} + +void OutlinePlanner::addToPath(Polygon polygon, const Point& robot_position, std::vector& path, + Polygon& innermost_polygon) +{ + shiftPolygonToStartNearRobot(polygon, robot_position); + + if (polygon.is_clockwise_oriented()) + { + polygon.reverse_orientation(); + } + + // Add outermost outline + if (params_.outline_layers >= 1) + { + for (const auto& point : polygon.container()) + { + path.emplace_back(PointType::Outline, point); + } + path.emplace_back(PointType::Outline, polygon.container().front()); + } + innermost_polygon = polygon; + 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()) + { + path.emplace_back(PointType::Outline, p); + } + // Close polygon by repeating first point + path.emplace_back(PointType::Outline, inner_polygons.front()->container().front()); + innermost_polygon = *inner_polygons.front(); + } +} + +void OutlinePlanner::shiftPolygonToStartNearRobot(Polygon& polygon, const Point& robot_position) +{ + auto comp_dist_to_robot = [&robot_position](const Point& a, const Point& b) { + auto comp = CGAL::compare_distance_to_point(robot_position, a, b); + return (comp == CGAL::SMALLER); + }; + + std::vector points; + points.insert(points.end(), polygon.vertices_begin(), polygon.vertices_end()); + + auto closest_point_iter = std::min_element(points.begin(), points.end(), comp_dist_to_robot); + + std::rotate(points.begin(), closest_point_iter, points.end()); + + polygon.clear(); + std::copy(points.begin(), points.end(), std::back_inserter(polygon)); +} diff --git a/boustrophedon_server/src/boustrophedon_server/striping_planner.cpp b/boustrophedon_server/src/boustrophedon_server/striping_planner.cpp new file mode 100644 index 0000000..7051eaf --- /dev/null +++ b/boustrophedon_server/src/boustrophedon_server/striping_planner.cpp @@ -0,0 +1,97 @@ +#include +#include +#include +#include + +void StripingPlanner::setParameters(StripingPlanner::Parameters parameters) +{ + params_ = parameters; +} + +void StripingPlanner::addToPath(const Polygon& polygon, const Point& robot_position, std::vector& path) +{ + std::vector new_path_section; + + const auto& last_point = path.empty() ? robot_position : path.back().point; + + 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) + { + AffineTransform flip_transform(-1, 0, 0, 1); + transformPoints(polygon.vertices_begin(), polygon.vertices_end(), flip_transform); + } + + fillPolygon(polygon, new_path_section); + + if (!left_closer) + { + 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()); +} + +void StripingPlanner::fillPolygon(const Polygon& polygon, std::vector& path) +{ + auto left_vertex = *CGAL::left_vertex_2(polygon.container().begin(), polygon.container().end()); + 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; + + for (auto stripe_num = 0; stripe_num < stripe_count; stripe_num++) + { + auto x = (stripe_num * params_.stripe_separation) + min_x; + + 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); + return (comp == CGAL::SMALLER); + }; + + std::sort(intersections.begin(), intersections.end(), comp_dist_to_curr_point); + + intersections.erase(std::unique(intersections.begin(), intersections.end()), intersections.end()); + + path.emplace_back(PointType::StripeStart, intersections.front()); + path.emplace_back(PointType::StripeEnd, intersections.back()); + current_point = intersections.back(); + } + } +} + +std::vector StripingPlanner::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) + { + 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); + } + } + } + return intersections; +}