greenzie / boustrophedon_planner

Coverage path planner

Warning

NOTE: This repository is no longer supported or updated. If you wish to continue to develop this code yourself, we recommend you fork it.

Boustrophedon Planner

Boustrophedon Planner is a coverage path planner that implements a modified cellular decomposition algorithm. The original can be found here.

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.

Changelog

• January 23, 2020:
  • The boustrophedon planner can now handle all types of simple polygons that it is given, both convex and concave.
  • Inner boundaries are supported, allowing the planner to create paths at a certain offset from the initial given boundary.
  • "Half - Y" turns are supported, allowing for some curvature at the start / end of paths to assist robots that are not completely holonomic.
  • Many new launch file parameters allow the user to specify more precisely the behavior that they want the plan to have

License

This repository is subject to version 3 of the GNU Lesser General Public License or later due to its dependencies.

The geometric operations rely on CGAL which is restricted by version 3 of the GNU Lesser General Public License.

boustrophedon_planner's Issues

Cmake error

Hi

I have that cmake error that i do not know how to solve

-- +++ processing catkin package: 'boustrophedon_server'
-- ==> add_subdirectory(boustrophedon_planner/boustrophedon_server)
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
CMake Error at boustrophedon_planner/boustrophedon_server/CMakeLists.txt:16 (find_package):
By not providing "FindCGAL.cmake" in CMAKE_MODULE_PATH this project has
asked CMake to find a package configuration file provided by "CGAL", but
CMake did not find one.

Could not find a package configuration file provided by "CGAL" with any of
the following names:

CGALConfig.cmake
cgal-config.cmake

Add the installation prefix of "CGAL" to CMAKE_PREFIX_PATH or set
"CGAL_DIR" to a directory containing one of the above files. If "CGAL"
provides a separate development package or SDK, be sure it has been
installed.

-- Configuring incomplete, errors occurred!
See also "/home/francois/catkin_ws/build/CMakeFiles/CMakeOutput.log".
See also "/home/francois/catkin_ws/build/CMakeFiles/CMakeError.log".
Makefile:5150: recipe for target 'cmake_check_build_system' failed
make: *** [cmake_check_build_system] Error 1
Invoking "make cmake_check_build_system" failed

Thank in advance

Francois

How to use the package to tackle a picture with obstacles?

Always fail to wait for result?

Hi, I had write an action client node, but it always fail to wait for result, why?

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
// #include <boustrophedon_msgs/PlanMovingPathAction.h>
#include "boustrophedon_server/boustrophedon_planner_server.h"

int main (int argc, char **argv)
{
  ros::init(argc, argv, "boustrophedon_planner_client_node");

  // create the action client
  // true causes the client to spin its own thread
  actionlib::SimpleActionClient<boustrophedon_msgs::PlanMowingPathAction> ac("plan_path", true);

  ROS_INFO("Waiting for action server to start.");
  // wait for the action server to start
  ac.waitForServer(); //will wait for infinite time

  ROS_INFO("Action server started, sending goal.");
  // send a goal to the action
  boustrophedon_msgs::PlanMowingPathGoal goal;
  geometry_msgs::PolygonStamped property;
  property.header.frame_id = "map";
  geometry_msgs::Point32 point;
  point.z = 0;
  double length = 5;
  point.x = -length;
  point.y = length;
  property.polygon.points.push_back(point);
  point.x = length;
  point.y = length;
  property.polygon.points.push_back(point);
  point.x = length;
  point.y = -length;
  property.polygon.points.push_back(point);
  point.x = -length;
  point.y = -length;
  property.polygon.points.push_back(point);
  point.x = -length;
  point.y = length;
  property.polygon.points.push_back(point);

  geometry_msgs::PoseStamped robot_position;
  robot_position.header.frame_id = "map";
  robot_position.pose.orientation.x = 0;
  robot_position.pose.orientation.y = 0;
  robot_position.pose.orientation.z = 0;
  robot_position.pose.orientation.w = 1;
  robot_position.pose.position.x = 1;
  robot_position.pose.position.y = 1;
  robot_position.pose.position.z = 0;
  
  goal.property = property;
  goal.robot_position = robot_position;
  ac.sendGoal(goal);

  // //wait for the action to return
  bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));

  if (finished_before_timeout)
  {
    actionlib::SimpleClientGoalState state = ac.getState();
    ROS_INFO("Action finished: %s",state.toString().c_str());
  }
  else
    ROS_INFO("Action did not finish before the time out.");

  //exit
  return 0;
}

