├── CMakeLists.txt ├── README.txt ├── include └── coverage_planner │ ├── coverage_planner.h │ ├── discr_environment.h │ ├── environment.h │ └── visualize.h ├── package.xml └── src ├── coverage_planner.cpp └── coverage_planner_node.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(coverage_planner) 3 | set(CMAKE_CXX_STANDARD 11) 4 | 5 | ## Compile as C++11, supported in ROS Kinetic and newer 6 | # add_compile_options(-std=c++11) 7 | 8 | ## Find catkin macros and libraries 9 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 10 | ## is used, also find other catkin packages 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | rospy 14 | std_msgs 15 | geometry_msgs 16 | visualization_msgs 17 | tf 18 | ) 19 | 20 | ## System dependencies are found with CMake's conventions 21 | find_package(Boost REQUIRED COMPONENTS system thread) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a exec_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | # add_message_files( 55 | # FILES 56 | # Message1.msg 57 | # Message2.msg 58 | # ) 59 | 60 | ## Generate services in the 'srv' folder 61 | # add_service_files( 62 | # FILES 63 | # Service1.srv 64 | # Service2.srv 65 | # ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | # generate_messages( 76 | # DEPENDENCIES 77 | # std_msgs 78 | # ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if your package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | INCLUDE_DIRS include ${Boost_INCLUDE_DIRS} 111 | LIBRARIES coverage_planner 112 | CATKIN_DEPENDS roscpp rospy std_msgs visualization_msgs 113 | DEPENDS system_lib Boost 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | 120 | ## Specify additional locations of header files 121 | ## Your package locations should be listed before other locations 122 | include_directories( 123 | include 124 | ${catkin_INCLUDE_DIRS} 125 | ${Boost_INCLUDE_DIRS} 126 | ) 127 | 128 | ## Declare a C++ library 129 | add_library(coverage_planner_ 130 | src/coverage_planner.cpp 131 | ) 132 | 133 | ## Add cmake target dependencies of the library 134 | ## as an example, code may need to be generated before libraries 135 | ## either from message generation or dynamic reconfigure 136 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 137 | 138 | ## Declare a C++ executable 139 | ## With catkin_make all packages are built within a single CMake context 140 | ## The recommended prefix ensures that target names across packages don't collide 141 | add_executable(${PROJECT_NAME}_node src/coverage_planner_node.cpp) 142 | 143 | ## Rename C++ executable without prefix 144 | ## The above recommended prefix causes long target names, the following renames the 145 | ## target back to the shorter version for ease of user use 146 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 147 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 148 | 149 | ## Add cmake target dependencies of the executable 150 | ## same as for the library above 151 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 152 | 153 | ## Specify libraries to link a library or executable target against 154 | target_link_libraries(${PROJECT_NAME}_node 155 | ${catkin_LIBRARIES} coverage_planner_ 156 | ) 157 | 158 | ############# 159 | ## Install ## 160 | ############# 161 | 162 | # all install targets should use catkin DESTINATION variables 163 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 164 | 165 | ## Mark executable scripts (Python etc.) for installation 166 | ## in contrast to setup.py, you can choose the destination 167 | # install(PROGRAMS 168 | # scripts/my_python_script 169 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark executables and/or libraries for installation 173 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 174 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 177 | # ) 178 | 179 | ## Mark cpp header files for installation 180 | # install(DIRECTORY include/${PROJECT_NAME}/ 181 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 182 | # FILES_MATCHING PATTERN "*.h" 183 | # PATTERN ".svn" EXCLUDE 184 | # ) 185 | 186 | ## Mark other files for installation (e.g. launch and bag files, etc.) 187 | # install(FILES 188 | # # myfile1 189 | # # myfile2 190 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 191 | # ) 192 | 193 | ############# 194 | ## Testing ## 195 | ############# 196 | 197 | ## Add gtest based cpp test target and link libraries 198 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_coverage_planner.cpp) 199 | # if(TARGET ${PROJECT_NAME}-test) 200 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 201 | # endif() 202 | 203 | ## Add folders to be run by python nosetests 204 | # catkin_add_nosetests(test) 205 | -------------------------------------------------------------------------------- /README.txt: -------------------------------------------------------------------------------- 1 | Instructions for building and running the package, taking input, visualization in rviz and 2 | a brief description of the code files. 3 | 4 | Assumptions: 5 | 6 | 1) The environment is bounded by a convex polygon and contains obstacles as convex polygons 7 | 2) The robot can execute the motions up, left, down, right. 8 | 9 | 10 | 1. Making the coverage planner package: 11 | 12 | This package is for ros kinetic. It may or may not run in newer or older versions. 13 | Download the package in your catkin workspace and build the package using catkin make. 14 | It has dependencies already included in ros kinetic and on the boost library. 15 | 16 | 17 | 2. To run the package, execute the following command after having built the package: 18 | 19 | rosrun coverage_planner coverage_planner_node 20 | 21 | coverage_planner_node is name of the C++ executable 22 | 23 | 24 | 3. Visulization in rviz: 25 | 26 | For visualizing in rviz, add 2 markers and a marker array as the display items. 27 | 28 | The topics for markers are: 29 | /visualization_marker 30 | /visualization_marker_path 31 | 32 | The topic for marker array is: 33 | /visualization_marker_obs 34 | 35 | Also add grid and axes for better display. Fixed frame will have the name 'map'. 36 | 37 | NOTE: if topic of marker is added to marker array or vice-versa, ros will give an error. 38 | 39 | 4. Instructions for input from rviz: 40 | 41 | The terminal will ask you to click points on rviz and then enter a digit. 42 | To enter a polygon, enter its vertices by clicking points on rviz screen using the '2D Pose Estimate' button. 43 | Adjacent vertices shall be entered in order. i.e., The points shall be clicked in either clockwise or counter-clockwise direction. 44 | After entering polygon vertices, enter a digit on the terminal screen. If a convex polygon is not entered, the program will exit. 45 | 46 | After entering the vertices, you shall now be able to visualize the polygon in blue. 47 | 48 | Next, the terminal will ask you to enter number of obstacles. If a negative number is entered, the program will exit. 49 | The terminal will then ask you to enter obstacle points. As above, click points on rviz using '2D Pose Estimate' to form a convex polygon. After entering points for each obstacle, enter a digit. 50 | 51 | After entering the obstacles as polygon vertices, you shall now be able to visualize the obstacles in green. 52 | 53 | Once, done, the plan will be computed. The plan will be displayed in rviz in red. 54 | 55 | 5. Description of code files 56 | 57 | The major source code lies in the file src/coverage_planner.cpp 58 | The main function lies in src/coverage_planner_node.cpp 59 | visualization code lies in include/coverage_planner/visualization.h 60 | Data types for representing environments lie in environment.h and discr_environment.h 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /include/coverage_planner/coverage_planner.h: -------------------------------------------------------------------------------- 1 | #ifndef COVERAGE_PLANNER_ 2 | #define COVERAGE_PLANNER_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | struct cont_point{ 16 | double x, y; 17 | }; 18 | 19 | class coverage_planner 20 | { 21 | public: 22 | 23 | ros::NodeHandle n; 24 | 25 | ros::Subscriber sub_polygon; 26 | 27 | //ros::Publisher marker_pub, marker_pub_obs; 28 | 29 | environment env; 30 | 31 | discr_environment envd; 32 | 33 | coverage_planner(ros::NodeHandle& nh); 34 | 35 | ~coverage_planner(); 36 | 37 | visualize vis; 38 | 39 | bool exit_condition; 40 | 41 | static void exit_gracefully(coverage_planner* c); 42 | 43 | void compute_plan(); // function which does all the plan computation 44 | 45 | boost::thread* th_input; 46 | 47 | //boost::thread th_exit; 48 | 49 | private: 50 | 51 | void vertices_callback(const geometry_msgs::PoseWithCovarianceStamped& msg); 52 | 53 | void input_polygon(); // input thread function 54 | 55 | void define_motions(); 56 | 57 | void continuous_to_discrete(double x, double y, int& x_dis, int& y_dis, const environment& e); 58 | 59 | void discrete_to_continuous(int x, int y, double& cx, double& cy, const environment& e); 60 | 61 | bool valid_polygon(const polygon& p); 62 | 63 | void compute_line_eqn(double& a, double& b, double& c, const vertex& v1, const vertex& v2); 64 | 65 | bool line_segment_intersect(const vertex& v1, const vertex& v2, const vertex& v3, const vertex& v4); 66 | 67 | bool point_within_polygon(double x, double y, const polygon& p); 68 | 69 | void iterate_over_adjacent_nodes(bool& edge_added, disc_point& adj, disc_point& current); 70 | 71 | void iterate_over_adjacent_nodes_(bool& edge_added, disc_point& adj, disc_point& current); 72 | 73 | bool within_bounds(disc_point p, int x_cells, int y_cells); 74 | 75 | bool within_obstacle(double, double, const environment& e); 76 | 77 | disc_point compute_adjacent(const disc_point& p, string m); 78 | 79 | void compute_source(vertex& v); 80 | 81 | void make_graph(environment& e, discr_environment& d); 82 | 83 | bool add_node(disc_point adj, disc_point current, const environment& e, discr_environment& d); 84 | 85 | void shortest_path(disc_point s, disc_point g, vector& plan_, discr_environment& d); 86 | 87 | void output_plan(const vector&); 88 | 89 | int count_vertices, count_obstacles, no_obstacles; 90 | 91 | vertex test_vertex; 92 | 93 | vector motions; 94 | 95 | vector plan; 96 | 97 | bool vertices_done, obstacles_done; 98 | 99 | }; 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /include/coverage_planner/discr_environment.h: -------------------------------------------------------------------------------- 1 | #ifndef DISCR_ENVIRONENT_H 2 | #define DISCR_ENVIRONMENT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace std; 11 | 12 | // This code contains data types for storing a discrete representation of the environment 13 | 14 | 15 | struct disc_point{ // data type for storing discrete points 16 | int x, y; 17 | disc_point(int, int); 18 | bool operator == (const disc_point& d) const; 19 | bool operator != (const disc_point& d) const; 20 | }; 21 | 22 | disc_point::disc_point(int x_ = 0, int y_ = 0) 23 | { 24 | x = x_; 25 | y = y_; 26 | } 27 | 28 | bool disc_point::operator == (const disc_point& d) const 29 | { 30 | return (x == d.x && y == d.y); 31 | } 32 | 33 | bool disc_point::operator != (const disc_point& d) const 34 | { 35 | return !(x == d.x && y == d.y); 36 | } 37 | 38 | struct node{ // data type for storing node attributes, i.e. discrete point, parent, child and flag 39 | disc_point p; 40 | disc_point parent, child; 41 | vector neighbors; 42 | disc_point qparent; 43 | bool inqueue; 44 | bool nodereached; 45 | node(); 46 | }; 47 | 48 | node::node() 49 | { 50 | parent.x = -1; 51 | parent.y = -1; 52 | nodereached = false; 53 | } 54 | 55 | struct classcomp { 56 | bool operator() (const disc_point& lhs, const disc_point& rhs) const 57 | { 58 | if (lhs.x < rhs.x) 59 | { return 1;} 60 | else if (lhs.x > rhs.x) 61 | { return 0; } 62 | else if (lhs.x == rhs.x) 63 | { 64 | return lhs.y < rhs.y; 65 | } 66 | } 67 | }; 68 | 69 | 70 | class discr_environment // this class stores a set and a map for containing discrete point values and nodes of the environment 71 | { 72 | public: 73 | 74 | set pointset; 75 | 76 | map graphnodes; 77 | 78 | int x_cells, y_cells; 79 | 80 | void initialize_discr_environment(const environment&); 81 | 82 | }; 83 | 84 | void discr_environment::initialize_discr_environment(const environment& e) 85 | { 86 | x_cells = floor((abs(e.xmax - e.xmin))/e.resolution); 87 | y_cells = floor((abs(e.ymax - e.ymin))/e.resolution); 88 | } 89 | 90 | #endif -------------------------------------------------------------------------------- /include/coverage_planner/environment.h: -------------------------------------------------------------------------------- 1 | #ifndef ENVIRONMENT_H 2 | #define ENVIRONMENT_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | // This code contains data type and functions for representing the environment in form of polygons and obstacle polygons 11 | 12 | struct vertex // data type for storing x, y values of a vertex and also for storing continuous x, y point values 13 | { 14 | double x, y; 15 | vertex(double x_, double y_); 16 | }; 17 | 18 | vertex::vertex(double x_ = 0.0, double y_ = 0.0) 19 | { 20 | x = x_; 21 | y = y_; 22 | } 23 | 24 | struct polygon // polygon represented as a vector of x, y values 25 | { 26 | vector vertices; 27 | }; 28 | 29 | struct environment 30 | { 31 | polygon pboundary; // pboundary stores the input polygon 32 | vector obstacles; // obstacles store the input polygonal obstacles 33 | double xmin, ymin, xmax, ymax; 34 | double resolution; 35 | environment(); 36 | void compute_env_bounds(); 37 | void compute_resolution(); 38 | void output_parameters(); 39 | void initialize_environment(); 40 | }; 41 | 42 | environment::environment() 43 | { 44 | resolution = 0.05; 45 | xmin = 0.0; 46 | ymin = 0.0; 47 | xmax = 1.0; 48 | ymax = 1.0; 49 | } 50 | 51 | void environment::compute_env_bounds() // computes min and mx x,y, values from input polygon 52 | { 53 | xmin = pboundary.vertices[0].x; 54 | ymin = pboundary.vertices[0].y; 55 | xmax = pboundary.vertices[0].x; 56 | ymax = pboundary.vertices[0].y; 57 | for (int i = 0; i < pboundary.vertices.size(); i++) 58 | { 59 | xmin = pboundary.vertices[i].x < xmin ? pboundary.vertices[i].x : xmin; 60 | ymin = pboundary.vertices[i].y < ymin ? pboundary.vertices[i].y : ymin; 61 | xmax = pboundary.vertices[i].x > xmax ? pboundary.vertices[i].x : xmax; 62 | ymax = pboundary.vertices[i].y > ymax ? pboundary.vertices[i].y : ymax; 63 | } 64 | } 65 | 66 | void environment::compute_resolution() 67 | { 68 | resolution = (min(abs(xmin - xmax), abs(ymin - ymax)))/10; 69 | resolution = 0.25; 70 | } 71 | 72 | void environment::output_parameters() 73 | { 74 | cout<<"xmin: "< 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | // This code visualizes polygons, obstacles and paths. It takes input from functions in coverage_planner.cpp 17 | 18 | class visualize 19 | { 20 | public: 21 | 22 | visualize(ros::NodeHandle& nh); 23 | 24 | ~visualize(); 25 | 26 | void initialize_marker_variables_polygon(bool v, const environment& env); 27 | 28 | void initialize_marker_variables_obstacles(int c, const environment& env); 29 | 30 | void initialize_marker_variables_path(const vector& plan); 31 | 32 | void draw_polygon(); 33 | 34 | void draw_obstacles(); 35 | 36 | void vis_path(); 37 | 38 | bool marker_initialize, obs_marker_initialized, path_marker_initialized; 39 | bool draw_polygons_running, draw_obstacles_running, vis_path_running; 40 | 41 | ros::Publisher marker_pub; 42 | ros::Publisher marker_pub_obs; 43 | ros::Publisher marker_pub_path; 44 | 45 | ros::NodeHandle n; 46 | 47 | boost::thread* th_draw_polygons; 48 | boost::thread* th_draw_obstacles; 49 | boost::thread* th_vis_path; 50 | 51 | private: 52 | 53 | visualization_msgs::Marker line_strip; 54 | 55 | visualization_msgs::Marker line_strip_path; 56 | 57 | //vector obs_line_strip; 58 | 59 | visualization_msgs::MarkerArray obs_line_strip; 60 | 61 | 62 | }; 63 | 64 | visualize::visualize(ros::NodeHandle& nh) 65 | { 66 | n = nh; 67 | marker_initialize = 0; 68 | obs_marker_initialized = 0; 69 | path_marker_initialized = 0; 70 | marker_pub = n.advertise("visualization_marker", 10); 71 | marker_pub_obs = n.advertise("visualization_marker_obs", 1); 72 | marker_pub_path = n.advertise("visualization_marker_path", 10); 73 | draw_polygons_running = 1; 74 | draw_obstacles_running = 1; 75 | vis_path_running = 1; 76 | th_draw_polygons = new boost::thread(&visualize::draw_polygon, this); 77 | th_draw_obstacles = new boost::thread(&visualize::draw_obstacles, this); 78 | th_vis_path = new boost::thread(&visualize::vis_path, this); 79 | } 80 | 81 | visualize::~visualize() 82 | { 83 | delete th_draw_polygons; 84 | delete th_draw_obstacles; 85 | delete th_vis_path; 86 | cout<<"threads deleted\n"; 87 | } 88 | 89 | 90 | void visualize::initialize_marker_variables_polygon(bool vertices_done, const environment& env) 91 | { 92 | line_strip.header.frame_id = "map"; 93 | line_strip.ns = "polygon"; 94 | line_strip.action = visualization_msgs::Marker::ADD; 95 | line_strip.pose.orientation.w = 1.0; 96 | line_strip.id = 1; 97 | line_strip.type = visualization_msgs::Marker::LINE_STRIP; 98 | line_strip.scale.x = 0.1; 99 | line_strip.color.b = 1.0; 100 | line_strip.color.a = 1.0; 101 | if (vertices_done) 102 | { 103 | geometry_msgs::Point p; 104 | line_strip.header.stamp = ros::Time::now(); 105 | for (int i = 0; i < env.pboundary.vertices.size(); i++) 106 | { 107 | p.x = env.pboundary.vertices[i].x; 108 | p.y = env.pboundary.vertices[i].y; 109 | p.z = 0.0; 110 | line_strip.points.push_back(p); 111 | } 112 | if (env.pboundary.vertices.size() > 0) 113 | { 114 | p.x = env.pboundary.vertices[0].x; 115 | p.y = env.pboundary.vertices[0].y; 116 | line_strip.points.push_back(p); 117 | } 118 | cout<<"initialized marker variables\n"; 119 | } 120 | marker_initialize = 1; 121 | } 122 | 123 | void visualize::initialize_marker_variables_obstacles(int count_obstacles, const environment& env) 124 | { 125 | // initializing marker variables 126 | obs_line_strip.markers.resize(env.obstacles.size()); 127 | for (int i = 0; i < env.obstacles.size(); i++) 128 | { 129 | geometry_msgs::Point p; 130 | obs_line_strip.markers[i].header.frame_id = "map"; 131 | stringstream ss; 132 | ss << i; 133 | string str = ss.str(); 134 | string ns = "polygon" + str; 135 | obs_line_strip.markers[i].ns = ns; 136 | obs_line_strip.markers[i].action = visualization_msgs::Marker::ADD; 137 | obs_line_strip.markers[i].pose.orientation.w = 1.0; 138 | obs_line_strip.markers[i].id = i; 139 | obs_line_strip.markers[i].type = visualization_msgs::Marker::LINE_STRIP; 140 | obs_line_strip.markers[i].scale.x = 0.1; 141 | obs_line_strip.markers[i].color.g = 1.0; 142 | obs_line_strip.markers[i].color.a = 1.0; 143 | for (int j = 0; j < env.obstacles[i].vertices.size(); j++) 144 | { 145 | p.x = env.obstacles[i].vertices[j].x; 146 | p.y = env.obstacles[i].vertices[j].y; 147 | p.z = 0.0; 148 | obs_line_strip.markers[i].points.push_back(p); 149 | } 150 | if (env.obstacles[i].vertices.size() > 0) 151 | { 152 | p.x = env.obstacles[i].vertices[0].x; 153 | p.y = env.obstacles[i].vertices[0].y; 154 | obs_line_strip.markers[i].points.push_back(p); 155 | } 156 | } 157 | cout<<"initialized obstacle marker variables\n"; 158 | obs_marker_initialized = env.obstacles.size() > 0 ? 1 : 0; 159 | 160 | } 161 | 162 | void visualize::initialize_marker_variables_path(const vector& plan) 163 | { 164 | geometry_msgs::Point p; 165 | p.z = 0.0; 166 | line_strip_path.header.frame_id = "map"; 167 | line_strip_path.ns = "path"; 168 | line_strip_path.action = visualization_msgs::Marker::ADD; 169 | line_strip_path.pose.orientation.w = 1.0; 170 | line_strip_path.id = 1; 171 | line_strip_path.type = visualization_msgs::Marker::LINE_STRIP; 172 | line_strip_path.scale.x = 0.1; 173 | line_strip_path.color.r = 1.0; 174 | line_strip_path.color.a = 1.0; 175 | line_strip_path.header.stamp = ros::Time::now(); 176 | for (int i = 0; i < plan.size(); i++) 177 | { 178 | p.x = plan[i].x; 179 | p.y = plan[i].y; 180 | line_strip_path.points.push_back(p); 181 | } 182 | cout<<"initialized path marker variables.\n"; 183 | path_marker_initialized = plan.size() > 0 ? 1 : 0; 184 | 185 | } 186 | 187 | void visualize::draw_polygon() 188 | { 189 | tf::TransformBroadcaster br; 190 | tf::Transform trans; 191 | trans.setOrigin(tf::Vector3(0.0,0.0,0.0)); 192 | tf::Quaternion q; 193 | q.setRPY(0,0,0); 194 | trans.setRotation(q); 195 | while(draw_polygons_running && ros::ok()) 196 | { 197 | br.sendTransform(tf::StampedTransform(trans, ros::Time::now(), "map", "myframe")); 198 | if (marker_initialize) 199 | { 200 | line_strip.header.stamp = ros::Time::now(); 201 | marker_pub.publish(line_strip); 202 | } 203 | } 204 | cout<<"exiting draw polygon thread\n"; 205 | } 206 | 207 | void visualize::draw_obstacles() 208 | { 209 | while(draw_obstacles_running && ros::ok()) 210 | { 211 | if (obs_marker_initialized) 212 | { 213 | for (int i = 0; i < obs_line_strip.markers.size(); i++) 214 | { 215 | obs_line_strip.markers[i].header.stamp = ros::Time::now(); 216 | } 217 | marker_pub_obs.publish(obs_line_strip); 218 | } 219 | 220 | } 221 | cout<<"exiting draw obstacles thread\n"; 222 | } 223 | 224 | void visualize::vis_path() 225 | { 226 | while(vis_path_running && ros::ok()) 227 | { 228 | ros::Rate r(2); 229 | r.sleep(); 230 | if (path_marker_initialized) 231 | { 232 | line_strip_path.header.stamp = ros::Time::now(); 233 | marker_pub_path.publish(line_strip_path); 234 | } 235 | } 236 | cout<<"exiting visualize path thread\n"; 237 | } 238 | 239 | #endif 240 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | coverage_planner 4 | 0.0.0 5 | The coverage_planner package 6 | 7 | 8 | 9 | 10 | kush 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | visualization_msgs 56 | tf 57 | roscpp 58 | rospy 59 | std_msgs 60 | visualization_msgs 61 | tf 62 | visualization_msgs 63 | tf 64 | roscpp 65 | rospy 66 | std_msgs 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /src/coverage_planner.cpp: -------------------------------------------------------------------------------- 1 | // Code written by Kush Prasad // 2 | // email: prasad.kush@gmail.com // 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | /* 11 | This code computes a coverage plan for a convex polygon environment with obstacles (also as convex polygons) 12 | The environment is taken as input from rviz and stored in environment variables 13 | The input environment is checked wether it is valid (convex polygon). If not, the program exits. 14 | The above is done in input_polygon() function which is called as a thread from the constructor. 15 | For a valid environment, environment variables (resolution, bounds) are initialized 16 | The coverage plan is then computed by a call to the compute_plan() function from the input_polygon() function 17 | 18 | Assumptions: 19 | 20 | 1) The environment is bounded by a convex polygon and contains obstacles as convex polygons 21 | 2) The robot only executes the motions up, left, down and right. 22 | 23 | */ 24 | 25 | coverage_planner::coverage_planner(ros::NodeHandle& nh) : vis(nh) 26 | { 27 | 28 | n = nh; 29 | 30 | count_vertices = 0; 31 | vertices_done = 0; 32 | count_obstacles = 0; 33 | obstacles_done = 0; 34 | 35 | exit_condition = false; 36 | 37 | define_motions(); 38 | 39 | sub_polygon = n.subscribe("/initialpose", 1, &coverage_planner::vertices_callback, this); 40 | 41 | th_input = new boost::thread(&coverage_planner::input_polygon, this); 42 | 43 | } 44 | 45 | coverage_planner::~coverage_planner() 46 | { 47 | exit_condition = true; 48 | delete th_input; 49 | } 50 | 51 | 52 | void coverage_planner::define_motions() 53 | { 54 | motions.resize(4); 55 | motions[0] = "down"; 56 | motions[1] = "right"; 57 | motions[2] = "up"; 58 | motions[3] = "left"; 59 | } 60 | 61 | void coverage_planner::exit_gracefully(coverage_planner* c) 62 | { 63 | ros::Rate r(2); 64 | //while(ros::ok()) 65 | //{ 66 | cout<<"exit_condition: "<exit_condition<<"\n"; 67 | if (c->exit_condition) 68 | { 69 | cout<<"exiting due to invalid input\n"; 70 | c->vis.draw_polygons_running = false; 71 | c->vis.draw_obstacles_running = false; 72 | c->vis.vis_path_running = false; 73 | c->vis.marker_initialize = false; 74 | c->vis.obs_marker_initialized = false; 75 | c->vis.path_marker_initialized = false; 76 | c->vis.th_draw_polygons->join(); 77 | c->vis.th_draw_obstacles->join(); 78 | c->vis.th_vis_path->join(); 79 | //c->th_input->join(); 80 | cout<<"all threads done\n"; 81 | ros::shutdown(); 82 | exit(0); 83 | } 84 | //r.sleep(); 85 | //} 86 | } 87 | 88 | // this function is the subscriber callback function for input values from rviz 89 | // It stores the values in environment variables 90 | 91 | void coverage_planner::vertices_callback(const geometry_msgs::PoseWithCovarianceStamped& msg) 92 | { 93 | cout<<"received point\n"; 94 | if (vertices_done == 0) 95 | { 96 | vertex input_vert(msg.pose.pose.position.x, msg.pose.pose.position.y); 97 | env.pboundary.vertices.push_back(input_vert); 98 | count_vertices += 1; 99 | } 100 | if (vertices_done == 1 && no_obstacles > 0) 101 | { 102 | vertex input_vert(msg.pose.pose.position.x, msg.pose.pose.position.y); 103 | env.obstacles[count_obstacles].vertices.push_back(input_vert); 104 | } 105 | if (vertices_done == 1 && obstacles_done == 1) 106 | { 107 | vertex input_vert(msg.pose.pose.position.x, msg.pose.pose.position.y); 108 | test_vertex = input_vert; 109 | cout<<"point x: "<>n; 124 | vertices_done = 1; 125 | no_obstacles = 0; 126 | r.sleep(); 127 | bool polygonvalid = valid_polygon(env.pboundary); 128 | cout<<"polygon valid: "<>no_obstacles; 138 | cout<<"no of obstacles: "< 0) 146 | { 147 | env.obstacles.resize(max(0,no_obstacles)); 148 | for (int i = 0; i < no_obstacles; i++) 149 | { 150 | cout<<"\nPlease click polygon obstacle points on rviz using 2D Pose Estimate button and then enter a digit.\n"; 151 | cin>>n; 152 | count_obstacles += 1; 153 | } 154 | cout<<"Note: only convex and polygon obstacles will be considered.\n"; 155 | r.sleep(); 156 | vector::iterator it; 157 | it = env.obstacles.begin(); 158 | while (it != env.obstacles.end()) // this loop erases invalid (non polygon and non-convex) obstacles 159 | { 160 | if (!valid_polygon(*it)) 161 | { 162 | it = env.obstacles.erase(it); 163 | } 164 | else 165 | { 166 | it++; 167 | } 168 | } 169 | cout<<"no of valid obstacles: "<= 0 ? 1 : -1; 216 | count++; 217 | } 218 | if (count > 1 && valsign*prevvalsign < 0) 219 | { return false; } 220 | } 221 | } 222 | } 223 | if (psize <= 2) 224 | { return false; } 225 | 226 | return true; 227 | } 228 | 229 | //this function determines if a point lies within a polygon 230 | 231 | bool coverage_planner::point_within_polygon(double x, double y, const polygon& p) 232 | { 233 | double a, b, c, val, pointval; 234 | int psize = p.vertices.size(), valsign, pointvalsign, count; 235 | for (int i = 0; i < psize; i++) 236 | { 237 | int k = (i + 1) % psize; 238 | compute_line_eqn(a,b,c,p.vertices[i],p.vertices[k]); 239 | pointval = (a*x + b*y + c)/sqrt(a*a + b*b); 240 | if (a != 0 || b != 0) 241 | { 242 | count = 0; 243 | for (int j = 0; j < p.vertices.size(); j++) 244 | { 245 | valsign = 1; pointvalsign = 1; 246 | if (j != i && j != k) 247 | { 248 | val = (a*p.vertices[j].x + b*p.vertices[j].y + c)/sqrt(a*a + b*b); 249 | valsign = val >= 0 ? 1 : -1; 250 | pointvalsign = pointval >= 0 ? 1 : -1; 251 | count++; 252 | } 253 | if (valsign * pointvalsign < 0) 254 | { 255 | return false; 256 | } 257 | } 258 | } 259 | } 260 | if (psize <= 2) 261 | { return false; } 262 | 263 | return true; 264 | } 265 | 266 | 267 | //this function determines if two line segments intersect 268 | 269 | bool coverage_planner::line_segment_intersect(const vertex& v1, const vertex& v2, const vertex& v3, const vertex& v4) 270 | { 271 | double a1, b1, c1, a2, b2, c2, m1, m2, x_intersect, y_intersect; 272 | compute_line_eqn(a1,b1,c1,v1,v2); 273 | compute_line_eqn(a2,b2,c2,v3,v4); 274 | if ((a2*b1 - a1*b2) == 0.0) 275 | { 276 | return false; 277 | } 278 | else 279 | { 280 | x_intersect = (b2*c1 - b1*c2)/(a2*b1 - b2*a1); 281 | y_intersect = (a1*c2 - a2*c1)/(a2*b1 - b2*a1); 282 | double alpha1, alpha2; 283 | alpha1 = b1 != 0.0 ? (x_intersect - v1.x)/(v2.x - v1.x) : (y_intersect - v1.y)/(v2.y - v1.y); 284 | alpha2 = b2 != 0.0 ? (x_intersect - v3.x)/(v4.x - v3.x) : (y_intersect - v3.y)/(v4.y - v3.y); 285 | if (alpha1 >= 0 && alpha1 <= 1 && alpha2 >= 0 && alpha2 <= 1) 286 | { 287 | return true; 288 | } 289 | else 290 | { 291 | return false; 292 | } 293 | } 294 | 295 | } 296 | 297 | 298 | // this function computes equation of a line given two points and stores the eqn ax + by + c = 0 in a,b,c. 299 | 300 | void coverage_planner::compute_line_eqn(double& a, double& b, double& c, const vertex& v1, const vertex& v2) 301 | { 302 | if (v1.x == v2.x && v1.y == v2.y) 303 | { 304 | a = 0.0; b = 0.0; c = 0.0; 305 | } 306 | if (v1.x == v2.x && v1.y != v2.y) 307 | { 308 | a = 1.0; 309 | b = 0.0; 310 | c = -v1.x; 311 | } 312 | else if (v1.x != v2.x && v1.y == v2.y) 313 | { 314 | a = 0.0; 315 | b = 1.0; 316 | c = -v1.y; 317 | } 318 | else 319 | { 320 | double slope; 321 | slope = (v2.y - v1.y)/(v2.x - v1.x); 322 | a = -slope; 323 | b = 1.0; 324 | c = -(a*v2.x + b*v2.y); 325 | } 326 | } 327 | 328 | //this function determines wether a given point is within any obstace in the environment 329 | 330 | bool coverage_planner::within_obstacle(double x, double y, const environment& e) 331 | { 332 | if (!point_within_polygon(x,y,e.pboundary)) 333 | { 334 | return true; 335 | } 336 | for (int i = 0; i < e.obstacles.size(); i++) 337 | { 338 | if(point_within_polygon(x,y,e.obstacles[i])) 339 | { 340 | return true; 341 | } 342 | } 343 | return false; 344 | } 345 | 346 | 347 | //this function stores discrete nodes in a set and in a map 348 | 349 | void coverage_planner::make_graph(environment& e, discr_environment& d) 350 | { 351 | e.initialize_environment(); 352 | d.x_cells = floor((abs(e.xmax - e.xmin))/e.resolution); 353 | d.y_cells = floor((abs(e.ymax - e.ymin))/e.resolution); 354 | cout<<"x_cells: "< neighbors; 365 | disc_point p(i,j), adj; 366 | n.p = p; 367 | n.nodereached = false; 368 | for (int k = 0; k < motions.size(); k++) 369 | { 370 | adj = compute_adjacent(p, motions[k]); 371 | discrete_to_continuous(adj.x,adj.y,x,y,e); 372 | if (within_bounds(adj,d.x_cells,d.y_cells) && !within_obstacle(x,y,e)) 373 | { 374 | neighbors.push_back(adj); 375 | } 376 | } 377 | n.neighbors = neighbors; 378 | d.pointset.insert(p); 379 | d.graphnodes[p] = n; 380 | } 381 | } 382 | } 383 | } 384 | 385 | 386 | void coverage_planner::compute_source(vertex& v) 387 | { 388 | double xmin = env.pboundary.vertices[0].x; 389 | v = env.pboundary.vertices[0]; 390 | for (int i = 0; i < env.pboundary.vertices.size(); i++) 391 | { 392 | if (env.pboundary.vertices[i].x < xmin) 393 | { 394 | xmin = env.pboundary.vertices[i].x; 395 | v = env.pboundary.vertices[i]; 396 | } 397 | } 398 | } 399 | 400 | //compute adjacent node given current node and a motion 401 | 402 | disc_point coverage_planner::compute_adjacent(const disc_point& p, string m) 403 | { 404 | if (m == "down") 405 | { 406 | disc_point adj(p.x,p.y-1); 407 | return adj; 408 | } 409 | else if (m == "right") 410 | { 411 | disc_point adj(p.x+1,p.y); 412 | return adj; 413 | } 414 | else if (m == "up") 415 | { 416 | disc_point adj(p.x,p.y+1); 417 | return adj; 418 | } 419 | else if (m == "left") 420 | { 421 | disc_point adj(p.x-1,p.y); 422 | return adj; 423 | } 424 | else 425 | { 426 | disc_point adj; 427 | adj.x = p.x; adj.y = p.y; 428 | return adj; 429 | } 430 | } 431 | 432 | bool coverage_planner::within_bounds(disc_point p, int x_cells, int y_cells) 433 | { 434 | return (p.x >= 0 && p.x <= x_cells) && (p.y >= 0 && p.y <= y_cells); 435 | } 436 | 437 | 438 | void coverage_planner::shortest_path(disc_point s, disc_point g, vector& plan_, discr_environment& d) 439 | { 440 | map::iterator it; 441 | queue q; 442 | disc_point current; 443 | double x, y; 444 | geometry_msgs::Point p; 445 | p.z = 0.0; 446 | for (it = d.graphnodes.begin(); it != d.graphnodes.end(); it++) 447 | { 448 | it->second.inqueue = false; 449 | } 450 | q.push(s); 451 | d.graphnodes[s].inqueue = true; 452 | current = s; 453 | while (!q.empty() && current != g) 454 | { 455 | vector neigh; 456 | neigh = d.graphnodes[current].neighbors; 457 | for (int i = 0; i < neigh.size(); i++) 458 | { 459 | if (!d.graphnodes[neigh[i]].inqueue) 460 | { q.push(neigh[i]); 461 | d.graphnodes[neigh[i]].inqueue = true; 462 | d.graphnodes[neigh[i]].qparent = current; 463 | } 464 | } 465 | q.pop(); 466 | current.x = q.front().x; 467 | current.y = q.front().y; 468 | } 469 | vector temp_plan; 470 | if (current == g && s != g) 471 | { 472 | while (current != s) 473 | { 474 | discrete_to_continuous(current.x,current.y,x,y,env); 475 | p.x = x; 476 | p.y = y; 477 | temp_plan.push_back(p); 478 | current = d.graphnodes[current].qparent; 479 | } 480 | if (temp_plan.size() > 0) 481 | { 482 | for (int i = temp_plan.size() - 1; i >= 0; i--) 483 | { 484 | plan_.push_back(temp_plan[i]); 485 | } 486 | } 487 | } 488 | 489 | } 490 | 491 | bool coverage_planner::add_node(disc_point adj, disc_point current, const environment& e, discr_environment& d) 492 | { 493 | double x,y; 494 | discrete_to_continuous(adj.x,adj.y,x,y,e); 495 | if (within_bounds(adj,d.x_cells,d.y_cells) && !within_obstacle(x,y,e) && d.graphnodes[adj].nodereached == 0) 496 | { 497 | d.graphnodes[adj].parent = current; 498 | d.graphnodes[current].child = adj; 499 | d.graphnodes[adj].nodereached = true; 500 | return true; 501 | } 502 | return false; 503 | } 504 | 505 | void coverage_planner::iterate_over_adjacent_nodes_(bool& edge_added, disc_point& adj, disc_point& current) 506 | { 507 | double x, y; 508 | for (int i = 0; i < motions.size(); i++) 509 | { 510 | if (!edge_added) 511 | { 512 | adj = compute_adjacent(current,motions[i]); 513 | discrete_to_continuous(adj.x,adj.y,x,y,env); 514 | if (within_bounds(adj,envd.x_cells,envd.y_cells) && !within_obstacle(x,y,env) && envd.graphnodes[adj].nodereached == 0) 515 | { edge_added = true; } 516 | } 517 | if (edge_added) 518 | { 519 | break; 520 | } 521 | } 522 | } 523 | 524 | void coverage_planner::iterate_over_adjacent_nodes(bool& edge_added, disc_point& adj, disc_point& current) 525 | { 526 | for (int i = 0; i < motions.size(); i++) 527 | { 528 | if (!edge_added) 529 | { 530 | adj = compute_adjacent(current,motions[i]); 531 | edge_added = add_node(adj,current,env,envd); 532 | } 533 | if (edge_added) 534 | { 535 | current = adj; 536 | break; 537 | } 538 | } 539 | } 540 | 541 | void coverage_planner::compute_plan() 542 | { 543 | vertex v; 544 | disc_point source, current, adj; 545 | geometry_msgs::Point p; 546 | int no_of_jumps = 0; 547 | p.z = 0.0; 548 | double x, y; 549 | set::iterator it; 550 | make_graph(env,envd); // fill discrete valid node values in a set and a map 551 | it = envd.pointset.begin(); 552 | current.x = it->x; 553 | current.y = it->y; 554 | source = current; 555 | envd.graphnodes[current].nodereached = true; 556 | discrete_to_continuous(current.x,current.y,p.x,p.y,env); 557 | plan.push_back(p); 558 | while (!envd.pointset.empty()) 559 | { 560 | bool edge_added = false; 561 | it = envd.pointset.find(current); //remove node being expanded from set 562 | if (it != envd.pointset.end()) 563 | { envd.pointset.erase(it); } 564 | iterate_over_adjacent_nodes(edge_added,adj,current); // consider adjacent nodes for motion in counter-clock wise direction 565 | if (edge_added) // only obstacle free and nodes not yet reached are considered valid 566 | { 567 | discrete_to_continuous(adj.x,adj.y,p.x,p.y,env); 568 | plan.push_back(p); // add to plan if a valid adjacent node is found 569 | continue; 570 | } 571 | else 572 | { 573 | if (current == source) 574 | { 575 | break; 576 | } 577 | else 578 | { 579 | disc_point start; 580 | start = current; 581 | while(!edge_added && current != source) // this loop entered when no obstacle free adjacent node is ... 582 | { // ... present. we then go back to a node in the path from which ... 583 | current = envd.graphnodes[current].parent; // ... a valid adjacent node is reachable 584 | iterate_over_adjacent_nodes_(edge_added,adj,current); 585 | if (edge_added) 586 | { 587 | shortest_path(start,current,plan,envd); 588 | discrete_to_continuous(current.x,current.y,p.x,p.y,env); 589 | plan.push_back(p); 590 | no_of_jumps += 1; 591 | } 592 | } 593 | } 594 | } 595 | 596 | } 597 | vis.initialize_marker_variables_path(plan); 598 | output_plan(plan); 599 | cout<<"\nsolution found\n"; 600 | cout<<"plan size: "<& plan) 607 | { 608 | for (int i = 0; i < plan.size(); i++) 609 | { 610 | cout< 5 | #include 6 | #include 7 | #include 8 | 9 | // This code instantiates an object of the coverage_planner class 10 | // The constructor of the coverage_planner class calls the input_polygon() function from which ... 11 | // ... compute_plan() function is called which computes the coverage plan 12 | 13 | void spinthread() 14 | { 15 | ros::spin(); 16 | } 17 | 18 | void signalHandler(int sig) 19 | { 20 | ros::Rate r(2); 21 | ros::shutdown(); 22 | exit(sig); 23 | } 24 | 25 | int main(int argc, char** argv) 26 | { 27 | ros::init(argc, argv, "planner"); 28 | tf::TransformListener tf_(ros::Duration(10)); 29 | ros::NodeHandle n; 30 | ros::Rate r(10); 31 | signal(SIGINT, signalHandler); 32 | coverage_planner planner(n); 33 | boost::thread th(spinthread); 34 | planner.th_input->join(); 35 | while (1) 36 | { 37 | r.sleep(); 38 | if (planner.exit_condition) 39 | { 40 | coverage_planner::exit_gracefully(&planner); 41 | } 42 | } 43 | 44 | //ros::spin(); 45 | return 0; 46 | } 47 | 48 | 49 | --------------------------------------------------------------------------------