I use add_executable() in CMakeList.txt, and run the following command after catkin_make

$ roslaunch boustrophedon_server boustrophedon_server.launch
... logging to /home/sysu/.ros/log/6d834cf4-dcff-11ea-92c4-62d947dadec9/roslaunch-sysu-14592.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://sysu:33685/

SUMMARY
========

PARAMETERS
 * /boustrophedon_server/allow_points_outside_boundary: False
 * /boustrophedon_server/enable_half_y_turns: False
 * /boustrophedon_server/enable_stripe_angle_orientation: True
 * /boustrophedon_server/intermediary_separation: 0.0
 * /boustrophedon_server/outline_clockwise: True
 * /boustrophedon_server/outline_layer_count: 0
 * /boustrophedon_server/points_per_turn: 15
 * /boustrophedon_server/publish_path_points: True
 * /boustrophedon_server/publish_polygons: True
 * /boustrophedon_server/repeat_boundary: False
 * /boustrophedon_server/skip_outlines: True
 * /boustrophedon_server/stripe_angle: 0.0
 * /boustrophedon_server/stripe_separation: 1.0
 * /boustrophedon_server/travel_along_boundary: True
 * /boustrophedon_server/turn_start_offset: 0.5
 * /rosdistro: melodic
 * /rosversion: 1.14.7

NODES
  /
    boustrophedon_server (boustrophedon_server/boustrophedon_planner_server)

ROS_MASTER_URI=http://10.152.45.18:11311

process[boustrophedon_server-1]: started with pid [14601]
[ INFO] [1597280911.186913171]: Got Striping Angle from Orientation: 0
Skipping outlining!
[ INFO] [1597280911.297863542]: Decomposing boundary polygon into sub-polygons...
[ INFO] [1597280911.298188815]: Broke the boundary up into 1 sub-polygons

In another terminal, run

$ rosrun boustrophedon_server boustrophedon_planner_client
[ INFO] [1597200759.464738519]: Waiting for action server to start.
[ INFO] [1597200767.914228864]: Action server started, sending goal.
[ INFO] [1597200797.914582982]: Action did not finish before the time out.

License issue

          I would like to comment that on the readme says this project has a GPL license, but the LICENSE file attached is for a LGPL license.

Originally posted by @Gonzalo-Mier in #4 (comment)

How to run the package to get the phenomenon？

Your paper and code have benefited me a lot, but I don't know how to run it. I roslaunch the launch node in the server, but I don't seem to see any effect. How can I operate to see the phenomenon in the paper。
Thank you！

Error while processing a polygon

While processing this polygon i get the following error

[ INFO] [1636493628.111111246]: Got Striping Angle from Orientation: 0
Skipping outlining!
[ INFO] [1636493628.117593756]: Decomposing boundary polygon into sub-polygons...
free(): double free detected in tcache 2
[boustrophedon_server-2] process has died [pid 11555, exit code -6, cmd /home/francois/catkin_ws/devel/lib/boustrophedon_server/boustrophedon_planner_server __name:=boustrophedon_server __log:=/home/francois/.ros/log/9f66a5b2-41a4-11ec-bcf7-38d547191d23/boustrophedon_server-2.log].
log file: /home/francois/.ros/log/9f66a5b2-41a4-11ec-bcf7-38d547191d23/boustrophedon_server-2*.log

I have the same problem with this simple polygon

And a different problem with this other simple polygon

[ INFO] [1636506497.912456973]: Decomposing boundary polygon into sub-polygons...
[boustrophedon_server-4] process has died [pid 24207, exit code -11, cmd /home/francois/catkin_ws/devel/lib/boustrophedon_server/boustrophedon_planner_server __name:=boustrophedon_server __log:=/home/francois/.ros/log/a8436896-41c2-11ec-bcf7-38d547191d23/boustrophedon_server-4.log].
log file: /home/francois/.ros/log/a8436896-41c2-11ec-bcf7-38d547191d23/boustrophedon_server-4*.log

Valid license?

We would like to contribute to the project but we're concerned about the licensing.
The repo is setup as "MIT" but you appear to be using the CGAL library which is either GPL or LGPL so we have concerns about the licensing restrictions attached to this code.
Have you confirmed that you're allowed to release this code as "MIT"?

Support for inner polygon areas to not cover?

Hi,

If we had a kitchen with an island, could we create a polygon for that island internal to the outline polygon to stay out of with this implementation?

We're looking at coverage planners for ROS2 / Nav2 integration and this one has come to mind

How do I run this package

I'm a bit confused on if I'm running this package correctly. there is only one launch file.when I roslaunch boustrophedon_server boustrophedon_server.launch
I get this:

started roslaunch server http://localhost:35547/

SUMMARY
========

PARAMETERS
 * /boustrophedon_server/allow_points_outside_boundary: False
 * /boustrophedon_server/enable_half_y_turns: False
 * /boustrophedon_server/enable_stripe_angle_orientation: True
 * /boustrophedon_server/intermediary_separation: 0.0
 * /boustrophedon_server/outline_clockwise: True
 * /boustrophedon_server/outline_layer_count: 0
 * /boustrophedon_server/points_per_turn: 15
 * /boustrophedon_server/publish_path_points: True
 * /boustrophedon_server/publish_polygons: True
 * /boustrophedon_server/repeat_boundary: False
 * /boustrophedon_server/skip_outlines: True
 * /boustrophedon_server/stripe_angle: 0.0
 * /boustrophedon_server/stripe_separation: 1.0
 * /boustrophedon_server/travel_along_boundary: True
 * /boustrophedon_server/turn_start_offset: 0.5
 * /rosdistro: melodic
 * /rosversion: 1.14.13

NODES
  /
    boustrophedon_server (boustrophedon_server/boustrophedon_planner_server)

ROS_MASTER_URI=http://localhost:11311

process[boustrophedon_server-1]: started with pid [16616]

But nothing seems to happen.I wishto use the planner for the turtlebot3_world.world map (I'm using turtlebot3).

A little help please!

Hi Charles,
As per an earlier Slack chat, I'd love to try this planner on my mower. I doubt will end up being what I use, as I don't need a coverage planner. I need something a bit different in an orchard environment.
To get this planner going though, could you please make some suggestions? I'm not the greatest ROS user.
To begin with, for example, I could draw a polygon in Google Earth and export the KML file. That contains the points. I then need to be able to publish the points via a polygon msg to the boustrophedon server. I also need to publish a starting pose?
The server will then generate a striping plan. Does it generate a stream of points or a path? Just thinking on how to turn the output into goals for the global_planner.
Hope you can help! :)

Ta,
Ben

Doesn't allow concave polygons

Any attempt to feed in a polygon* that is convex results in the following CGAL error:

[ INFO] [1622994939.809321796]: Decomposing boundary polygon into sub-polygons...
terminate called after throwing an instance of 'CGAL::Assertion_exception'
what(): CGAL ERROR: assertion violation!
Expr: ptr_->count > 0
File: /usr/local/include/CGAL/Handle_for.h
Line: 108

[boustrophedon_server-2] process has died...

*Example polygon coordinates:
1.1, -1.0
0.3, -1.0
0.3, 1.7
-0.3, 1.7
-0.3, -1.0
-0.7, -1.0
-0.7, 2.0
1.1, 2.0

Referenced assertion violation in Handle_for.h:

Handle_for(const Handle_for& h)
  : ptr_(h.ptr_)
{
CGAL_assume (ptr_->count > 0);
    ++(ptr_->count);
}

This error is consistent with all concave polygons I attempt. Any idea why this is happening?

Client Application

Hello,

Is there any test application for testing and show the striping plan?
I couldn't find out in repo. Could you help me, please?

Thanks!

怎么运行？

我想把它加入move_base中，请问怎么使用，这只有一个server嘛？

Error running 'clang-tidy': No such file or directory

Hi, when I'm trying to run this project by catkin_make I am getting the error "Error running 'clang-tidy': No such file or directory"

To be more specific the entire error says,

Error running 'clang-tidy': No such file or directory
boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/build.make:62: recipe for target 'boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/boustrophedon_planner_node.cpp.o' failed
make[2]: *** [boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/boustrophedon_planner_node.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs....
Error running 'clang-tidy': No such file or directory
[ 86%] Automatic MOC for target rviz_polygon_tool
boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/build.make:86: recipe for target 'boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/boustrophedon_planner_server.cpp.o' failed
make[2]: *** [boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/boustrophedon_planner_server.cpp.o] Error 1
Error running 'clang-tidy': No such file or directory
Error running 'clang-tidy': No such file or directory
boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/build.make:110: recipe for target 'boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/striping_planner.cpp.o' failed
make[2]: *** [boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/striping_planner.cpp.o] Error 1
boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/build.make:134: recipe for target 'boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/outline_planner.cpp.o' failed
make[2]: *** [boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/outline_planner.cpp.o] Error 1
Error running 'clang-tidy': No such file or directory
boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/build.make:158: recipe for target 'boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/cgal_utils.cpp.o' failed
make[2]: *** [boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/cgal_utils.cpp.o] Error 1
Error running 'clang-tidy': No such file or directory
boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/build.make:182: recipe for target 'boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/boustrophedon_types.cpp.o' failed
make[2]: *** [boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/boustrophedon_types.cpp.o] Error 1
CMakeFiles/Makefile2:9983: recipe for target 'boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/all' failed
make[1]: *** [boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
[ 86%] Built target rviz_polygon_tool_autogen
[ 90%] Built target polygon_coverage_planners
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j8 -l8" failed

Disclaimer- there might be a few statements of my other projects getting built, so feel free to ignore them but I think I've posted most of the relevant lines.

from CGAL::Polygon_2<CGAL::Epick> to CGAL::Polygon_2<CGAL::Cartesian<double>>

in outline_planner.cpp, function addInnerOutline, innermost_polygon = *inner_polygons.front();
error is no known conversion from CGAL::Polygon_2CGAL::Epick to CGAL::Polygon_2<CGAL::Cartesian>
Can you help me with this problem? Thank you very much!

CGAL issue

I've cloned and tried to compile with catkin_make.

There is an error appeared like this:

[ 48%] Building CXX object boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/striping_planner.cpp.o
In file included from /usr/include/CGAL/Straight_skeleton_items_2.h:22:0,
                 from /usr/include/CGAL/Straight_skeleton_2.h:23,
                 from /usr/include/CGAL/Straight_skeleton_builder_2.h:37,
                 from /usr/include/CGAL/create_straight_skeleton_2.h:23,
                 from /usr/include/CGAL/create_offset_polygons_2.h:23,
                 from /home/gokhan/catkin_ws/src/boustrophedon_planner/boustrophedon_server/src/boustrophedon_server/striping_planner.cpp:1:
/usr/include/CGAL/Straight_skeleton_vertex_base_2.h: In constructor ‘CGAL::Straight_skeleton_vertex_base_base_2<Refs, P, N>::Straight_skeleton_vertex_base_base_2(int)’:
/usr/include/CGAL/Straight_skeleton_vertex_base_2.h:166:14: error: ‘ORIGIN’ was not declared in this scope
     , mP    (ORIGIN) 
              ^
boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/build.make:110: recipe for target 'boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/striping_planner.cpp.o' failed
make[2]: *** [boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/src/boustrophedon_server/striping_planner.cpp.o] Error 1
CMakeFiles/Makefile2:26682: recipe for target 'boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/all' failed
make[1]: *** [boustrophedon_planner/boustrophedon_server/CMakeFiles/boustrophedon_planner_server.dir/all] Error 2
Makefile:138: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j1" failed

How can this problem be solved?