├── src ├── full_coverage_path_planner │ ├── __init__.py │ └── stroke_joins.py ├── full_coverage_path_planner.cpp ├── spiral_stc.cpp └── common.cpp ├── doc ├── bsa.png ├── robot_plus_tool.png ├── fcpp_robot_0_5m_plus_tool_0_2m.png ├── fcpp_robot_0_5m_plus_tool_0_5m.png └── robot_plus_tool.ipe ├── maps ├── grid.png ├── basement.png ├── grid.yaml └── basement.yaml ├── test ├── full_coverage_path_planner │ ├── param │ │ ├── planners.yaml │ │ ├── controllers.yaml │ │ └── local_costmap_params.yaml │ ├── test_full_coverage_path_planner.test │ ├── test_full_coverage_path_planner_plugin.launch │ ├── test_full_coverage_path_planner_system.py │ ├── test_full_coverage_path_planner.launch │ └── fcpp.rviz ├── simple_goal.yaml ├── README.md ├── include │ └── full_coverage_path_planner │ │ └── util.h └── src │ ├── util.cpp │ ├── test_common.cpp │ └── test_spiral_stc.cpp ├── .github └── workflows │ └── main.yml ├── fcpp_plugin.xml ├── package.xml ├── CMakeLists.txt ├── include └── full_coverage_path_planner │ ├── spiral_stc.h │ ├── common.h │ └── full_coverage_path_planner.h ├── README.md ├── nodes └── coverage_progress └── LICENSE /src/full_coverage_path_planner/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /doc/bsa.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nobleo/full_coverage_path_planner/HEAD/doc/bsa.png -------------------------------------------------------------------------------- /maps/grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nobleo/full_coverage_path_planner/HEAD/maps/grid.png -------------------------------------------------------------------------------- /maps/basement.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nobleo/full_coverage_path_planner/HEAD/maps/basement.png -------------------------------------------------------------------------------- /doc/robot_plus_tool.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nobleo/full_coverage_path_planner/HEAD/doc/robot_plus_tool.png -------------------------------------------------------------------------------- /doc/fcpp_robot_0_5m_plus_tool_0_2m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nobleo/full_coverage_path_planner/HEAD/doc/fcpp_robot_0_5m_plus_tool_0_2m.png -------------------------------------------------------------------------------- /doc/fcpp_robot_0_5m_plus_tool_0_5m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nobleo/full_coverage_path_planner/HEAD/doc/fcpp_robot_0_5m_plus_tool_0_5m.png -------------------------------------------------------------------------------- /maps/grid.yaml: -------------------------------------------------------------------------------- 1 | image: grid.png 2 | resolution: 0.05 3 | origin: [-5, -5, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/param/planners.yaml: -------------------------------------------------------------------------------- 1 | planners: 2 | - name: 'SpiralSTC' 3 | type: 'full_coverage_path_planner/SpiralSTC' 4 | -------------------------------------------------------------------------------- /maps/basement.yaml: -------------------------------------------------------------------------------- 1 | image: basement.png 2 | resolution: 0.050000 3 | origin: [-24.024998, -6.275000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/param/controllers.yaml: -------------------------------------------------------------------------------- 1 | controllers: 2 | - name: 'tracking_pid' 3 | type: 'tracking_pid/TrackingPidLocalPlanner' 4 | 5 | controller: 6 | holonomic_robot: false 7 | 8 | mbf_tolerance_check: true 9 | dist_tolerance: 0.2 10 | angle_tolerance: 0.5 11 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/param/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_link 4 | update_frequency: 10.0 5 | publish_frequency: 5.0 6 | width: 5.0 7 | height: 5.0 8 | resolution: 0.05 9 | static_map: false 10 | rolling_window: true 11 | -------------------------------------------------------------------------------- /test/simple_goal.yaml: -------------------------------------------------------------------------------- 1 | header: 2 | seq: 1 3 | stamp: 4 | secs: 0 5 | nsecs: 0 6 | frame_id: 'map' 7 | goal_id: 8 | stamp: 9 | secs: 0 10 | nsecs: 0 11 | id: '' 12 | goal: 13 | target_pose: 14 | header: 15 | seq: 0 16 | stamp: 17 | secs: 0 18 | nsecs: 0 19 | frame_id: 'map' 20 | pose: 21 | position: 22 | x: 0.0 23 | y: 0.0 24 | z: 0.0 25 | orientation: 26 | x: 0.0 27 | y: 0.0 28 | z: 0.0 29 | w: 1.0 30 | -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | industrial_ci: 7 | strategy: 8 | matrix: 9 | env: 10 | - {ROS_DISTRO: melodic, ROS_REPO: main, UPSTREAM_WORKSPACE: 'github:nobleo/tracking_pid#master'} 11 | - {ROS_DISTRO: noetic, ROS_REPO: main, UPSTREAM_WORKSPACE: 'github:nobleo/tracking_pid#master'} 12 | runs-on: ubuntu-latest 13 | steps: 14 | - uses: actions/checkout@v1 15 | - uses: 'ros-industrial/industrial_ci@master' 16 | env: ${{matrix.env}} 17 | -------------------------------------------------------------------------------- /fcpp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Plans a path that covers all accessible points in a costmap by using Spiral-STC. 5 | Spiral-STC works by following the walls and spiraling inwards until it cannot go further. 6 | Then it uses A* to go back outside of the current spiral and then spirals again. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /test/README.md: -------------------------------------------------------------------------------- 1 | Test for Full coverage path planner 2 | =================================== 3 | 4 | The full coverage path planner consists of several parts that are each tested separately. 5 | 6 | The move_base_flex plugin consists of several parts, each unit-tested separately: 7 | - test_common: tests common.h 8 | - test_spiral_stc: tests static functions of spiral_stc.h 9 | 10 | Besides unittests, there are also some launch files that both illustrate how to use the 11 | - SpiralSTC-plugin, in test/full_coverage_path_planner/test_full_coverage_path_planner.launch 12 | 13 | Note that the .launch-files do not do any automatic testing or verification of anything, 14 | they are there to make manual testing easier. 15 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/test_full_coverage_path_planner.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /test/include/full_coverage_path_planner/util.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | // Created by nobleo on 27-9-18. 5 | // 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #ifndef FULL_COVERAGE_PATH_PLANNER_UTIL_H 13 | #define FULL_COVERAGE_PATH_PLANNER_UTIL_H 14 | /** 15 | * Create a X*Y grid 16 | * 17 | * @param x number of elements in horizontal direction (columns) 18 | * @param y number of elements in vertical direction (rows) 19 | * @param fill what to fill the rows with? 20 | * @return a vector of vectors. The inner vector has size x, the outer vector contains y x-sized vectors 21 | */ 22 | std::vector > makeTestGrid(int x, int y, bool fill = false); 23 | /** 24 | * Fill a test grid with a fraction of random obstacles 25 | * @param grid a vector of vectors that will be modified to have obstacles (true elements) in random places 26 | * @param obstacle_fraction between 0 and 100, what is the percentage of cells that must be marked as obstacle 27 | * @return bool indicating success 28 | */ 29 | bool randomFillTestGrid(std::vector > &grid, float obstacle_fraction); 30 | 31 | bool operator==(const Point_t &lhs, const Point_t &rhs); 32 | 33 | struct CompareByPosition 34 | { 35 | bool operator()(const Point_t &lhs, const Point_t &rhs) 36 | { 37 | if (lhs.x != rhs.x) 38 | { 39 | return lhs.x < rhs.x; 40 | } 41 | return lhs.y < rhs.y; 42 | } 43 | }; 44 | 45 | #endif // FULL_COVERAGE_PATH_PLANNER_UTIL_H 46 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | full_coverage_path_planner 4 | 0.6.4 5 | Full coverage path planning provides a move_base_flex plugin that can plan a path that will fully cover a given area 6 | Cesar Lopez 7 | Ferry Schoenmakers 8 | Tim Clephas 9 | Jerrel Unkel 10 | Loy van Beek 11 | Yury Brodskiy 12 | Cesar Lopez 13 | 14 | Apache 2.0 15 | http://wiki.ros.org/full_coverage_path_planner 16 | 17 | catkin 18 | roslint 19 | rostest 20 | base_local_planner 21 | costmap_2d 22 | pluginlib 23 | nav_core 24 | roscpp 25 | tf2_ros 26 | amcl 27 | joint_state_publisher 28 | map_server 29 | move_base 30 | move_base_flex 31 | cv_bridge 32 | rosunit 33 | tracking_pid 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /test/src/util.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | // Created by nobleo on 27-9-18. 5 | // 6 | 7 | #include 8 | #include 9 | 10 | std::vector > makeTestGrid(int x, int y, bool fill) 11 | { 12 | std::vector > grid; 13 | for (int i = 0; i < y; ++i) 14 | { 15 | std::vector row; 16 | for (int j = 0; j < x; ++j) 17 | { 18 | row.push_back(fill); 19 | } 20 | grid.push_back(row); 21 | } 22 | return grid; 23 | } 24 | 25 | bool operator==(const Point_t &lhs, const Point_t &rhs) 26 | { 27 | return lhs.x == rhs.x && lhs.y == rhs.y; 28 | } 29 | 30 | bool randomFillTestGrid(std::vector > &grid, float obstacle_fraction) 31 | { 32 | unsigned int seed = time(NULL); 33 | int max_y = grid.size(); 34 | if (max_y < 1) 35 | { 36 | // Cannot work on less than a row 37 | return false; 38 | } 39 | int max_x = grid.front().size(); 40 | if (max_x < 1) 41 | { 42 | // Cannot work on less than a column 43 | return false; 44 | } 45 | 46 | int total_cells = max_y * max_x; 47 | int total_obstacles = total_cells * (obstacle_fraction / 100); 48 | // std::cout << "total_cells: " << total_cells << ", total_obstacles: " << total_obstacles << std::endl; 49 | 50 | // For the amount of obstacles we need to create, generate random coordinates and insert an obstacle 51 | for (int i = 0; i < total_obstacles; ++i) 52 | { 53 | int x = rand_r(&seed) % max_x; 54 | int y = rand_r(&seed) % max_y; 55 | // std::cout << "Obstacle at (" << x << ", " << y << ")" << std::endl; 56 | grid[y][x] = true; 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /src/full_coverage_path_planner/stroke_joins.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from math import sqrt 3 | 4 | def half_circle(start, end, previous=None): 5 | """Generate a half circle between start and end, returns interpolated vector i and half-circle j coordinates""" 6 | 7 | r = (end - start) / 2 8 | for i in np.linspace(start, end)[1:-1]: 9 | # circle formula: j = sqrt(r^2 - (i-a)^2) + b 10 | # a is offset in i and in our case identical to the radius 11 | # b is offset in j and not relevant in our case 12 | j = sqrt(r ** 2 - (i - start - r) ** 2) 13 | yield (i, j) 14 | 15 | 16 | def mwm(start, end, previous, v_depth=1.0, v_bottom_off_center=0.0): 17 | """Generate a path that connects two strokes via a V, so that the two strokes combined look like an M, 18 | with a small v in the middle 19 | :param start: one top of the v 20 | :param end: the other top of the v 21 | :param previous: To indicate direction of the V (to not make a ^), previous is the point that comes before the start of the V (so the bottom-left point of an M or top-left in a W) 22 | :return: yields all the points of the v (so the bottom of the v) 23 | 24 | >>> mwm(np.array((1, 2)), np.array((2, 3)), np.array((3, 0))) 25 | array([ 2., 2.]) 26 | """ 27 | 28 | # This needs to know in which direction to put the V. This may or may not be alternating even: 29 | # The pattern could be M M M but just as well M M M 30 | # W W (v point up) V V with the inner-v point 31 | start, end, previous = np.array(start), np.array(end), np.array(previous) 32 | 33 | r = (end - start) / 2. 34 | middle = start + r 35 | 36 | delta_ver = previous - start 37 | delta_hor = start - end 38 | 39 | distance_hor = np.linalg.norm(delta_hor) 40 | distance_ver = np.linalg.norm(delta_ver) 41 | 42 | normalized_delta_hor = (delta_hor / distance_hor) if np.any(delta_hor) else 0.0 43 | normalized_delta_ver = (delta_ver / distance_ver) if np.any(delta_ver) else 0.0 44 | 45 | # try: 46 | v = middle + (normalized_delta_ver * distance_hor * v_depth) 47 | v2 = v + (normalized_delta_hor * v_bottom_off_center * r) 48 | 49 | yield tuple(v2) -------------------------------------------------------------------------------- /test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(full_coverage_path_planner) 3 | add_compile_options(-std=c++11) 4 | 5 | find_package(catkin REQUIRED 6 | COMPONENTS 7 | base_local_planner 8 | costmap_2d 9 | nav_core 10 | pluginlib 11 | roscpp 12 | roslint 13 | rostest 14 | tf 15 | ) 16 | 17 | include_directories( 18 | include 19 | test/include 20 | ${catkin_INCLUDE_DIRS} 21 | ) 22 | add_definitions(${EIGEN3_DEFINITIONS}) 23 | 24 | catkin_package( 25 | INCLUDE_DIRS include 26 | LIBRARIES ${PROJECT_NAME} 27 | CATKIN_DEPENDS 28 | base_local_planner 29 | costmap_2d 30 | nav_core 31 | pluginlib 32 | roscpp 33 | ) 34 | 35 | add_library(${PROJECT_NAME} 36 | src/common.cpp 37 | src/${PROJECT_NAME}.cpp 38 | src/spiral_stc.cpp 39 | ) 40 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 41 | target_link_libraries(${PROJECT_NAME} 42 | ${catkin_LIBRARIES} 43 | ) 44 | 45 | install(TARGETS 46 | ${PROJECT_NAME} 47 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 48 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 49 | ) 50 | 51 | install(DIRECTORY include/${PROJECT_NAME} 52 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 53 | ) 54 | 55 | install(FILES fcpp_plugin.xml 56 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 57 | ) 58 | 59 | catkin_install_python( 60 | PROGRAMS 61 | nodes/coverage_progress 62 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | if (CATKIN_ENABLE_TESTING) 66 | catkin_add_gtest(test_common test/src/test_common.cpp test/src/util.cpp src/common.cpp) 67 | 68 | catkin_add_gtest(test_spiral_stc test/src/test_spiral_stc.cpp test/src/util.cpp src/spiral_stc.cpp src/common.cpp src/${PROJECT_NAME}.cpp) 69 | add_dependencies(test_spiral_stc ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 70 | target_link_libraries(test_spiral_stc ${catkin_LIBRARIES}) 71 | 72 | find_package(OpenCV) 73 | include_directories(${OpenCV_INCLUDE_DIRS}) 74 | target_link_libraries(test_spiral_stc ${OpenCV_LIBRARIES}) 75 | 76 | add_rostest(test/${PROJECT_NAME}/test_${PROJECT_NAME}.test) 77 | 78 | endif() 79 | 80 | roslint_cpp() 81 | -------------------------------------------------------------------------------- /include/full_coverage_path_planner/spiral_stc.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using std::string; 22 | 23 | #ifndef FULL_COVERAGE_PATH_PLANNER_SPIRAL_STC_H 24 | #define FULL_COVERAGE_PATH_PLANNER_SPIRAL_STC_H 25 | 26 | #include "full_coverage_path_planner/full_coverage_path_planner.h" 27 | namespace full_coverage_path_planner 28 | { 29 | class SpiralSTC : public nav_core::BaseGlobalPlanner, private full_coverage_path_planner::FullCoveragePathPlanner 30 | { 31 | public: 32 | /** 33 | * Find a path that spirals inwards from init until an obstacle is seen in the grid 34 | * @param grid 2D grid of bools. true == occupied/blocked/obstacle 35 | * @param init start position 36 | * @param visited all the nodes visited by the spiral 37 | * @return list of nodes that form the spiral 38 | */ 39 | static std::list spiral(std::vector > const &grid, std::list &init, 40 | std::vector > &visited); 41 | 42 | /** 43 | * Perform Spiral-STC (Spanning Tree Coverage) coverage path planning. 44 | * In essence, the robot moves forward until an obstacle or visited node is met, then turns right (making a spiral) 45 | * When stuck in the middle of the spiral, use A* to get out again and start a new spiral, until a* can't find a path to uncovered cells 46 | * @param grid 47 | * @param init 48 | * @return 49 | */ 50 | static std::list spiral_stc(std::vector > const &grid, 51 | Point_t &init, 52 | int &multiple_pass_counter, 53 | int &visited_counter); 54 | 55 | private: 56 | /** 57 | * @brief Given a goal pose in the world, compute a plan 58 | * @param start The start pose 59 | * @param goal The goal pose 60 | * @param plan The plan... filled by the planner 61 | * @return True if a valid plan was found, false otherwise 62 | */ 63 | bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, 64 | std::vector &plan); 65 | 66 | /** 67 | * @brief Initialization function for the FullCoveragePathPlanner object 68 | * @param name The name of this planner 69 | * @param costmap A pointer to the ROS wrapper of the costmap to use for planning 70 | */ 71 | void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); 72 | }; 73 | 74 | } // namespace full_coverage_path_planner 75 | #endif // FULL_COVERAGE_PATH_PLANNER_SPIRAL_STC_H 76 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/test_full_coverage_path_planner_system.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from geometry_msgs.msg import PoseStamped 4 | from nav_msgs.msg import Path 5 | from std_msgs.msg import Bool, Float32 6 | from tracking_pid.msg import traj_point, PidDebug 7 | import math 8 | import numpy as np 9 | import rospy 10 | import sys 11 | import tf 12 | import unittest 13 | 14 | PKG = 'tracking_pid' 15 | 16 | 17 | class TestTrackingPID(unittest.TestCase): 18 | 19 | def setUp(self): 20 | rospy.init_node("rostest_tracking_pid_node") 21 | self.trajectory_finished_sub = rospy.Subscriber("trajectory_finished", Bool, 22 | self.trajectory_finished_callback, queue_size=1) 23 | self.listener = tf.TransformListener() 24 | self.trajectory_finished = False 25 | 26 | def trajectory_finished_callback(self, trajectory_finished_msg): 27 | rospy.loginfo("Trajectory finished message received on topic") 28 | self.trajectory_finished = trajectory_finished_msg.data 29 | 30 | def quaternion_to_yaw(self, quaternion_in): 31 | quaternion = ( 32 | quaternion_in.x, 33 | quaternion_in.y, 34 | quaternion_in.z, 35 | quaternion_in.w) 36 | euler = tf.transformations.euler_from_quaternion(quaternion) 37 | return (euler[2] + math.pi) % (2 * math.pi) - math.pi # wrap 38 | 39 | def test_tracking_pid(self): 40 | """ Several checks are done: 41 | - Test that interpolator point and robot start moving 42 | - Test that error at all times is bounded 43 | - Test that after some time final goal is reached 44 | A path that does not start along the y-axis is expected 45 | """ 46 | p1_msg = rospy.wait_for_message("trajectory", traj_point, timeout=5) 47 | p1_yaw = self.quaternion_to_yaw(p1_msg.pose.pose.orientation) 48 | self.listener.waitForTransform('map', 'base_link', rospy.Time(0), rospy.Duration(1.0)) 49 | (trans1, rot1) = self.listener.lookupTransform('map', 'base_link', rospy.Time(0)) 50 | rospy.sleep(2.0) 51 | p2_msg = rospy.wait_for_message("trajectory", traj_point, timeout=5) 52 | p2_yaw = self.quaternion_to_yaw(p2_msg.pose.pose.orientation) 53 | 54 | if (p1_msg.pose.pose.position.x == p2_msg.pose.pose.position.x and 55 | p1_msg.pose.pose.position.y == p2_msg.pose.pose.position.y and 56 | p1_yaw == p2_yaw): 57 | self.assertTrue(False, "Trajectory point has not moved") 58 | 59 | rospy.loginfo("Wait max 140 seconds for reaching goal") 60 | test_start_time = rospy.Time.now().to_sec() 61 | while rospy.Time.now().to_sec() - test_start_time < 140.0: 62 | self.debug_msg = rospy.wait_for_message("debug", PidDebug, timeout=5) 63 | error_vec = ( 64 | self.debug_msg.error.linear.x, 65 | self.debug_msg.error.linear.y, 66 | self.debug_msg.error.linear.z) 67 | error = np.linalg.norm(error_vec) 68 | self.assertLess(error, 0.5, "Linear error greater than 0.5 m") 69 | yaw_error = self.debug_msg.error.angular.z 70 | self.assertLess(yaw_error, 0.5, "Linear error greater than 0.5 rad") 71 | self.coverage_msg = rospy.wait_for_message("coverage_progress", Float32, timeout=5) 72 | if self.trajectory_finished is True: 73 | break 74 | 75 | self.assertTrue(self.trajectory_finished, "Trajectory not finished in 140 seconds") 76 | self.assertGreaterEqual(self.coverage_msg.data, 0.95, "Coverage less than 95%") 77 | 78 | 79 | if __name__ == '__main__': 80 | import rostest 81 | rostest.rosrun(PKG, 'rostest_tracking_pid_node', TestTrackingPID) 82 | -------------------------------------------------------------------------------- /include/full_coverage_path_planner/common.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | // Created by nobleo on 6-9-18. 5 | // 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #ifndef FULL_COVERAGE_PATH_PLANNER_COMMON_H 12 | #define FULL_COVERAGE_PATH_PLANNER_COMMON_H 13 | 14 | typedef struct 15 | { 16 | int x, y; 17 | } 18 | Point_t; 19 | 20 | inline std::ostream &operator << (std::ostream &os, Point_t &p) 21 | { 22 | return os << "(" << p.x << ", " << p.y << ")"; 23 | } 24 | 25 | typedef struct 26 | { 27 | Point_t pos; 28 | 29 | /** Path cost 30 | * cost of the path from the start node to gridNode_t 31 | */ 32 | int cost; 33 | 34 | /** Heuristic cost 35 | * cost of the cheapest path from this gridNode_t to the goal 36 | */ 37 | int he; 38 | } 39 | gridNode_t; 40 | 41 | inline std::ostream &operator << (std::ostream &os, gridNode_t &g) 42 | { 43 | return os << "gridNode_t(" << g.cost << ", " << g.he << ", " << g.pos << ")"; 44 | } 45 | 46 | 47 | typedef struct 48 | { 49 | float x, y; 50 | } 51 | fPoint_t; 52 | 53 | inline std::ostream &operator << (std::ostream &os, fPoint_t &p) 54 | { 55 | return os << "(" << p.x << ", " << p.y << ")"; 56 | } 57 | 58 | enum 59 | { 60 | eNodeOpen = false, 61 | eNodeVisited = true 62 | }; 63 | 64 | /** 65 | * Find the distance from poi to the closest point in goals 66 | * @param poi Starting point 67 | * @param goals Potential next points to find the closest of 68 | * @return Distance to the closest point (out of 'goals') to 'poi' 69 | */ 70 | int distanceToClosestPoint(Point_t poi, std::list const &goals); 71 | 72 | /** 73 | * Calculate the distance between two points, squared 74 | */ 75 | int distanceSquared(const Point_t &p1, const Point_t &p2); 76 | 77 | /** 78 | * Perform A* shorted path finding from init to one of the points in heuristic_goals 79 | * @param grid 2D grid of bools. true == occupied/blocked/obstacle 80 | * @param init start position 81 | * @param cost cost of traversing a free node 82 | * @param visited grid 2D grid of bools. true == visited 83 | * @param open_space Open space that A* need to find a path towards. Only used for the heuristic and directing search 84 | * @param pathNodes nodes that form the path from init to the closest point in heuristic_goals 85 | * @return whether we resign from finding a path or not. true is we resign and false if we found a path 86 | */ 87 | bool a_star_to_open_space(std::vector > const &grid, gridNode_t init, int cost, 88 | std::vector > &visited, std::list const &open_space, 89 | std::list &pathNodes); 90 | 91 | /** 92 | * Print a grid according to the internal representation 93 | * @param grid 94 | * @param visited 95 | * @param fullPath 96 | */ 97 | void printGrid(std::vector > const& grid, 98 | std::vector > const& visited, 99 | std::list const& path); 100 | 101 | /** 102 | * Print a grid according to the internal representation 103 | * @param grid 104 | * @param visited 105 | * @param fullPath 106 | * @param start 107 | * @param end 108 | */ 109 | void printGrid(std::vector > const& grid, 110 | std::vector > const& visited, 111 | std::list const& path, 112 | gridNode_t start, 113 | gridNode_t end); 114 | 115 | /** 116 | * Print a 2D array of bools to stdout 117 | */ 118 | void printGrid(std::vector > const& grid); 119 | 120 | /** 121 | * Convert 2D grid of bools to a list of Point_t 122 | * @param grid 2D grid representing a map 123 | * @param value_to_search points matching this value will be returned 124 | * @return a list of points that have the given value_to_search 125 | */ 126 | std::list map_2_goals(std::vector > const& grid, bool value_to_search); 127 | #endif // FULL_COVERAGE_PATH_PLANNER_COMMON_H 128 | -------------------------------------------------------------------------------- /include/full_coverage_path_planner/full_coverage_path_planner.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | /** include the libraries you need in your planner here */ 5 | /** for global path planner interface */ 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | using std::string; 25 | 26 | #ifndef FULL_COVERAGE_PATH_PLANNER_FULL_COVERAGE_PATH_PLANNER_H 27 | #define FULL_COVERAGE_PATH_PLANNER_FULL_COVERAGE_PATH_PLANNER_H 28 | 29 | #include "full_coverage_path_planner/common.h" 30 | 31 | // #define DEBUG_PLOT 32 | 33 | #ifndef dabs 34 | #define dabs(a) ((a) >= 0 ? (a):-(a)) 35 | #endif 36 | #ifndef dmin 37 | #define dmin(a, b) ((a) <= (b) ? (a):(b)) 38 | #endif 39 | #ifndef dmax 40 | #define dmax(a, b) ((a) >= (b) ? (a):(b)) 41 | #endif 42 | #ifndef clamp 43 | #define clamp(a, lower, upper) dmax(dmin(a, upper), lower) 44 | #endif 45 | 46 | enum 47 | { 48 | eDirNone = 0, 49 | eDirRight = 1, 50 | eDirUp = 2, 51 | eDirLeft = -1, 52 | eDirDown = -2, 53 | }; 54 | 55 | namespace full_coverage_path_planner 56 | { 57 | class FullCoveragePathPlanner 58 | { 59 | public: 60 | /** 61 | * @brief Default constructor for the NavFnROS object 62 | */ 63 | FullCoveragePathPlanner(); 64 | FullCoveragePathPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros); 65 | 66 | /** 67 | * @brief Publish a path for visualization purposes 68 | */ 69 | void publishPlan(const std::vector& path); 70 | 71 | ~FullCoveragePathPlanner() 72 | { 73 | } 74 | 75 | virtual bool makePlan(const geometry_msgs::PoseStamped& start, 76 | const geometry_msgs::PoseStamped& goal, std::vector& plan) = 0; 77 | 78 | protected: 79 | /** 80 | * Convert internal representation of a to a ROS path 81 | * @param start Start pose of robot 82 | * @param goalpoints Goal points from Spiral Algorithm 83 | * @param plan Output plan variable 84 | */ 85 | void parsePointlist2Plan(const geometry_msgs::PoseStamped& start, std::list const& goalpoints, 86 | std::vector& plan); 87 | 88 | /** 89 | * Convert ROS Occupancy grid to internal grid representation, given the size of a single tile 90 | * @param cpp_grid_ ROS occupancy grid representation. Cells higher that 65 are considered occupied 91 | * @param grid internal map representation 92 | * @param tileSize size (in meters) of a cell. This can be the robot's size 93 | * @param realStart Start position of the robot (in meters) 94 | * @param scaledStart Start position of the robot on the grid 95 | * @return success 96 | */ 97 | bool parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_, 98 | std::vector >& grid, 99 | float robotRadius, 100 | float toolRadius, 101 | geometry_msgs::PoseStamped const& realStart, 102 | Point_t& scaledStart); 103 | ros::Publisher plan_pub_; 104 | ros::ServiceClient cpp_grid_client_; 105 | nav_msgs::OccupancyGrid cpp_grid_; 106 | float robot_radius_; 107 | float tool_radius_; 108 | float plan_resolution_; 109 | float tile_size_; 110 | fPoint_t grid_origin_; 111 | bool initialized_; 112 | geometry_msgs::PoseStamped previous_goal_; 113 | 114 | struct spiral_cpp_metrics_type 115 | { 116 | int visited_counter; 117 | int multiple_pass_counter; 118 | int accessible_counter; 119 | double total_area_covered; 120 | }; 121 | spiral_cpp_metrics_type spiral_cpp_metrics_; 122 | }; 123 | 124 | 125 | /** 126 | * Sort function for sorting Points on distance to a POI 127 | */ 128 | struct ComparatorForPointSort 129 | { 130 | explicit ComparatorForPointSort(Point_t poi) : _poi(poi) 131 | { 132 | } 133 | 134 | bool operator()(const Point_t& first, const Point_t& second) const 135 | { 136 | return distanceSquared(first, _poi) < distanceSquared(second, _poi); 137 | } 138 | 139 | private: 140 | Point_t _poi; 141 | }; 142 | } // namespace full_coverage_path_planner 143 | #endif // FULL_COVERAGE_PATH_PLANNER_FULL_COVERAGE_PATH_PLANNER_H 144 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/test_full_coverage_path_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Full Coverage Path Planner (FCPP) 2 | 3 | > :grey_exclamation: This package is not in use by our company anymore. Contributions to this package are still welcome! 4 | 5 | Paid support options are also available, please contact us at [nobleo-technology.nl](https://nobleo-technology.nl). 6 | 7 | ## Overview 8 | 9 | This package provides an implementation of a Full Coverage Path Planner (FCPP) using the Backtracking Spiral Algorithm (BSA), see [1]. 10 | 11 | This packages acts as a global planner plugin to the Move Base package (http://wiki.ros.org/move_base). 12 | 13 | ![BSA](doc/fcpp_robot_0_5m_plus_tool_0_2m.png) 14 | 15 | The user can configure robot radius and tool radius separately: 16 | 17 | ![robot_plus_tool](doc/robot_plus_tool.png) 18 | 19 | 20 | **Keywords:** coverage path planning, move base 21 | 22 | ### License 23 | 24 | Apache 2.0 25 | 26 | **Author(s): Yury Brodskiy, Ferry Schoenmakers, Tim Clephas, Jerrel Unkel, Loy van Beek, Cesar lopez** 27 | 28 | **Maintainer: Cesar Lopez, cesar.lopez@nobleo.nl** 29 | 30 | **Affiliation: Nobleo Projects BV, Eindhoven, the Netherlands** 31 | 32 | The Full Coverage Path Planner package has been tested under [ROS] Melodic and Ubuntu 18.04. 33 | 34 | ## Installation 35 | 36 | 37 | ### Building from Source 38 | 39 | 40 | #### Dependencies 41 | 42 | - [Robot Operating System (ROS)](http://wiki.ros.org) (middleware for robotics), 43 | - [Move Base Flex (MBF)](http://wiki.ros.org/move_base_flex) (move base flex node) used for system testing 44 | 45 | 46 | #### Building 47 | 48 | To build from source, clone the latest version from this repository into your workspace and compile the package using 49 | 50 | cd catkin_workspace/src 51 | git clone https://github.com/nobleo/full_coverage_path_planner.git 52 | cd ../ 53 | catkin_make 54 | 55 | ### Unit Tests 56 | 57 | All tests can be run using: 58 | 59 | catkin build full_coverage_path_planner --catkin-make-args run_tests 60 | 61 | #### test_common 62 | Unit test that checks the basic functions used by the repository 63 | 64 | #### test_spiral_stc 65 | Unit test that checks the basis spiral algorithm for full coverage. The test is performed for different situations to check that the algorithm coverage the accessible map cells. A test is also performed in randomly generated maps. 66 | 67 | #### test_full_coverage_path_planner.test 68 | ROS system test that checks the full coverage path planner together with a tracking pid. A simulation is run such that a robot moves to fully cover the accessible cells in a given map. 69 | 70 | 71 | ## Usage 72 | 73 | Run a full navigation example using: 74 | 75 | roslaunch full_coverage_path_planner test_full_coverage_path_planner.launch 76 | 77 | Give a 2D-goal in rviz to start path planning algorithm 78 | 79 | Depends on: 80 | 81 | [mobile_robot_simulator](https://github.com/mrath/mobile_robot_simulator.git) that integrates /cmd_vel into a base_link TF-frame and an odometry publisher 82 | 83 | [tracking_pid](https://github.com/nobleo/tracking_pid/) Global path tracking controller 84 | 85 | 86 | ## Launch files 87 | 88 | ### test/full_coverage_path_planner/test_full_coverage_path_planner.launch 89 | 90 | Runs the full_coverage_path_planner global planner in combination with tracking PID local planner. 91 | Moreover a coverage progress tracking node is launched to monitor the coverage progress. 92 | Mobile_robot_simulator is used to integrate cmd_vel output into TF and odometry. 93 | 94 | Arguments: 95 | 96 | * **`map`**: path to a global costmap. Default: `$(find full_coverage_path_planner)/maps/basement.yaml)` 97 | * **`target_x_vel`**: target x velocity for use in interpolator. Default: `0.2` 98 | * **`target_yaw_vel`**: target yaw velocity for use in interpolator. Default: `0.2` 99 | * **`robot_radius`**: radius of the robot for use in the global planner. Default: `0.6` 100 | * **`tool_radius`**: radius of the tool for use in the global planner. Default: `0.2` 101 | 102 | 103 | Start planning and tracking by giving a 2D nav goal. 104 | 105 | 106 | ## Nodes 107 | 108 | ### coverage_progress 109 | The CoverageProgressNode keeps track of coverage progress. It does this by periodically looking up the position of the coverage disk in an occupancy grid. Cells within a radius from this position are 'covered' 110 | 111 | #### Subscribed Topics 112 | 113 | * **`/tf`** ([tf2_msgs/TFMessage]) 114 | ros tf dynamic transformations 115 | * **`/tf_static`** ([tf2_msgs/TFMessage]) 116 | ros tf static transformations 117 | #### Published Topics 118 | 119 | * **`/coverage_grid`** ([nav_msgs/OccupancyGrid]) 120 | occupancy grid to visualize coverage progress 121 | * **`/coverage_progress`** ([std_msgs/Float32]) 122 | monitors coverage (from 0 none to 1 full) on the given area 123 | 124 | #### Services 125 | 126 | * **`/coverage_progress/reset`** ([std_srvs/SetBool]) 127 | resets coverage_progress node. For instance when robot position needs to be manually updated 128 | 129 | 130 | #### Parameters 131 | 132 | * **`target_area/x`**: size in x of the target area to monitor 133 | * **`target_area/y`**: size in y of the target area to monitor 134 | * **`coverage_radius`**: radius of the tool to compute coverage progress 135 | 136 | 137 | ## Plugins 138 | ### full_coverage_path_planner/SpiralSTC 139 | For use in move_base(\_flex) as "base_global_planner"="full_coverage_path_planner/SpiralSTC". It uses global_cost_map and global_costmap/robot_radius. 140 | 141 | #### Parameters 142 | 143 | * **`robot_radius`**: robot radius, which is used by the CPP algorithm to check for collisions with static map 144 | * **`tool_radius`**: tool radius, which is used by the CPP algorithm to discretize the space and find a full coverage plan 145 | 146 | 147 | ## References 148 | 149 | [1] GONZALEZ, Enrique, et al. BSA: A complete coverage algorithm. In: Proceedings of the 2005 IEEE International Conference on Robotics and Automation. IEEE, 2005. p. 2040-2044. 150 | 151 | ## Bugs & Feature Requests 152 | 153 | Please report bugs and request features using the [Issue Tracker](https://github.com/nobleo/full_coverage_path_planner/issues). 154 | 155 | 156 | [ROS]: http://www.ros.org 157 | [rviz]: http://wiki.ros.org/rviz 158 | [MBF]: http://wiki.ros.org/move_base_flex 159 | 160 | ## Acknowledgments 161 | 162 | 166 | 167 | 168 | rosin_logo 170 | 171 | 172 | Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. 173 | More information: rosin-project.eu 174 | 175 | eu_flag 177 | 178 | This project has received funding from the European Union’s Horizon 2020 179 | research and innovation programme under grant agreement no. 732287. 180 | -------------------------------------------------------------------------------- /nodes/coverage_progress: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import rospy 4 | import tf2_ros 5 | from nav_msgs.msg import OccupancyGrid 6 | from numpy import ones, sum 7 | from std_msgs.msg import Float32, Header 8 | from std_srvs.srv import Trigger 9 | 10 | # Constants for more readable index lookup 11 | X, Y, Z, W = 0, 1, 2, 3 12 | 13 | 14 | class CoverageProgressNode(object): 15 | """The CoverageProgressNode keeps track of coverage progress. 16 | It does this by periodically looking up the position of the coverage disk in an occupancy grid. 17 | Cells within a radius from this position are 'covered' 18 | 19 | Cell values are interpreted in this way: Lower is covered, higher is less covered 20 | - 100: uncovered (initial value) 21 | - < 100: covered 22 | 23 | The node emits a coverage progress, 24 | which is the ratio of cells considered coverage (0.0 is everything uncovered, 1.0 is all covered) 25 | """ 26 | 27 | DIRTY = 100 28 | 29 | def __init__(self): 30 | self.tf_buffer = tf2_ros.Buffer() 31 | self.listener = tf2_ros.TransformListener(self.tf_buffer) 32 | 33 | self._coverage_area = None # type: Tuple[float, float] 34 | 35 | self.coverage_resolution = None # type: float # How big is a cell [m] 36 | 37 | # How well covered is a cell after it has been covered for 1 time step 38 | self.coverage_effectivity = None # type: int 39 | 40 | self.map_frame = None # type: str 41 | self.coverage_frame = None # type: str 42 | 43 | self.grid = self._initialize_map() 44 | 45 | self.progress_pub = rospy.Publisher("coverage_progress", Float32, queue_size=1) 46 | self.grid_pub = rospy.Publisher("coverage_grid", OccupancyGrid, queue_size=1) 47 | 48 | self.reset_srv = rospy.Service('reset', Trigger, self.reset) 49 | 50 | self._rate = rospy.get_param("~rate", 10.0) 51 | self._update_timer = rospy.Timer(rospy.Duration(1.0/self._rate), self._update_callback, reset=True) 52 | 53 | def _initialize_map(self): 54 | # Initialize coverage matrix 55 | 56 | # Define parameters 57 | x = rospy.get_param("~target_area/x", 10.0) # height of the area to cover, in x direction of the map 58 | y = rospy.get_param("~target_area/y", 5.0) # width of the area to cover, in y direction of the map 59 | self._coverage_area = (x, y) 60 | 61 | try: 62 | self.coverage_radius_meters = rospy.get_param("~coverage_radius") 63 | except KeyError: 64 | try: 65 | self.coverage_radius_meters = rospy.get_param("~coverage_width") / 2.0 66 | except KeyError: 67 | rospy.logerr("Specify either coverage_width or coverage_radius") 68 | raise ValueError("Neither ~coverage_radius nor ~coverage_width specified, one of these is required") 69 | 70 | self.coverage_resolution = rospy.get_param("~coverage_resolution", 0.05) # How big is a cell [m] 71 | 72 | # How much covered is a cell after it has been covered for 1 time step 73 | self.coverage_effectivity = rospy.get_param("~coverage_effectivity", 5) 74 | 75 | self.map_frame = rospy.get_param("~map_frame", "map") 76 | self.coverage_frame = rospy.get_param("~coverage_frame", "base_link") 77 | 78 | self.coverage_radius_meters += 2 * self.coverage_resolution # Compensate for discretization 79 | self.coverage_radius_cells = int((self.coverage_radius_meters) / self.coverage_resolution) 80 | 81 | grid = OccupancyGrid() 82 | grid.info.resolution = self.coverage_resolution 83 | 84 | grid.info.width = abs(int(self._coverage_area[X] / self.coverage_resolution)) 85 | grid.info.height = abs(int(self._coverage_area[Y] / self.coverage_resolution)) 86 | 87 | grid.info.origin.position.x = 0 if self._coverage_area[X] > 0 else self._coverage_area[X] 88 | grid.info.origin.position.y = 0 if self._coverage_area[Y] > 0 else self._coverage_area[Y] 89 | grid.info.origin.orientation.w = 1 90 | 91 | # Initialize OccupancyGrid to have all cells DIRTY 92 | grid.data = self.DIRTY * ones(grid.info.width * grid.info.height, dtype=int) 93 | 94 | return grid 95 | 96 | def _update_callback(self, event): 97 | # Get the position of point (0,0,0) the coverage_disk frame wrt. the map frame (which can both be remapped if need be) 98 | 99 | try: 100 | coverage_tf = self.tf_buffer.lookup_transform(self.map_frame, self.coverage_frame, rospy.Time(0)) 101 | except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: 102 | rospy.logwarn(e) 103 | return 104 | 105 | # Element of matrix corresponding to middle of coverage surface 106 | x_point = int((coverage_tf.transform.translation.x - self.grid.info.origin.position.x) / self.coverage_resolution) 107 | y_point = int((coverage_tf.transform.translation.y - self.grid.info.origin.position.y) / self.coverage_resolution) 108 | 109 | # Initialize message 110 | self.grid.header = Header() 111 | self.grid.header.frame_id = self.map_frame 112 | self.grid.header.stamp = coverage_tf.header.stamp 113 | 114 | # Loop over amount of cells covered in x (j) and y (i) direction 115 | for i in range(2 * self.coverage_radius_cells): 116 | for j in range(2 * self.coverage_radius_cells): 117 | 118 | x_index = j - self.coverage_radius_cells 119 | y_index = i - self.coverage_radius_cells 120 | 121 | array_index = x_point + x_index + self.grid.info.width * (y_point + y_index) 122 | 123 | cell_in_coverage_circle = x_index ** 2 + y_index ** 2 < self.coverage_radius_cells ** 2 124 | 125 | cell_in_grid = 0 <= x_point + x_index < abs(int(self._coverage_area[X] / self.coverage_resolution)) \ 126 | and 0 <= y_point + y_index < abs(int(self._coverage_area[Y] / self.coverage_resolution)) 127 | 128 | if cell_in_coverage_circle and cell_in_grid: 129 | self.grid.data[array_index] = max(0, self.grid.data[array_index] - self.coverage_effectivity) 130 | 131 | coverage_progress = float(sum([self.grid.data < self.DIRTY])) / (self.grid.info.width * self.grid.info.height) 132 | 133 | self.progress_pub.publish(coverage_progress) 134 | self.grid_pub.publish(self.grid) 135 | 136 | def finish_callback(self, msg): 137 | 138 | if msg: 139 | coverage_progress = float(sum([self.grid.data < self.DIRTY])) / (self.grid.info.width * self.grid.info.height) 140 | 141 | self.progress_pub.publish(coverage_progress) 142 | 143 | def reset(self, srv_request): 144 | rospy.loginfo("Reset coverage progress and grid") 145 | self.grid = self._initialize_map() 146 | return (True, "Reset coverage progress and grid") 147 | 148 | if __name__ == '__main__': 149 | rospy.init_node('coverage_progress') 150 | try: 151 | node = CoverageProgressNode() 152 | rospy.spin() 153 | except rospy.ROSInterruptException: 154 | pass 155 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/fcpp.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 554 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | Name: Tool Properties 16 | Splitter Ratio: 0.5886790156364441 17 | - Class: rviz/Views 18 | Expanded: 19 | - /Current View1 20 | Name: Views 21 | Splitter Ratio: 0.5 22 | - Class: rviz/Time 23 | Experimental: false 24 | Name: Time 25 | SyncMode: 0 26 | SyncSource: "" 27 | Preferences: 28 | PromptSaveOnExit: true 29 | Toolbars: 30 | toolButtonStyle: 2 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.029999999329447746 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Class: rviz/TF 53 | Enabled: true 54 | Frame Timeout: 15 55 | Frames: 56 | All Enabled: true 57 | base_link: 58 | Value: true 59 | coverage_map: 60 | Value: true 61 | map: 62 | Value: true 63 | odom: 64 | Value: true 65 | Marker Scale: 1 66 | Name: TF 67 | Show Arrows: true 68 | Show Axes: true 69 | Show Names: true 70 | Tree: 71 | map: 72 | coverage_map: 73 | {} 74 | odom: 75 | base_link: 76 | {} 77 | Update Interval: 0 78 | Value: true 79 | - Alpha: 0.30000001192092896 80 | Class: rviz/Map 81 | Color Scheme: map 82 | Draw Behind: false 83 | Enabled: true 84 | Name: Map 85 | Topic: /map 86 | Unreliable: false 87 | Use Timestamp: false 88 | Value: true 89 | - Alpha: 1 90 | Buffer Length: 1 91 | Class: rviz/Path 92 | Color: 25; 255; 0 93 | Enabled: true 94 | Head Diameter: 0.05000000074505806 95 | Head Length: 0.019999999552965164 96 | Length: 0.30000001192092896 97 | Line Style: Lines 98 | Line Width: 0.029999999329447746 99 | Name: Path 100 | Offset: 101 | X: 0 102 | Y: 0 103 | Z: 0 104 | Pose Color: 255; 85; 255 105 | Pose Style: Arrows 106 | Radius: 0.029999999329447746 107 | Shaft Diameter: 0.019999999552965164 108 | Shaft Length: 0.05000000074505806 109 | Topic: /move_base/SpiralSTC/plan 110 | Unreliable: false 111 | Value: true 112 | - Angle Tolerance: 0.10000000149011612 113 | Class: rviz/Odometry 114 | Covariance: 115 | Orientation: 116 | Alpha: 0.5 117 | Color: 255; 255; 127 118 | Color Style: Unique 119 | Frame: Local 120 | Offset: 1 121 | Scale: 1 122 | Value: true 123 | Position: 124 | Alpha: 0.30000001192092896 125 | Color: 204; 51; 204 126 | Scale: 1 127 | Value: true 128 | Value: true 129 | Enabled: true 130 | Keep: 10000 131 | Name: Odometry 132 | Position Tolerance: 0.10000000149011612 133 | Shape: 134 | Alpha: 1 135 | Axes Length: 1 136 | Axes Radius: 0.10000000149011612 137 | Color: 255; 25; 0 138 | Head Length: 0.019999999552965164 139 | Head Radius: 0.05000000074505806 140 | Shaft Length: 0.05000000074505806 141 | Shaft Radius: 0.019999999552965164 142 | Value: Arrow 143 | Topic: /odom 144 | Unreliable: false 145 | Value: true 146 | - Class: rviz/Marker 147 | Enabled: true 148 | Marker Topic: /visualization_marker 149 | Name: Tracking_pid local planner 150 | Namespaces: 151 | axle point: true 152 | control point: true 153 | goal point: true 154 | Queue Size: 100 155 | Value: true 156 | - Class: rviz/Marker 157 | Enabled: true 158 | Marker Topic: /move_base_flex/tracking_pid/interpolator 159 | Name: Interpolator (mbf) 160 | Namespaces: 161 | {} 162 | Queue Size: 100 163 | Value: true 164 | - Alpha: 0.30000001192092896 165 | Class: rviz/Map 166 | Color Scheme: map 167 | Draw Behind: false 168 | Enabled: true 169 | Name: Coverage progress map 170 | Topic: /coverage_grid 171 | Unreliable: false 172 | Use Timestamp: false 173 | Value: true 174 | - Class: rviz/Marker 175 | Enabled: true 176 | Marker Topic: /move_base/TrackingPidLocalPlanner/interpolator 177 | Name: Interpolator (mb) 178 | Namespaces: 179 | {} 180 | Queue Size: 100 181 | Value: true 182 | - Alpha: 1 183 | Class: rviz/Polygon 184 | Color: 25; 255; 0 185 | Enabled: true 186 | Name: Footprint 187 | Topic: /move_base_flex/global_costmap/footprint 188 | Unreliable: false 189 | Value: true 190 | Enabled: true 191 | Global Options: 192 | Background Color: 48; 48; 48 193 | Default Light: true 194 | Fixed Frame: map 195 | Frame Rate: 30 196 | Name: root 197 | Tools: 198 | - Class: rviz/Interact 199 | Hide Inactive Objects: true 200 | - Class: rviz/MoveCamera 201 | - Class: rviz/Select 202 | - Class: rviz/FocusCamera 203 | - Class: rviz/Measure 204 | - Class: rviz/SetInitialPose 205 | Theta std deviation: 0.2617993950843811 206 | Topic: /set_pose 207 | X std deviation: 0.5 208 | Y std deviation: 0.5 209 | - Class: rviz/SetGoal 210 | Topic: /move_base_simple/goal 211 | - Class: rviz/PublishPoint 212 | Single click: true 213 | Topic: /clicked_point 214 | Value: true 215 | Views: 216 | Current: 217 | Angle: -3.1400022506713867 218 | Class: rviz/TopDownOrtho 219 | Enable Stereo Rendering: 220 | Stereo Eye Separation: 0.05999999865889549 221 | Stereo Focal Distance: 1 222 | Swap Stereo Eyes: false 223 | Value: false 224 | Invert Z Axis: false 225 | Name: Current View 226 | Near Clip Distance: 0.009999999776482582 227 | Scale: 97.19247436523438 228 | Target Frame: 229 | Value: TopDownOrtho (rviz) 230 | X: -0.14492475986480713 231 | Y: 1.0135213136672974 232 | Saved: ~ 233 | Window Geometry: 234 | Displays: 235 | collapsed: false 236 | Height: 1023 237 | Hide Left Dock: false 238 | Hide Right Dock: true 239 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000361fc0200000008fb0000001200530065006c0065006300740069006f006e010000003d000000a60000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000e9000002b5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f0000003efc0100000002fb0000000800540069006d006501000000000000039f000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000022f0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 240 | Selection: 241 | collapsed: false 242 | Time: 243 | collapsed: true 244 | Tool Properties: 245 | collapsed: true 246 | Views: 247 | collapsed: true 248 | Width: 927 249 | X: 67 250 | Y: 27 251 | -------------------------------------------------------------------------------- /doc/robot_plus_tool.ipe: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 0 0 m 9 | -1 0.333 l 10 | -1 -0.333 l 11 | h 12 | 13 | 14 | 15 | 16 | 0 0 m 17 | -1 0.333 l 18 | -1 -0.333 l 19 | h 20 | 21 | 22 | 23 | 24 | 0 0 m 25 | -1 0.333 l 26 | -0.8 0 l 27 | -1 -0.333 l 28 | h 29 | 30 | 31 | 32 | 33 | 0 0 m 34 | -1 0.333 l 35 | -0.8 0 l 36 | -1 -0.333 l 37 | h 38 | 39 | 40 | 41 | 42 | 0.6 0 0 0.6 0 0 e 43 | 0.4 0 0 0.4 0 0 e 44 | 45 | 46 | 47 | 48 | 0.6 0 0 0.6 0 0 e 49 | 50 | 51 | 52 | 53 | 54 | 0.5 0 0 0.5 0 0 e 55 | 56 | 57 | 0.6 0 0 0.6 0 0 e 58 | 0.4 0 0 0.4 0 0 e 59 | 60 | 61 | 62 | 63 | 64 | -0.6 -0.6 m 65 | 0.6 -0.6 l 66 | 0.6 0.6 l 67 | -0.6 0.6 l 68 | h 69 | -0.4 -0.4 m 70 | 0.4 -0.4 l 71 | 0.4 0.4 l 72 | -0.4 0.4 l 73 | h 74 | 75 | 76 | 77 | 78 | -0.6 -0.6 m 79 | 0.6 -0.6 l 80 | 0.6 0.6 l 81 | -0.6 0.6 l 82 | h 83 | 84 | 85 | 86 | 87 | 88 | -0.5 -0.5 m 89 | 0.5 -0.5 l 90 | 0.5 0.5 l 91 | -0.5 0.5 l 92 | h 93 | 94 | 95 | -0.6 -0.6 m 96 | 0.6 -0.6 l 97 | 0.6 0.6 l 98 | -0.6 0.6 l 99 | h 100 | -0.4 -0.4 m 101 | 0.4 -0.4 l 102 | 0.4 0.4 l 103 | -0.4 0.4 l 104 | h 105 | 106 | 107 | 108 | 109 | 110 | 111 | -0.43 -0.57 m 112 | 0.57 0.43 l 113 | 0.43 0.57 l 114 | -0.57 -0.43 l 115 | h 116 | 117 | 118 | -0.43 0.57 m 119 | 0.57 -0.43 l 120 | 0.43 -0.57 l 121 | -0.57 0.43 l 122 | h 123 | 124 | 125 | 126 | 127 | 128 | 0 0 m 129 | -1 0.333 l 130 | -1 -0.333 l 131 | h 132 | 133 | 134 | 135 | 136 | 0 0 m 137 | -1 0.333 l 138 | -0.8 0 l 139 | -1 -0.333 l 140 | h 141 | 142 | 143 | 144 | 145 | 0 0 m 146 | -1 0.333 l 147 | -0.8 0 l 148 | -1 -0.333 l 149 | h 150 | 151 | 152 | 153 | 154 | -1 0.333 m 155 | 0 0 l 156 | -1 -0.333 l 157 | 158 | 159 | 160 | 161 | 0 0 m 162 | -1 0.333 l 163 | -1 -0.333 l 164 | h 165 | -1 0 m 166 | -2 0.333 l 167 | -2 -0.333 l 168 | h 169 | 170 | 171 | 172 | 173 | 0 0 m 174 | -1 0.333 l 175 | -1 -0.333 l 176 | h 177 | -1 0 m 178 | -2 0.333 l 179 | -2 -0.333 l 180 | h 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 176 752 m 259 | 176 688 l 260 | 304 688 l 261 | 304 752 l 262 | h 263 | 264 | 265 | 192 768 m 266 | 192 752 l 267 | 224 752 l 268 | 224 768 l 269 | h 270 | 271 | 272 | 192 688 m 273 | 192 672 l 274 | 224 672 l 275 | 224 688 l 276 | h 277 | 278 | 279 | 31.241 0 0 31.241 240 720 e 280 | 281 | 282 | 71.5542 0 0 71.5542 240 720 e 283 | 284 | 285 | 276 724 m 286 | 276 716 l 287 | 292 716 l 288 | 292 724 l 289 | h 290 | 291 | 292 | 293 | 284 720 m 294 | 296 720 l 295 | 296 | 297 | 240 724 m 298 | 240 716 l 299 | 300 | 301 | 236 720 m 302 | 244 720 l 303 | 304 | robot radius 305 | tool radius 306 | 307 | 263.673 739.795 m 308 | 294.193 766.331 l 309 | 322.062 766.258 l 310 | 311 | 312 | 270.306 784.565 m 313 | 270.476 784.41 l 314 | 321.523 784.366 l 315 | 316 | 317 | 239.996 720.147 m 318 | 270.197 784.118 l 319 | 320 | 321 | 240.045 719.919 m 322 | 263.539 740.101 l 323 | 324 | 325 | 326 | -------------------------------------------------------------------------------- /src/full_coverage_path_planner.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | 7 | #include "full_coverage_path_planner/full_coverage_path_planner.h" 8 | 9 | /* *** Note the coordinate system *** 10 | * grid[][] is a 2D-vector: 11 | * where ix is column-index and x-coordinate in map, 12 | * iy is row-index and y-coordinate in map. 13 | * 14 | * Cols [ix] 15 | * _______________________ 16 | * |__|__|__|__|__|__|__|__| 17 | * |__|__|__|__|__|__|__|__| 18 | * Rows |__|__|__|__|__|__|__|__| 19 | * [iy] |__|__|__|__|__|__|__|__| 20 | * |__|__|__|__|__|__|__|__| 21 | *y-axis |__|__|__|__|__|__|__|__| 22 | * ^ |__|__|__|__|__|__|__|__| 23 | * ^ |__|__|__|__|__|__|__|__| 24 | * | |__|__|__|__|__|__|__|__| 25 | * | |__|__|__|__|__|__|__|__| 26 | * 27 | * O --->> x-axis 28 | */ 29 | 30 | // #define DEBUG_PLOT 31 | 32 | // Default Constructor 33 | namespace full_coverage_path_planner 34 | { 35 | FullCoveragePathPlanner::FullCoveragePathPlanner() : initialized_(false) 36 | { 37 | } 38 | 39 | void FullCoveragePathPlanner::publishPlan(const std::vector& path) 40 | { 41 | if (!initialized_) 42 | { 43 | ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 44 | return; 45 | } 46 | 47 | // create a message for the plan 48 | nav_msgs::Path gui_path; 49 | gui_path.poses.resize(path.size()); 50 | 51 | if (!path.empty()) 52 | { 53 | gui_path.header.frame_id = path[0].header.frame_id; 54 | gui_path.header.stamp = path[0].header.stamp; 55 | } 56 | 57 | // Extract the plan in world co-ordinates, we assume the path is all in the same frame 58 | for (unsigned int i = 0; i < path.size(); i++) 59 | { 60 | gui_path.poses[i] = path[i]; 61 | } 62 | 63 | plan_pub_.publish(gui_path); 64 | } 65 | 66 | void FullCoveragePathPlanner::parsePointlist2Plan(const geometry_msgs::PoseStamped& start, 67 | std::list const& goalpoints, 68 | std::vector& plan) 69 | { 70 | geometry_msgs::PoseStamped new_goal; 71 | std::list::const_iterator it, it_next, it_prev; 72 | int dx_now, dy_now, dx_next, dy_next, move_dir_now = 0, move_dir_prev = 0, move_dir_next = 0; 73 | bool do_publish = false; 74 | float orientation = eDirNone; 75 | ROS_INFO("Received goalpoints with length: %lu", goalpoints.size()); 76 | if (goalpoints.size() > 1) 77 | { 78 | for (it = goalpoints.begin(); it != goalpoints.end(); ++it) 79 | { 80 | it_next = it; 81 | it_next++; 82 | it_prev = it; 83 | it_prev--; 84 | 85 | // Check for the direction of movement 86 | if (it == goalpoints.begin()) 87 | { 88 | dx_now = it_next->x - it->x; 89 | dy_now = it_next->y - it->y; 90 | } 91 | else 92 | { 93 | dx_now = it->x - it_prev->x; 94 | dy_now = it->y - it_prev->y; 95 | dx_next = it_next->x - it->x; 96 | dy_next = it_next->y - it->y; 97 | } 98 | 99 | // Calculate direction enum: dx + dy*2 will give a unique number for each of the four possible directions because 100 | // of their signs: 101 | // 1 + 0*2 = 1 102 | // 0 + 1*2 = 2 103 | // -1 + 0*2 = -1 104 | // 0 + -1*2 = -2 105 | move_dir_now = dx_now + dy_now * 2; 106 | move_dir_next = dx_next + dy_next * 2; 107 | 108 | // Check if this points needs to be published (i.e. a change of direction or first or last point in list) 109 | do_publish = move_dir_next != move_dir_now || it == goalpoints.begin() || 110 | (it != goalpoints.end() && it == --goalpoints.end()); 111 | move_dir_prev = move_dir_now; 112 | 113 | // Add to vector if required 114 | if (do_publish) 115 | { 116 | new_goal.header.frame_id = "map"; 117 | new_goal.pose.position.x = (it->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5; 118 | new_goal.pose.position.y = (it->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5; 119 | // Calculate desired orientation to be in line with movement direction 120 | switch (move_dir_now) 121 | { 122 | case eDirNone: 123 | // Keep orientation 124 | break; 125 | case eDirRight: 126 | orientation = 0; 127 | break; 128 | case eDirUp: 129 | orientation = M_PI / 2; 130 | break; 131 | case eDirLeft: 132 | orientation = M_PI; 133 | break; 134 | case eDirDown: 135 | orientation = M_PI * 1.5; 136 | break; 137 | } 138 | new_goal.pose.orientation = tf::createQuaternionMsgFromYaw(orientation); 139 | if (it != goalpoints.begin()) 140 | { 141 | previous_goal_.pose.orientation = new_goal.pose.orientation; 142 | // republish previous goal but with new orientation to indicate change of direction 143 | // useful when the plan is strictly followed with base_link 144 | plan.push_back(previous_goal_); 145 | } 146 | ROS_DEBUG("Voila new point: x=%f, y=%f, o=%f,%f,%f,%f", new_goal.pose.position.x, new_goal.pose.position.y, 147 | new_goal.pose.orientation.x, new_goal.pose.orientation.y, new_goal.pose.orientation.z, 148 | new_goal.pose.orientation.w); 149 | plan.push_back(new_goal); 150 | previous_goal_ = new_goal; 151 | } 152 | } 153 | } 154 | else 155 | { 156 | new_goal.header.frame_id = "map"; 157 | new_goal.pose.position.x = (goalpoints.begin()->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5; 158 | new_goal.pose.position.y = (goalpoints.begin()->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5; 159 | new_goal.pose.orientation = tf::createQuaternionMsgFromYaw(0); 160 | plan.push_back(new_goal); 161 | } 162 | /* Add poses from current position to start of plan */ 163 | 164 | // Compute angle between current pose and first plan point 165 | double dy = plan.begin()->pose.position.y - start.pose.position.y; 166 | double dx = plan.begin()->pose.position.x - start.pose.position.x; 167 | // Arbitrary choice of 100.0*FLT_EPSILON to determine minimum angle precision of 1% 168 | if (!(fabs(dy) < 100.0 * FLT_EPSILON && fabs(dx) < 100.0 * FLT_EPSILON)) 169 | { 170 | // Add extra translation waypoint 171 | double yaw = std::atan2(dy, dx); 172 | geometry_msgs::Quaternion quat_temp = tf::createQuaternionMsgFromYaw(yaw); 173 | geometry_msgs::PoseStamped extra_pose; 174 | extra_pose = *plan.begin(); 175 | extra_pose.pose.orientation = quat_temp; 176 | plan.insert(plan.begin(), extra_pose); 177 | extra_pose = start; 178 | extra_pose.pose.orientation = quat_temp; 179 | plan.insert(plan.begin(), extra_pose); 180 | } 181 | 182 | // Insert current pose 183 | plan.insert(plan.begin(), start); 184 | 185 | ROS_INFO("Plan ready containing %lu goals!", plan.size()); 186 | } 187 | 188 | bool FullCoveragePathPlanner::parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_, 189 | std::vector >& grid, 190 | float robotRadius, 191 | float toolRadius, 192 | geometry_msgs::PoseStamped const& realStart, 193 | Point_t& scaledStart) 194 | { 195 | int ix, iy, nodeRow, nodeColl; 196 | uint32_t nodeSize = dmax(floor(toolRadius / cpp_grid_.info.resolution), 1); // Size of node in pixels/units 197 | uint32_t robotNodeSize = dmax(floor(robotRadius / cpp_grid_.info.resolution), 1); // RobotRadius in pixels/units 198 | uint32_t nRows = cpp_grid_.info.height, nCols = cpp_grid_.info.width; 199 | ROS_INFO("nRows: %u nCols: %u nodeSize: %d", nRows, nCols, nodeSize); 200 | 201 | if (nRows == 0 || nCols == 0) 202 | { 203 | return false; 204 | } 205 | 206 | // Save map origin and scaling 207 | tile_size_ = nodeSize * cpp_grid_.info.resolution; // Size of a tile in meters 208 | grid_origin_.x = cpp_grid_.info.origin.position.x; // x-origin in meters 209 | grid_origin_.y = cpp_grid_.info.origin.position.y; // y-origin in meters 210 | 211 | // Scale starting point 212 | scaledStart.x = static_cast(clamp((realStart.pose.position.x - grid_origin_.x) / tile_size_, 0.0, 213 | floor(cpp_grid_.info.width / tile_size_))); 214 | scaledStart.y = static_cast(clamp((realStart.pose.position.y - grid_origin_.y) / tile_size_, 0.0, 215 | floor(cpp_grid_.info.height / tile_size_))); 216 | 217 | // Scale grid 218 | for (iy = 0; iy < nRows; iy = iy + nodeSize) 219 | { 220 | std::vector gridRow; 221 | for (ix = 0; ix < nCols; ix = ix + nodeSize) 222 | { 223 | bool nodeOccupied = false; 224 | for (nodeRow = 0; (nodeRow < robotNodeSize) && ((iy + nodeRow) < nRows) && (nodeOccupied == false); ++nodeRow) 225 | { 226 | for (nodeColl = 0; (nodeColl < robotNodeSize) && ((ix + nodeColl) < nCols); ++nodeColl) 227 | { 228 | int index_grid = dmax((iy + nodeRow - ceil(static_cast(robotNodeSize - nodeSize) / 2.0)) 229 | * nCols + (ix + nodeColl - ceil(static_cast(robotNodeSize - nodeSize) / 2.0)), 0); 230 | if (cpp_grid_.data[index_grid] > 65) 231 | { 232 | nodeOccupied = true; 233 | break; 234 | } 235 | } 236 | } 237 | gridRow.push_back(nodeOccupied); 238 | } 239 | grid.push_back(gridRow); 240 | } 241 | return true; 242 | } 243 | } // namespace full_coverage_path_planner 244 | -------------------------------------------------------------------------------- /src/spiral_stc.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "full_coverage_path_planner/spiral_stc.h" 11 | #include 12 | 13 | // register this planner as a BaseGlobalPlanner plugin 14 | PLUGINLIB_EXPORT_CLASS(full_coverage_path_planner::SpiralSTC, nav_core::BaseGlobalPlanner) 15 | 16 | namespace full_coverage_path_planner 17 | { 18 | void SpiralSTC::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 19 | { 20 | if (!initialized_) 21 | { 22 | // Create a publisher to visualize the plan 23 | ros::NodeHandle private_nh("~/"); 24 | ros::NodeHandle nh, private_named_nh("~/" + name); 25 | 26 | plan_pub_ = private_named_nh.advertise("plan", 1); 27 | // Try to request the cpp-grid from the cpp_grid map_server 28 | cpp_grid_client_ = nh.serviceClient("static_map"); 29 | 30 | // Define robot radius (radius) parameter 31 | float robot_radius_default = 0.5f; 32 | private_named_nh.param("robot_radius", robot_radius_, robot_radius_default); 33 | // Define tool radius (radius) parameter 34 | float tool_radius_default = 0.5f; 35 | private_named_nh.param("tool_radius", tool_radius_, tool_radius_default); 36 | initialized_ = true; 37 | } 38 | } 39 | 40 | std::list SpiralSTC::spiral(std::vector > const& grid, std::list& init, 41 | std::vector >& visited) 42 | { 43 | int dx, dy, dx_prev, x2, y2, i, nRows = grid.size(), nCols = grid[0].size(); 44 | // Spiral filling of the open space 45 | // Copy incoming list to 'end' 46 | std::list pathNodes(init); 47 | // Create iterator for gridNode_t list and let it point to the last element of end 48 | std::list::iterator it = --(pathNodes.end()); 49 | if (pathNodes.size() > 1) // if list is length 1, keep iterator at end 50 | it--; // Let iterator point to second to last element 51 | 52 | gridNode_t prev = *(it); 53 | bool done = false; 54 | while (!done) 55 | { 56 | if (it != pathNodes.begin()) 57 | { 58 | // turn ccw 59 | dx = pathNodes.back().pos.x - prev.pos.x; 60 | dy = pathNodes.back().pos.y - prev.pos.y; 61 | dx_prev = dx; 62 | dx = -dy; 63 | dy = dx_prev; 64 | } 65 | else 66 | { 67 | // Initialize spiral direction towards y-axis 68 | dx = 0; 69 | dy = 1; 70 | } 71 | done = true; 72 | 73 | for (int i = 0; i < 4; ++i) 74 | { 75 | x2 = pathNodes.back().pos.x + dx; 76 | y2 = pathNodes.back().pos.y + dy; 77 | if (x2 >= 0 && x2 < nCols && y2 >= 0 && y2 < nRows) 78 | { 79 | if (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen) 80 | { 81 | Point_t new_point = { x2, y2 }; 82 | gridNode_t new_node = 83 | { 84 | new_point, // Point: x,y 85 | 0, // Cost 86 | 0, // Heuristic 87 | }; 88 | prev = pathNodes.back(); 89 | pathNodes.push_back(new_node); 90 | it = --(pathNodes.end()); 91 | visited[y2][x2] = eNodeVisited; // Close node 92 | done = false; 93 | break; 94 | } 95 | } 96 | // try next direction cw 97 | dx_prev = dx; 98 | dx = dy; 99 | dy = -dx_prev; 100 | } 101 | } 102 | return pathNodes; 103 | } 104 | 105 | std::list SpiralSTC::spiral_stc(std::vector > const& grid, 106 | Point_t& init, 107 | int &multiple_pass_counter, 108 | int &visited_counter) 109 | { 110 | int x, y, nRows = grid.size(), nCols = grid[0].size(); 111 | // Initial node is initially set as visited so it does not count 112 | multiple_pass_counter = 0; 113 | visited_counter = 0; 114 | 115 | std::vector > visited; 116 | visited = grid; // Copy grid matrix 117 | x = init.x; 118 | y = init.y; 119 | 120 | Point_t new_point = { x, y }; 121 | gridNode_t new_node = 122 | { 123 | new_point, // Point: x,y 124 | 0, // Cost 125 | 0, // Heuristic 126 | }; 127 | std::list pathNodes; 128 | std::list fullPath; 129 | pathNodes.push_back(new_node); 130 | visited[y][x] = eNodeVisited; 131 | 132 | #ifdef DEBUG_PLOT 133 | ROS_INFO("Grid before walking is: "); 134 | printGrid(grid, visited, fullPath); 135 | #endif 136 | 137 | pathNodes = SpiralSTC::spiral(grid, pathNodes, visited); // First spiral fill 138 | std::list goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints 139 | // Add points to full path 140 | std::list::iterator it; 141 | for (it = pathNodes.begin(); it != pathNodes.end(); ++it) 142 | { 143 | Point_t newPoint = { it->pos.x, it->pos.y }; 144 | visited_counter++; 145 | fullPath.push_back(newPoint); 146 | } 147 | // Remove all elements from pathNodes list except last element 148 | pathNodes.erase(pathNodes.begin(), --(pathNodes.end())); 149 | 150 | #ifdef DEBUG_PLOT 151 | ROS_INFO("Current grid after first spiral is"); 152 | printGrid(grid, visited, fullPath); 153 | ROS_INFO("There are %d goals remaining", goals.size()); 154 | #endif 155 | while (goals.size() != 0) 156 | { 157 | // Remove all elements from pathNodes list except last element. 158 | // The last point is the starting point for a new search and A* extends the path from there on 159 | pathNodes.erase(pathNodes.begin(), --(pathNodes.end())); 160 | visited_counter--; // First point is already counted as visited 161 | // Plan to closest open Node using A* 162 | // `goals` is essentially the map, so we use `goals` to determine the distance from the end of a potential path 163 | // to the nearest free space 164 | bool resign = a_star_to_open_space(grid, pathNodes.back(), 1, visited, goals, pathNodes); 165 | if (resign) 166 | { 167 | #ifdef DEBUG_PLOT 168 | ROS_INFO("A_star_to_open_space is resigning", goals.size()); 169 | #endif 170 | break; 171 | } 172 | 173 | // Update visited grid 174 | for (it = pathNodes.begin(); it != pathNodes.end(); ++it) 175 | { 176 | if (visited[it->pos.y][it->pos.x]) 177 | { 178 | multiple_pass_counter++; 179 | } 180 | visited[it->pos.y][it->pos.x] = eNodeVisited; 181 | } 182 | if (pathNodes.size() > 0) 183 | { 184 | multiple_pass_counter--; // First point is already counted as visited 185 | } 186 | 187 | #ifdef DEBUG_PLOT 188 | ROS_INFO("Grid with path marked as visited is:"); 189 | gridNode_t SpiralStart = pathNodes.back(); 190 | printGrid(grid, visited, pathNodes, pathNodes.front(), pathNodes.back()); 191 | #endif 192 | 193 | // Spiral fill from current position 194 | pathNodes = spiral(grid, pathNodes, visited); 195 | 196 | #ifdef DEBUG_PLOT 197 | ROS_INFO("Visited grid updated after spiral:"); 198 | printGrid(grid, visited, pathNodes, SpiralStart, pathNodes.back()); 199 | #endif 200 | 201 | goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints 202 | 203 | for (it = pathNodes.begin(); it != pathNodes.end(); ++it) 204 | { 205 | Point_t newPoint = { it->pos.x, it->pos.y }; 206 | visited_counter++; 207 | fullPath.push_back(newPoint); 208 | } 209 | } 210 | 211 | return fullPath; 212 | } 213 | 214 | bool SpiralSTC::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, 215 | std::vector& plan) 216 | { 217 | if (!initialized_) 218 | { 219 | ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 220 | return false; 221 | } 222 | else 223 | { 224 | ROS_INFO("Initialized!"); 225 | } 226 | 227 | clock_t begin = clock(); 228 | Point_t startPoint; 229 | 230 | /********************** Get grid from server **********************/ 231 | std::vector > grid; 232 | nav_msgs::GetMap grid_req_srv; 233 | ROS_INFO("Requesting grid!!"); 234 | if (!cpp_grid_client_.call(grid_req_srv)) 235 | { 236 | ROS_ERROR("Could not retrieve grid from map_server"); 237 | return false; 238 | } 239 | 240 | if (!parseGrid(grid_req_srv.response.map, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint)) 241 | { 242 | ROS_ERROR("Could not parse retrieved grid"); 243 | return false; 244 | } 245 | 246 | #ifdef DEBUG_PLOT 247 | ROS_INFO("Start grid is:"); 248 | std::list printPath; 249 | printPath.push_back(startPoint); 250 | printGrid(grid, grid, printPath); 251 | #endif 252 | 253 | std::list goalPoints = spiral_stc(grid, 254 | startPoint, 255 | spiral_cpp_metrics_.multiple_pass_counter, 256 | spiral_cpp_metrics_.visited_counter); 257 | ROS_INFO("naive cpp completed!"); 258 | ROS_INFO("Converting path to plan"); 259 | 260 | parsePointlist2Plan(start, goalPoints, plan); 261 | // Print some metrics: 262 | spiral_cpp_metrics_.accessible_counter = spiral_cpp_metrics_.visited_counter 263 | - spiral_cpp_metrics_.multiple_pass_counter; 264 | spiral_cpp_metrics_.total_area_covered = (4.0 * tool_radius_ * tool_radius_) * spiral_cpp_metrics_.accessible_counter; 265 | ROS_INFO("Total visited: %d", spiral_cpp_metrics_.visited_counter); 266 | ROS_INFO("Total re-visited: %d", spiral_cpp_metrics_.multiple_pass_counter); 267 | ROS_INFO("Total accessible cells: %d", spiral_cpp_metrics_.accessible_counter); 268 | ROS_INFO("Total accessible area: %f", spiral_cpp_metrics_.total_area_covered); 269 | 270 | // TODO(CesarLopez): Check if global path should be calculated repetitively or just kept 271 | // (also controlled by planner_frequency parameter in move_base namespace) 272 | 273 | ROS_INFO("Publishing plan!"); 274 | publishPlan(plan); 275 | ROS_INFO("Plan published!"); 276 | ROS_DEBUG("Plan published"); 277 | 278 | clock_t end = clock(); 279 | double elapsed_secs = static_cast(end - begin) / CLOCKS_PER_SEC; 280 | std::cout << "elapsed time: " << elapsed_secs << "\n"; 281 | 282 | return true; 283 | } 284 | } // namespace full_coverage_path_planner 285 | -------------------------------------------------------------------------------- /src/common.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | int distanceToClosestPoint(Point_t poi, std::list const& goals) 13 | { 14 | // Return minimum distance from goals-list 15 | int min_dist = INT_MAX; 16 | std::list::const_iterator it; 17 | for (it = goals.begin(); it != goals.end(); ++it) 18 | { 19 | int cur_dist = distanceSquared((*it), poi); 20 | if (cur_dist < min_dist) 21 | { 22 | min_dist = cur_dist; 23 | } 24 | } 25 | return min_dist; 26 | } 27 | 28 | int distanceSquared(const Point_t& p1, const Point_t& p2) 29 | { 30 | int dx = p2.x - p1.x; 31 | int dy = p2.y - p1.y; 32 | 33 | int dx2 = dx * dx; 34 | if (dx2 != 0 && dx2 / dx != dx) 35 | { 36 | throw std::range_error("Integer overflow error for the given points"); 37 | } 38 | 39 | int dy2 = dy * dy; 40 | if (dy2 != 0 && dy2 / dy != dy) 41 | { 42 | throw std::range_error("Integer overflow error for the given points"); 43 | } 44 | 45 | if (dx2 > std::numeric_limits< int >::max() - dy2) 46 | throw std::range_error("Integer overflow error for the given points"); 47 | int d2 = dx2 + dy2; 48 | 49 | return d2; 50 | } 51 | 52 | /** 53 | * Sort vector by the heuristic value of the last element 54 | * @return whether last elem. of first has a larger heuristic value than last elem of second 55 | */ 56 | bool sort_gridNodePath_heuristic_desc(const std::vector &first, const std::vector &second) 57 | { 58 | return (first.back().he > second.back().he); 59 | } 60 | 61 | bool a_star_to_open_space(std::vector > const &grid, gridNode_t init, int cost, 62 | std::vector > &visited, std::list const &open_space, 63 | std::list &pathNodes) 64 | { 65 | uint dx, dy, dx_prev, nRows = grid.size(), nCols = grid[0].size(); 66 | 67 | std::vector > closed(nRows, std::vector(nCols, eNodeOpen)); 68 | // All nodes in the closest list are currently still open 69 | 70 | closed[init.pos.y][init.pos.x] = eNodeVisited; // Of course we have visited the current/initial location 71 | #ifdef DEBUG_PLOT 72 | std::cout << "A*: Marked init " << init << " as eNodeVisited (true)" << std::endl; 73 | printGrid(closed); 74 | #endif 75 | 76 | std::vector > open1(1, std::vector(1, init)); // open1 is a *vector* of paths 77 | 78 | while (true) 79 | { 80 | #ifdef DEBUG_PLOT 81 | std::cout << "A*: open1.size() = " << open1.size() << std::endl; 82 | #endif 83 | if (open1.size() == 0) // If there are no open paths, there's no place to go and we must resign 84 | { 85 | // Empty end_node list and add init as only element 86 | pathNodes.erase(pathNodes.begin(), --(pathNodes.end())); 87 | pathNodes.push_back(init); 88 | return true; // We resign, cannot find a path 89 | } 90 | else 91 | { 92 | // Sort elements from high to low (because sort_gridNodePath_heuristic_desc uses a > b) 93 | std::sort(open1.begin(), open1.end(), sort_gridNodePath_heuristic_desc); 94 | 95 | std::vector nn = open1.back(); // Get the *path* with the lowest heuristic cost 96 | open1.pop_back(); // The last element is no longer open because we use it here, so remove from open list 97 | #ifdef DEBUG_PLOT 98 | std::cout << "A*: Check out path from" << nn.front().pos << " to " << nn.back().pos 99 | << " of length " << nn.size() << std::endl; 100 | #endif 101 | 102 | // Does the path nn end in open space? 103 | if (visited[nn.back().pos.y][nn.back().pos.x] == eNodeOpen) 104 | { 105 | // If so, we found a path to open space 106 | // Copy the path nn to pathNodes so we can report that path (to get to open space) 107 | std::vector::iterator iter; 108 | for (iter = nn.begin(); iter != nn.end(); ++iter) 109 | { 110 | pathNodes.push_back((*iter)); 111 | } 112 | 113 | return false; // We do not resign, we found a path 114 | } 115 | else // Path nn does not lead to open space 116 | { 117 | if (nn.size() > 1) 118 | { 119 | // Create iterator for gridNode_t list and let it point to the last element of nn 120 | std::vector::iterator it = --(nn.end()); 121 | dx = it->pos.x - (it - 1)->pos.x; 122 | dy = it->pos.y - (it - 1)->pos.y; 123 | // TODO(CesarLopez) docs: this seems to cycle through directions 124 | // (notice the shift-by-one between both sides of the =) 125 | dx_prev = dx; 126 | dx = -dy; 127 | dy = dx_prev; 128 | } 129 | else 130 | { 131 | dx = 0; 132 | dy = 1; 133 | } 134 | 135 | // For all nodes surrounding the end of the end of the path nn 136 | for (uint i = 0; i < 4; ++i) 137 | { 138 | Point_t p2 = 139 | { 140 | static_cast(nn.back().pos.x + dx), 141 | static_cast(nn.back().pos.y + dy), 142 | }; 143 | 144 | #ifdef DEBUG_PLOT 145 | std::cout << "A*: Look around " << i << " at p2=(" << p2 << std::endl; 146 | #endif 147 | 148 | if (p2.x >= 0 && p2.x < nCols && p2.y >= 0 && p2.y < nRows) // Bounds check, do not sep out of map 149 | { 150 | // If the new node (a neighbor of the end of the path nn) is open, append it to newPath ( = nn) 151 | // and add that to the open1-list of paths. 152 | // Because of the pop_back on open1, what happens is that the path is temporarily 'checked out', 153 | // modified here, and then added back (if the condition above and below holds) 154 | if (closed[p2.y][p2.x] == eNodeOpen && grid[p2.y][p2.x] == eNodeOpen) 155 | { 156 | #ifdef DEBUG_PLOT 157 | std::cout << "A*: p2=" << p2 << " is OPEN" << std::endl; 158 | #endif 159 | std::vector newPath = nn; 160 | // # heuristic has to be designed to prefer a CCW turn 161 | Point_t new_point = { p2.x, p2.y }; 162 | gridNode_t new_node = 163 | { 164 | new_point, // Point: x,y 165 | static_cast(cost + nn.back().cost), // Cost 166 | static_cast(cost + nn.back().cost + distanceToClosestPoint(p2, open_space) + i), 167 | // Heuristic (+i so CCW turns are cheaper) 168 | }; 169 | newPath.push_back(new_node); 170 | closed[new_node.pos.y][new_node.pos.x] = eNodeVisited; // New node is now used in a path and thus visited 171 | 172 | #ifdef DEBUG_PLOT 173 | std::cout << "A*: Marked new_node " << new_node << " as eNodeVisited (true)" << std::endl; 174 | std::cout << "A*: Add path from " << newPath.front().pos << " to " << newPath.back().pos 175 | << " of length " << newPath.size() << " to open1" << std::endl; 176 | #endif 177 | open1.push_back(newPath); 178 | } 179 | #ifdef DEBUG_PLOT 180 | else 181 | { 182 | std::cout << "A*: p2=" << p2 << " is not open: " 183 | "closed[" << p2.y << "][" << p2.x << "]=" << closed[p2.y][p2.x] << ", " 184 | "grid[" << p2.y << "][" << p2.x << "]=" << grid[p2.y][p2.x] << std::endl; 185 | } 186 | #endif 187 | } 188 | #ifdef DEBUG_PLOT 189 | else 190 | { 191 | std::cout << "A*: p2=(" << p2.x << ", " << p2.y << ") is out of bounds" << std::endl; 192 | } 193 | #endif 194 | // Cycle around to next neighbor, CCW 195 | dx_prev = dx; 196 | dx = dy; 197 | dy = -dx_prev; 198 | } 199 | } 200 | } 201 | } 202 | } 203 | 204 | void printGrid(std::vector > const& grid, std::vector > const& visited, 205 | std::list const& path) 206 | { 207 | for (uint iy = grid.size() - 1; iy >= 0; --iy) 208 | { 209 | for (uint ix = 0; ix < grid[0].size(); ++ix) 210 | { 211 | if (visited[iy][ix]) 212 | { 213 | if (ix == path.front().x && iy == path.front().y) 214 | { 215 | std::cout << "\033[1;32m▓\033[0m"; // Show starting position in green color 216 | } 217 | else if (ix == path.back().x && iy == path.back().y) 218 | { 219 | std::cout << "\033[1;31m▓\033[0m"; // Show stopping position in red color 220 | } 221 | else if (visited[iy][ix] && grid[iy][ix]) 222 | { 223 | std::cout << "\033[1;33m▓\033[0m"; // Show walls in yellow color 224 | } 225 | else 226 | { 227 | std::cout << "\033[1;36m▓\033[0m"; 228 | } 229 | } 230 | else 231 | { 232 | std::cout << "\033[1;37m▓\033[0m"; 233 | } 234 | } 235 | std::cout << "\n"; 236 | } 237 | } 238 | 239 | void printGrid(std::vector > const& grid, std::vector > const& visited, 240 | std::list const& path, gridNode_t start, gridNode_t end) 241 | { 242 | for (uint iy = grid.size() - 1; iy >= 0; --iy) 243 | { 244 | for (uint ix = 0; ix < grid[0].size(); ++ix) 245 | { 246 | if (visited[iy][ix]) 247 | { 248 | if (ix == start.pos.x && iy == start.pos.y) 249 | { 250 | std::cout << "\033[1;32m▓\033[0m"; // Show starting position in green color 251 | } 252 | else if (ix == end.pos.x && iy == end.pos.y) 253 | { 254 | std::cout << "\033[1;31m▓\033[0m"; // Show stopping position in red color 255 | } 256 | else if (visited[iy][ix] && grid[iy][ix]) 257 | { 258 | std::cout << "\033[1;33m▓\033[0m"; // Show walls in yellow color 259 | } 260 | else 261 | { 262 | std::cout << "\033[1;36m▓\033[0m"; 263 | } 264 | } 265 | else 266 | { 267 | std::cout << "\033[1;37m▓\033[0m"; 268 | } 269 | } 270 | std::cout << "\n"; 271 | } 272 | } 273 | 274 | void printGrid(std::vector > const& grid) 275 | { 276 | for (uint iy = grid.size() - 1; iy >= 0; --iy) 277 | { 278 | for (uint ix = 0; ix < grid[0].size(); ++ix) 279 | { 280 | if (grid[iy][ix]) 281 | { 282 | std::cout << "\033[1;36m▓\033[0m"; 283 | } 284 | else 285 | { 286 | std::cout << "\033[1;37m▓\033[0m"; 287 | } 288 | } 289 | std::cout << "\n"; 290 | } 291 | } 292 | 293 | std::list map_2_goals(std::vector > const& grid, bool value_to_search) 294 | { 295 | std::list goals; 296 | int ix, iy; 297 | uint nRows = grid.size(); 298 | uint nCols = grid[0].size(); 299 | for (iy = 0; iy < nRows; ++(iy)) 300 | { 301 | for (ix = 0; ix < nCols; ++(ix)) 302 | { 303 | if (grid[iy][ix] == value_to_search) 304 | { 305 | Point_t p = { ix, iy }; // x, y 306 | goals.push_back(p); 307 | } 308 | } 309 | } 310 | return goals; 311 | } 312 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /test/src/test_common.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // Created by nobleo on 25-9-18. 4 | // 5 | 6 | /* 7 | * Run tests for all of the common function except the print* functions that do not return anything testable 8 | * Most important here is the conversion function and a variant of A*. Each test is explained below 9 | * 10 | */ 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | /** 21 | * DistanceSquared uses euclidian distance except for the expensive sqrt-call: returns dx^2+dy^2. 22 | */ 23 | TEST(TestDistanceSquared, testDistanceSquared) 24 | { 25 | // 1^2 + 1^2 = 1 + 1 = 2 26 | ASSERT_EQ(2, distanceSquared({0, 0}, {1, 1})); // NOLINT 27 | 28 | // 10^2 + 10^2 = 100 + 100 = 200 29 | ASSERT_EQ(200, distanceSquared({0, 0}, {10, 10})); // NOLINT 30 | 31 | /* The function uses plain integers. Squaring and then adding them can go out of range of int. 32 | * In that case: throw an exception, don't crash 33 | */ 34 | ASSERT_THROW(distanceSquared({0, 0}, {100000, 100000}), std::range_error); // NOLINT 35 | 36 | /* Points at equal distance in direct directions should have same value 37 | */ 38 | ASSERT_EQ(distanceSquared({0, 0}, {11, 10}), // NOLINT 39 | distanceSquared({0, 0}, {10, 11})); // NOLINT 40 | 41 | /* The function is used mostly to order points by distance, so the actual value doesn't matter. 42 | * The only property that is important is that points further away should have a larger value than those close by 43 | */ 44 | ASSERT_LE(distanceSquared({0, 0}, {10, 10}), // NOLINT 45 | distanceSquared({0, 0}, {11, 11})); // NOLINT 46 | } 47 | 48 | /* 49 | * Test basics of distanceToClosestPoint: 50 | * - does it return the only point if there is one only 51 | * - if we add another further away, is the first point still returned 52 | */ 53 | TEST(TestDistanceToClosestPoint, testDistanceToOnlyPoint) 54 | { 55 | Point_t poi = {0, 0}; // NOLINT 56 | std::list goals; 57 | goals.push_back({1, 1}); // NOLINT 58 | 59 | // There is only 1 point, at 1,1, so the (squared) distance is 2 60 | ASSERT_EQ(2, distanceToClosestPoint(poi, goals)); 61 | 62 | goals.push_back({2, 2}); // NOLINT // We add a point that is further away, so the first point is still closest 63 | ASSERT_EQ(2, distanceToClosestPoint(poi, goals)); 64 | } 65 | 66 | /* 67 | * Add several points, 2 at same distance 68 | */ 69 | TEST(TestDistanceToClosestPoint, testDistanceToEqualDistancePoints) 70 | { 71 | Point_t poi = {0, 0}; // NOLINT 72 | std::list goals; 73 | goals.push_back({0, 1}); // NOLINT // closest, d=1 74 | goals.push_back({1, 0}); // NOLINT // closest, d=1 75 | goals.push_back({1, 1}); // NOLINT 76 | goals.push_back({2, 2}); // NOLINT 77 | 78 | ASSERT_EQ(1, distanceToClosestPoint(poi, goals)); 79 | 80 | goals.push_back({0, 0}); // NOLINT // new closest, d=0 81 | ASSERT_EQ(0, distanceToClosestPoint(poi, goals)); 82 | } 83 | 84 | /* 85 | * distanceToClosestPoint cannot deal with dimensions larger than 2^16 because it does a square of that value 86 | * Function should not crash if one distance is <2^16 but raise an exception if we go over 87 | */ 88 | TEST(TestDistanceToClosestPoint, testDistanceAtIntLimits) 89 | { 90 | Point_t poi = {0, 0}; // NOLINT 91 | std::list goals; 92 | for (int i = 0; i < 32768; ++i) 93 | { 94 | goals.push_back({i, i}); // NOLINT 95 | } 96 | 97 | ASSERT_NO_THROW(distanceToClosestPoint(poi, goals)); // OK for small enough dimensions 98 | ASSERT_EQ(0, distanceToClosestPoint(poi, goals)); 99 | 100 | for (int i = 32768; i < 100000; ++i) 101 | { 102 | goals.push_back({i, i}); // NOLINT 103 | } 104 | 105 | // Squaring and adding 100000 is too much for an int 106 | ASSERT_THROW(distanceToClosestPoint(poi, goals), std::range_error); // Must throw for large enough dimensions 107 | } 108 | 109 | /* 110 | * Same as testDistanceAtIntLimits but with negative numbers 111 | */ 112 | TEST(TestDistanceToClosestPoint, testDistanceAtIntNegativeLimits) 113 | { 114 | Point_t poi = {0, 0}; // NOLINT 115 | std::list goals; 116 | for (int i = 0; i < 32768; ++i) 117 | { 118 | goals.push_back({-i, -i}); // NOLINT 119 | } 120 | 121 | ASSERT_NO_THROW(distanceToClosestPoint(poi, goals)); 122 | ASSERT_EQ(0, distanceToClosestPoint(poi, goals)); 123 | 124 | for (int i = 32768; i < 100000; ++i) 125 | { 126 | goals.push_back({-i, -i}); // NOLINT 127 | } 128 | 129 | // Squaring and adding 100000 is too much for an int 130 | ASSERT_THROW(distanceToClosestPoint(poi, goals), std::range_error); 131 | } 132 | 133 | /* 134 | * Test points in a block a bit further away 135 | */ 136 | TEST(TestDistanceToClosestPoint, testDistanceToBlock) 137 | { 138 | Point_t poi = {0, 0}; // NOLINT 139 | std::list goals; 140 | goals.push_back({10, 10}); // NOLINT // closest, d^2=200 141 | goals.push_back({11, 10}); // NOLINT 142 | goals.push_back({10, 11}); // NOLINT 143 | goals.push_back({12, 11}); // NOLINT 144 | 145 | ASSERT_EQ(200, distanceToClosestPoint(poi, goals)); 146 | 147 | goals.push_back({0, 0}); // NOLINT // new closest, d=0 148 | ASSERT_EQ(0, distanceToClosestPoint(poi, goals)); 149 | } 150 | 151 | /* 152 | * Test points in a stroke a bit further away 153 | */ 154 | TEST(TestDistanceToClosestPoint, testDistanceToDiagonal) 155 | { 156 | Point_t poi = {100, 100}; // NOLINT 157 | std::list goals; 158 | for (int i = 0; i < 10; ++i) 159 | { 160 | for (int j = 0; j < 10; ++j) 161 | { 162 | goals.push_back({i, j}); // NOLINT 163 | } 164 | } 165 | 166 | // Closest point is 9,9, so distance is 91^2 + 91^2 167 | ASSERT_EQ(16562, distanceToClosestPoint(poi, goals)); 168 | } 169 | 170 | /* 171 | * Test a test utility function to make grids. 172 | * Grids must all have specified size but now allow access beyond those limits. 173 | * Users of the resulting test grid are also not allowed to access beyond the limits 174 | * and should crash if they do for proper testing. 175 | */ 176 | TEST(TestMakeTestGrid, testDimensions) 177 | { 178 | std::vector > grid = makeTestGrid(3, 4); 179 | ASSERT_EQ(4, grid.size()); 180 | ASSERT_EQ(3, grid.at(0).size()); 181 | ASSERT_EQ(3, grid.at(1).size()); 182 | ASSERT_EQ(3, grid.at(2).size()); 183 | ASSERT_EQ(3, grid.at(3).size()); 184 | ASSERT_ANY_THROW(grid.at(3).at(3)); // Only 3 items in X direction (horizontal) so no index 3 185 | ASSERT_ANY_THROW(grid.at(4).size()); // Only 4 items in Y direction (vertical) so no index 4 186 | } 187 | 188 | /* 189 | * Test that if there is a NxN map with only a single element, only that single element is returned 190 | */ 191 | TEST(TestMap_2_goals, testFindSingle) 192 | { 193 | /* Map will be 3x3 with only the middle value set to true, the others being false 194 | * 195 | * [ 0 0 0 ] 196 | * [ 0 1 0 ] 197 | * [ 0 0 0 ] 198 | */ 199 | std::vector > grid = makeTestGrid(3, 3); 200 | grid[1][1] = true; 201 | std::list goals; 202 | goals = map_2_goals(grid, true); 203 | ASSERT_EQ(1, goals.size()); 204 | 205 | Point_t center = {1, 1}; // NOLINT 206 | ASSERT_EQ(center.x, goals.front().x); 207 | ASSERT_EQ(center.y, goals.front().y); 208 | } 209 | 210 | /* 211 | * Test that if there is a 3x3 map with 3 elements, that those elements and no more are returned 212 | */ 213 | TEST(TestMap_2_goals, testNumberOfGoals) 214 | { 215 | /* Map: 216 | * [ 1 0 0 ] 217 | * [ 0 1 0 ] 218 | * [ 0 0 1 ] 219 | */ 220 | std::vector > grid = makeTestGrid(3, 3); 221 | grid[0][0] = true; 222 | grid[1][1] = true; 223 | grid[2][2] = true; 224 | std::list goals; 225 | goals = map_2_goals(grid, true); 226 | std::vector goalVector = std::vector(goals.begin(), goals.end()); 227 | 228 | // We only set 3 points to 1, so we should only find those 3 229 | ASSERT_EQ(3, goals.size()); 230 | 231 | // And specifically those 3, not anything else 232 | Point_t corner0 = {0, 0}; // NOLINT 233 | Point_t center = {1, 1}; // NOLINT 234 | Point_t corner2 = {2, 2}; // NOLINT 235 | ASSERT_EQ(corner0, goalVector.at(0)); 236 | ASSERT_EQ(center, goalVector.at(1)); 237 | ASSERT_EQ(corner2, goalVector.at(2)); 238 | } 239 | 240 | /* 241 | * map_2_goals can look for both true and false cells. 242 | * In either case, we should return the correct number of goals 243 | */ 244 | TEST(TestMap_2_goals, testInvertedMap) 245 | { 246 | /* Map: 247 | * [ 1 1 1 ] 248 | * [ 1 0 1 ] 249 | * [ 1 1 1 ] 250 | */ 251 | std::vector > grid = makeTestGrid(3, 3, true); 252 | grid[1][1] = false; 253 | 254 | ASSERT_EQ(8, map_2_goals(grid, true).size()); // There are 8 true values 255 | ASSERT_EQ(1, map_2_goals(grid, false).size()); // There is only 1 false value 256 | } 257 | 258 | /* 259 | * If we use a non-square grid, do we use the correct order of indexing? 260 | * Coordinates use x,y and y indexes over rows, x over indices in that row 261 | */ 262 | TEST(TestMap_2_goals, testCoordinateOrder) 263 | { 264 | /* Map is 3x4 265 | * [ 0 0 0 ] 266 | * [ 0 0 0 ] 267 | * [ 0 0 0 ] 268 | * [ 0 0 1 ] 269 | * 270 | */ 271 | std::vector > grid = makeTestGrid(3, 4, false); 272 | grid.at(3).at(2) = true; 273 | std::list goals; 274 | 275 | goals = map_2_goals(grid, true); 276 | ASSERT_EQ(1, goals.size()); 277 | 278 | Point_t corner0 = {2, 3}; // NOLINT 279 | ASSERT_EQ(corner0.x, goals.front().x); 280 | ASSERT_EQ(corner0.y, goals.front().y); 281 | } 282 | 283 | /* LEGENDA 284 | * Note: in tests for the A* path finding algorithm, use this legend for the maps: 285 | * s: start 286 | * p: path node 287 | * v: visited 288 | * 1: obstacle / occupied 289 | * 0: open node / unoccupied 290 | */ 291 | 292 | /* 293 | * On an empty map, find a route to open space should be as simple as stepping to the next position 294 | * (when the current position is already visited and thus not OK) 295 | */ 296 | TEST(TestAStarToOpenSpace, testEmptyMap) 297 | { 298 | /* 299 | * [s] We start from here, so this is visited 300 | * [0] And this is still open, so we should go here to find open space 301 | * [0] 302 | * [0] 303 | */ 304 | std::vector > grid = makeTestGrid(1, 4, false); 305 | std::vector > visited = makeTestGrid(1, 4, false); 306 | std::list goals; 307 | goals.push_back({0, 3}); // NOLINT 308 | 309 | visited[0][0] = true; 310 | gridNode_t start; 311 | start.pos = {0, 0}; // NOLINT 312 | start.cost = 1; 313 | start.he = 0; 314 | 315 | std::list pathNodes; 316 | 317 | bool resign = a_star_to_open_space(grid, // map to traverse, all empty 318 | start, // Start 319 | 1, // Cost of traversing a node 320 | visited, // Visited nodes, of which there are none yet 321 | goals, 322 | pathNodes); 323 | /* 324 | * [p] We came from here 325 | * [p] and this is the first unvisited, non-obstacle cell so we step here 326 | * [0] 327 | * [0] 328 | */ 329 | 330 | std::vector pathNodesVector = std::vector(pathNodes.begin(), pathNodes.end()); 331 | ASSERT_EQ(false, resign); 332 | 333 | // First element is that initial node 334 | ASSERT_EQ(0, pathNodesVector.at(0).pos.x); 335 | ASSERT_EQ(0, pathNodesVector.at(0).pos.y); 336 | 337 | // First step we take reaches free space 338 | ASSERT_EQ(0, pathNodesVector.at(1).pos.x); 339 | ASSERT_EQ(1, pathNodesVector.at(1).pos.y); 340 | 341 | // 2 nodes: start and end 342 | ASSERT_EQ(2, pathNodes.size()); 343 | } 344 | 345 | /* 346 | * Small extension of testEmptyMap: there is one more cell already visited 347 | * That mean the path is thus from start, over the one visited cell to a new unvisited cell, so length 3 348 | */ 349 | TEST(TestAStarToOpenSpace, testSingleVisitedCellMap) 350 | { 351 | /* 352 | * [s] 353 | * [v] 354 | * [0] 355 | * [0] 356 | */ 357 | 358 | std::vector > grid = makeTestGrid(1, 4, false); 359 | std::vector > visited = makeTestGrid(1, 4, false); 360 | std::list goals = map_2_goals(grid, true); 361 | 362 | visited[0][0] = true; 363 | visited[1][0] = true; 364 | gridNode_t start; 365 | start.pos = {0, 0}; // NOLINT 366 | start.cost = 1; 367 | start.he = 0; 368 | 369 | std::list pathNodes; 370 | 371 | bool resign = a_star_to_open_space(grid, // map to traverse, all empty 372 | start, // Start 373 | 1, // Cost of traversing a node 374 | visited, // Visited nodes, of which there are none yet 375 | goals, 376 | pathNodes); 377 | /* 378 | * [p] 379 | * [p] 380 | * [p] 381 | * [0] 382 | */ 383 | std::vector pathNodesVector = std::vector(pathNodes.begin(), pathNodes.end()); 384 | ASSERT_EQ(false, resign); 385 | 386 | ASSERT_EQ(0, pathNodesVector.at(0).pos.x); 387 | ASSERT_EQ(0, pathNodesVector.at(0).pos.y); 388 | 389 | ASSERT_EQ(0, pathNodesVector.at(1).pos.x); 390 | ASSERT_EQ(1, pathNodesVector.at(1).pos.y); 391 | 392 | ASSERT_EQ(0, pathNodesVector.at(2).pos.x); 393 | ASSERT_EQ(2, pathNodesVector.at(2).pos.y); 394 | 395 | ASSERT_EQ(3, pathNodes.size()); 396 | } 397 | 398 | /* 399 | * When we start in the corner and have somehow visited the direct neighbors, the shortest path should involve 3 points 400 | */ 401 | TEST(TestAStarToOpenSpace, testSingleMulticellVisitedMap) 402 | { 403 | /* 404 | * [s v 0 0] 405 | * [v v 0 0] 406 | * [0 0 0 0] 407 | * [0 0 0 0] 408 | */ 409 | std::vector > grid = makeTestGrid(4, 4, false); 410 | std::vector > visited = makeTestGrid(4, 4, false); 411 | std::list goals = map_2_goals(grid, true); 412 | 413 | visited[0][0] = true; 414 | visited[1][0] = true; 415 | visited[0][1] = true; 416 | visited[1][1] = true; 417 | 418 | gridNode_t start; 419 | start.pos = {0, 0}; // NOLINT 420 | start.cost = 1; 421 | start.he = 0; 422 | 423 | std::list pathNodes; 424 | 425 | bool resign = a_star_to_open_space(grid, // map to traverse, all empty 426 | start, // Start 427 | 1, // Cost of traversing a node 428 | visited, // Visited nodes, of which there are none yet 429 | goals, 430 | pathNodes); 431 | /* Several paths possible, but each covers 3 nodes, e.g. 432 | * [p p p 0] 433 | * [v v 0 0] 434 | * [0 0 0 0] 435 | * [0 0 0 0] 436 | */ 437 | std::vector pathNodesVector = std::vector(pathNodes.begin(), pathNodes.end()); 438 | ASSERT_EQ(false, resign); 439 | ASSERT_EQ(3, pathNodes.size()); 440 | } 441 | 442 | /* 443 | * In a map with 2 walls and everything between those wall already visited, test that the path has the expected length 444 | */ 445 | TEST(TestAStarToOpenSpace, testMazeMap) 446 | { 447 | /* 448 | * [s v v v] 449 | * [1 1 1 v] 450 | * [0 v v v] goal is the single 0 in this map 451 | * [1 1 1 1] 452 | */ 453 | std::vector > grid = makeTestGrid(4, 4, false); 454 | grid[1][0] = 1; 455 | grid[1][1] = 1; 456 | grid[1][2] = 1; 457 | grid[3][0] = 1; 458 | grid[3][1] = 1; 459 | grid[3][2] = 1; 460 | grid[3][3] = 1; 461 | std::vector > visited = makeTestGrid(4, 4, false); 462 | std::list goals = map_2_goals(grid, true); 463 | 464 | visited[0][0] = 1; 465 | visited[0][1] = 1; 466 | visited[0][2] = 1; 467 | visited[0][3] = 1; 468 | visited[1][3] = 1; 469 | visited[2][3] = 1; 470 | visited[2][2] = 1; 471 | visited[2][1] = 1; 472 | 473 | gridNode_t start; 474 | start.pos = {0, 0}; // NOLINT 475 | start.cost = 1; 476 | start.he = 0; 477 | 478 | std::list pathNodes; 479 | 480 | bool resign = a_star_to_open_space(grid, // map to traverse, all empty 481 | start, // Start 482 | 1, // Cost of traversing a node 483 | visited, // Visited nodes, of which there are none yet 484 | goals, 485 | pathNodes); 486 | /* 487 | * [p p p p] 488 | * [1 1 1 p] 489 | * [p p p p] 490 | * [1 1 1 1] 491 | */ 492 | ASSERT_EQ(false, resign); 493 | ASSERT_EQ(9, pathNodes.size()); 494 | } 495 | 496 | /* 497 | * Extension of testMazeMap, but the bottom wall has a hole that is also not yet visited. 498 | * That is the closest open space, so check that the found path goes there. 499 | */ 500 | TEST(TestAStarToOpenSpace, testMazeWithHoleMap) 501 | { 502 | /* There are 2 open spaces in this test, the one at 3,3 is closest 503 | * [s v v v] 504 | * [1 1 1 v] 505 | * [0 v v v] 506 | * [1 1 1 0] 507 | */ 508 | std::vector > grid = makeTestGrid(4, 4, false); 509 | grid[1][0] = 1; 510 | grid[1][1] = 1; 511 | grid[1][2] = 1; 512 | grid[3][0] = 1; 513 | grid[3][1] = 1; 514 | grid[3][2] = 1; 515 | std::vector > visited = makeTestGrid(4, 4, false); 516 | std::list goals = map_2_goals(grid, true); 517 | 518 | visited[0][0] = 1; 519 | visited[0][1] = 1; 520 | visited[0][2] = 1; 521 | visited[0][3] = 1; 522 | visited[1][3] = 1; 523 | visited[2][3] = 1; 524 | visited[2][2] = 1; 525 | visited[2][1] = 1; 526 | 527 | gridNode_t start; 528 | start.pos = {0, 0}; // NOLINT 529 | start.cost = 1; 530 | start.he = 0; 531 | 532 | std::list pathNodes; 533 | 534 | bool resign = a_star_to_open_space(grid, // map to traverse, all empty 535 | start, // Start 536 | 1, // Cost of traversing a node 537 | visited, // Visited nodes, of which there are none yet 538 | goals, 539 | pathNodes); 540 | /* 541 | * [p p p p] 542 | * [1 1 1 p] 543 | * [0 v v p] // These 2 visited nodes but unconnected nodes cannot happen in reality but fine for this test 544 | * [1 1 1 p] 545 | */ 546 | ASSERT_EQ(false, resign); 547 | ASSERT_EQ(7, pathNodes.size()); 548 | } 549 | 550 | /* 551 | * When we start in the corner and have obstacles around us, check that we can indeed not find a path 552 | */ 553 | TEST(TestAStarToOpenSpace, testBlockedMap) 554 | { 555 | /* 556 | * [s 1 0 0] 557 | * [1 1 0 0] 558 | * [0 0 0 0] 559 | * [0 0 0 0] 560 | */ 561 | std::vector > grid = makeTestGrid(4, 4, false); 562 | std::vector > visited = makeTestGrid(4, 4, false); 563 | 564 | visited[0][0] = true; 565 | grid[1][0] = true; 566 | grid[0][1] = true; 567 | grid[1][1] = true; 568 | std::list goals = map_2_goals(grid, true); 569 | 570 | gridNode_t start; 571 | start.pos = {0, 0}; // NOLINT 572 | start.cost = 1; 573 | start.he = 0; 574 | 575 | std::list pathNodes; 576 | 577 | bool resign = a_star_to_open_space(grid, // map to traverse, all empty 578 | start, // Start 579 | 1, // Cost of traversing a node 580 | visited, // Visited nodes, of which there are none yet 581 | goals, 582 | pathNodes); 583 | // No path possible so we should resign 584 | ASSERT_EQ(true, resign); 585 | ASSERT_EQ(1, pathNodes.size()); // Only the cell we start at: 586 | ASSERT_EQ(start.pos, pathNodes.front().pos); 587 | } 588 | // Run all the tests that were declared with TEST() 589 | int main(int argc, char **argv) 590 | { 591 | testing::InitGoogleTest(&argc, argv); 592 | return RUN_ALL_TESTS(); 593 | } 594 | -------------------------------------------------------------------------------- /test/src/test_spiral_stc.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | // Created by nobleo on 27-9-18. 5 | // 6 | 7 | /* 8 | * Full coverage simply requires all reachable nodes to be visited. 9 | * We check this by counting the number of unique elements in the path. 10 | * By putting the path nodes in a set, we are left with only the unique elements 11 | * and then we can count how big that set is (i.e. the cardinality of the set of path nodes) 12 | */ 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | cv::Mat drawMap(std::vector > const& grid); 31 | 32 | cv::Mat drawPath(const cv::Mat &mapImg, 33 | const cv::Mat &pathImg, 34 | const Point_t &start, 35 | std::list &path); 36 | 37 | /* 38 | * On a map with nothing on it, spiral_stc should cover all the nodes of the map 39 | */ 40 | TEST(TestSpiralStc, testFillEmptyMap) 41 | { 42 | std::vector > grid = makeTestGrid(4, 4, false); 43 | 44 | Point_t start = {0, 0}; 45 | int multiple_pass_counter, visited_counter; 46 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 47 | start, 48 | multiple_pass_counter, 49 | visited_counter); 50 | 51 | ASSERT_EQ(4 * 4, path.size()); // All nodes of the 4x4 map are covered 52 | } 53 | 54 | /* 55 | * On a map with a single obstacle, all the other nodes should still be visited. 56 | */ 57 | TEST(TestSpiralStc, testFillMapWithOneObstacle) 58 | { 59 | /* 60 | * [s 0 0 0] 61 | * [0 0 0 0] 62 | * [0 0 1 0] 63 | * [0 0 0 0] 64 | */ 65 | std::vector > grid = makeTestGrid(4, 4, false); 66 | grid[2][2] = 1; 67 | 68 | Point_t start = {0, 0}; 69 | int multiple_pass_counter, visited_counter; 70 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 71 | start, 72 | multiple_pass_counter, 73 | visited_counter); 74 | 75 | // By Adding the nodes of the path to the set, we only retain the unique elements 76 | std::set pathSet(path.begin(), path.end()); 77 | 78 | // Because the area is 4*4 but with 1 obstacle, coverage all reachable nodes should over 4*4 - 1 = 15 nodes 79 | ASSERT_EQ((4 * 4) - 1, pathSet.size()); 80 | } 81 | 82 | /* 83 | * In a map with 2 obstacles, still the complete map should be covered except for those 2 obstacles 84 | */ 85 | TEST(TestSpiralStc, testFillMapWith2Obstacles) 86 | { 87 | /* 88 | * [s 0 0 0] 89 | * [0 0 0 0] 90 | * [0 0 1 0] 91 | * [0 0 0 1] 92 | */ 93 | std::vector > grid = makeTestGrid(4, 4, false); 94 | grid[2][2] = 1; 95 | grid[3][3] = 1; 96 | 97 | Point_t start = {0, 0}; 98 | int multiple_pass_counter, visited_counter; 99 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 100 | start, 101 | multiple_pass_counter, 102 | visited_counter); 103 | 104 | // By Adding the nodes of the path to the set, we only retain the unique elements 105 | std::set pathSet(path.begin(), path.end()); 106 | 107 | // Because the area is 4*4 but with 1 obstacle, coverage all reachable nodes should over 4*4 - 2 = 14 nodes 108 | ASSERT_EQ((4 * 4) - 2, pathSet.size()); 109 | } 110 | 111 | /* 112 | * On a 4x4 map where the opposite right half of the map is blocked, we can cover only the 4x2 reachable nodes 113 | */ 114 | TEST(TestSpiralStc, testFillMapWithHalfBlocked) 115 | { 116 | /* 117 | * [s 0 1 0] 118 | * [0 0 1 0] 119 | * [0 0 1 0] 120 | * [0 0 1 0] 121 | */ 122 | std::vector > grid = makeTestGrid(4, 4, false); 123 | grid[0][2] = 1; 124 | grid[1][2] = 1; 125 | grid[2][2] = 1; 126 | grid[3][2] = 1; 127 | 128 | Point_t start = {0, 0}; 129 | int multiple_pass_counter, visited_counter; 130 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 131 | start, 132 | multiple_pass_counter, 133 | visited_counter); 134 | 135 | // By Adding the nodes of the path to the set, we only retain the unique elements 136 | std::set pathSet(path.begin(), path.end()); 137 | 138 | // Because the area is 4*4 but can only visit half the map, half the 4x4 area should be covered 139 | ASSERT_EQ((4 * 4) / 2, pathSet.size()); 140 | } 141 | 142 | /* 143 | * On a map with a wall almost blocking off a half of the map, but leaving a gap to the other side, 144 | * spiral_stc should still cover all reachable nodes 145 | */ 146 | TEST(TestSpiralStc, testFillMapWithWall) 147 | { 148 | /* 149 | * [s 0 1 0] 150 | * [0 0 1 0] 151 | * [0 0 1 0] 152 | * [0 0 0 0] 153 | */ 154 | std::vector > grid = makeTestGrid(4, 4, false); 155 | grid[0][2] = 1; 156 | grid[1][2] = 1; 157 | grid[2][2] = 1; 158 | 159 | Point_t start = {0, 0}; 160 | int multiple_pass_counter, visited_counter; 161 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 162 | start, 163 | multiple_pass_counter, 164 | visited_counter); 165 | 166 | // By Adding the nodes of the path to the set, we only retain the unique elements 167 | std::set pathSet(path.begin(), path.end()); 168 | 169 | // Because the area is 4*4 there is a 3-length wall 170 | ASSERT_EQ((4 * 4) - 3, pathSet.size()); 171 | } 172 | 173 | /* 174 | * This test case features a very short dead-end 175 | */ 176 | TEST(TestSpiralStc, testDeadEnd1) 177 | { 178 | /* 179 | * [0 0 1 0] 180 | * [0 1 0 0] 181 | * [0 s 0 0] 182 | * [0 0 0 0] 183 | */ 184 | std::vector > grid = makeTestGrid(4, 4, false); 185 | grid[0][2] = 1; 186 | grid[1][1] = 1; 187 | 188 | cv::Mat mapImg = drawMap(grid); 189 | 190 | Point_t start = {1, 2}; 191 | int multiple_pass_counter, visited_counter; 192 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 193 | start, 194 | multiple_pass_counter, 195 | visited_counter); 196 | 197 | cv::Mat pathImg = mapImg.clone(); 198 | cv::Mat pathViz = drawPath(mapImg, pathImg, start, path); 199 | cv::imwrite("/tmp/testDeadEnd1.png", pathViz); 200 | 201 | // By Adding the nodes of the path to the set, we only retain the unique elements 202 | std::set pathSet(path.begin(), path.end()); 203 | 204 | // Because the area is 4*4 and there are2 obstacle cells 205 | ASSERT_EQ((4 * 4) - 2, pathSet.size()); 206 | } 207 | 208 | /* 209 | * This test case is an extension of testDeadEnd1, where the top row is also covered as an obstacle. 210 | * The top row is covered but the obstacle from testDeadEnd1 is shifted downwards 211 | * in an attempt to see if SpiralSTC also fails when a dead-end is not on the edge of the map 212 | * (but below a row of obstacles) 213 | */ 214 | TEST(TestSpiralStc, testDeadEnd1WithTopRow) 215 | { 216 | /* 217 | * [1 1 1 1] 218 | * [0 0 1 0] 219 | * [0 1 0 0] 220 | * [0 s 0 0] 221 | * [0 0 0 0] 222 | */ 223 | std::vector > grid = makeTestGrid(4, 5, false); 224 | grid[0][0] = 1; 225 | grid[0][1] = 1; 226 | grid[0][2] = 1; 227 | grid[0][3] = 1; 228 | 229 | grid[1][2] = 1; 230 | grid[2][1] = 1; 231 | 232 | cv::Mat mapImg = drawMap(grid); 233 | 234 | Point_t start = {1, 3}; 235 | int multiple_pass_counter, visited_counter; 236 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 237 | start, 238 | multiple_pass_counter, 239 | visited_counter); 240 | 241 | cv::Mat pathImg = mapImg.clone(); 242 | cv::Mat pathViz = drawPath(mapImg, pathImg, start, path); 243 | cv::imwrite("/tmp/testDeadEnd1WithTopRow.png", pathViz); 244 | 245 | // By Adding the nodes of the path to the set, we only retain the unique elements 246 | std::set pathSet(path.begin(), path.end()); 247 | 248 | // Because the area is 4*5 and there are 6 cells blocked in total 249 | ASSERT_EQ((4 * 5) - 6, pathSet.size()); 250 | } 251 | 252 | /* 253 | * This test case features a very short dead-end 254 | */ 255 | TEST(TestSpiralStc, testDeadEnd2) 256 | { 257 | /* 258 | * [1 0 0 0] 259 | * [0 1 0 0] 260 | * [0 s 0 0] 261 | * [0 0 0 0] 262 | */ 263 | std::vector > grid = makeTestGrid(4, 4, false); 264 | grid[0][0] = 1; 265 | grid[1][1] = 1; 266 | 267 | cv::Mat mapImg = drawMap(grid); 268 | 269 | Point_t start = {3, 0}; 270 | int multiple_pass_counter, visited_counter; 271 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 272 | start, 273 | multiple_pass_counter, 274 | visited_counter); 275 | 276 | cv::Mat pathImg = mapImg.clone(); 277 | cv::Mat pathViz = drawPath(mapImg, pathImg, start, path); 278 | cv::imwrite("/tmp/testDeadEnd2.png", pathViz); 279 | 280 | // By Adding the nodes of the path to the set, we only retain the unique elements 281 | std::set pathSet(path.begin(), path.end()); 282 | 283 | // Because the area is 4*4 and there are2 obstacle cells 284 | ASSERT_EQ((4 * 4) - 2, pathSet.size()); 285 | } 286 | 287 | /* 288 | * This test case features a very short dead-end 289 | */ 290 | TEST(TestSpiralStc, testDeadEnd3) 291 | { 292 | /* 293 | * [0 0 0 0 0 0 1 0 0] 294 | * [1 0 1 0 1 1 0 0 0] 295 | * [0 0 0 0 1 2 0 0 0] 296 | * [0 0 0 0 0 0 0 0 0] 297 | * [0 0 0 0 0 0 0 0 0] 298 | * [0 0 0 0 0 0 0 0 0] 299 | */ 300 | std::vector > grid = makeTestGrid(9, 6, false); 301 | grid[0][6] = 1; 302 | grid[1][0] = 1; 303 | grid[1][2] = 1; 304 | grid[1][4] = 1; 305 | grid[1][5] = 1; 306 | grid[2][4] = 1; 307 | 308 | cv::Mat mapImg = drawMap(grid); 309 | 310 | Point_t start = {5, 2}; 311 | int multiple_pass_counter, visited_counter; 312 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 313 | start, 314 | multiple_pass_counter, 315 | visited_counter); 316 | 317 | cv::Mat pathImg = mapImg.clone(); 318 | cv::Mat pathViz = drawPath(mapImg, pathImg, start, path); 319 | cv::imwrite("/tmp/testDeadEnd3.png", pathViz); 320 | 321 | // By Adding the nodes of the path to the set, we only retain the unique elements 322 | std::set pathSet(path.begin(), path.end()); 323 | 324 | // Because the area is 4*4 and there are2 obstacle cells 325 | ASSERT_EQ((6 * 9) - 6, pathSet.size()); 326 | } 327 | 328 | /* 329 | * This test case features a very short dead-end 330 | */ 331 | TEST(TestSpiralStc, testDeadEnd3WithTopRow) 332 | { 333 | /* 334 | * [1 1 1 1 1 1 1 1 1] 335 | * [0 0 0 0 0 0 1 0 0] 336 | * [1 0 1 0 1 1 0 0 0] 337 | * [0 0 0 0 1 2 0 0 0] 338 | * [0 0 0 0 0 0 0 0 0] 339 | * [0 0 0 0 0 0 0 0 0] 340 | * [0 0 0 0 0 0 0 0 0] 341 | */ 342 | std::vector > grid = makeTestGrid(9, 7, false); 343 | grid[0][0] = 1; 344 | grid[0][1] = 1; 345 | grid[0][2] = 1; 346 | grid[0][3] = 1; 347 | grid[0][4] = 1; 348 | grid[0][5] = 1; 349 | grid[0][6] = 1; 350 | 351 | grid[1][6] = 1; 352 | grid[2][0] = 1; 353 | grid[2][2] = 1; 354 | grid[2][4] = 1; 355 | grid[2][5] = 1; 356 | grid[3][4] = 1; 357 | 358 | cv::Mat mapImg = drawMap(grid); 359 | 360 | Point_t start = {5, 3}; // NOLINT 361 | int multiple_pass_counter, visited_counter; 362 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 363 | start, 364 | multiple_pass_counter, 365 | visited_counter); 366 | 367 | cv::Mat pathImg = mapImg.clone(); 368 | cv::Mat pathViz = drawPath(mapImg, pathImg, start, path); 369 | cv::imwrite("/tmp/testDeadEnd3WithTopRow.png", pathViz); 370 | 371 | // By Adding the nodes of the path to the set, we only retain the unique elements 372 | std::set pathSet(path.begin(), path.end()); 373 | 374 | // Because the area is 4*4 and there are2 obstacle cells 375 | ASSERT_EQ((7 * 9) - 6 - 7, pathSet.size()); 376 | } 377 | 378 | /** 379 | * Draw a nested vector of bools into an openCV image 380 | * @param grid 381 | * @return 2D 8-bit single-channel image 382 | */ 383 | cv::Mat drawMap(std::vector > const& grid) 384 | { 385 | int y_size = static_cast(grid.size()); 386 | int x_size = static_cast(grid[0].size()); 387 | 388 | cv::Mat mapImg = cv::Mat::zeros(y_size, x_size, CV_8U); // CV_8U 8bit unsigned int 1 channel 389 | for (int k = 0; k < y_size; k++) 390 | { 391 | for (int l = 0; l < x_size; l++) 392 | { 393 | if (grid[k][l]) 394 | { 395 | cv::rectangle(mapImg, {l, k}, {l, k}, 255); // NOLINT 396 | } 397 | } 398 | } 399 | return mapImg; 400 | } 401 | 402 | /** 403 | * Draw path on a copy of the map 404 | * This is done twice: one to serve as input for calcDifference and another is returned for visualisation purposes 405 | * @param mapImg original map with just obstacles 406 | * @param pathImg Image that will feed into calcDifference 407 | * @param start Where does the path start? 408 | * @param path the actual path to be drawn 409 | * @return 2D RGB image for visualisation purposes 410 | */ 411 | cv::Mat drawPath(const cv::Mat &mapImg, 412 | const cv::Mat &pathImg, 413 | const Point_t &start, 414 | std::list &path) 415 | { 416 | cv::Mat pathViz = cv::Mat::zeros(mapImg.cols, mapImg.rows, CV_8UC3); 417 | std::vector channels; 418 | channels.push_back(mapImg.clone()); 419 | channels.push_back(mapImg.clone()); 420 | channels.push_back(mapImg.clone()); 421 | cv::merge(channels, pathViz); 422 | 423 | int step = 0; 424 | for (std::list::iterator it = path.begin(); it != path.end(); ++it) 425 | { 426 | // std::cout << "Path at (" << it->x << ", " << it->y << ")" << std::endl; 427 | cv::rectangle(pathImg, {it->x, it->y}, {it->x, it->y}, 255); // NOLINT 428 | 429 | // Color the path in lighter and lighter color towards the end 430 | step++; 431 | int value = ((step * 200) / static_cast(path.size())) + 50; 432 | cv::Scalar color(value, 128, 128); 433 | cv::rectangle(pathViz, {it->x, it->y}, {it->x, it->y}, color); // NOLINT 434 | } 435 | 436 | // Draw the start and end in green and red, resp. 437 | cv::Scalar green(0, 255, 0); 438 | cv::Scalar red(0, 0, 255); 439 | cv::rectangle(pathViz, 440 | {start.x, start.y}, 441 | {start.x, start.y}, 442 | green); 443 | cv::rectangle(pathViz, 444 | {path.back().x, path.back().y}, 445 | {path.back().x, path.back().y}, 446 | red); 447 | return pathViz; 448 | } 449 | 450 | /** 451 | * Determine whether the drawn path covers all that can be covered 452 | * @param mapImg original map with obstacles 453 | * @param pathImg map with the path drawn into it, from drawPath 454 | * @param start where does the path start? 455 | * @return 456 | */ 457 | int calcDifference(const cv::Mat &mapImg, const cv::Mat &pathImg, const Point_t& start) 458 | { 459 | cv::Mat floodfilledImg = mapImg.clone(); 460 | cv::floodFill(floodfilledImg, {start.x, start.y}, 255); // NOLINT 461 | cv::Mat difference; 462 | cv::subtract(floodfilledImg, pathImg, difference); 463 | 464 | return cv::countNonZero(difference); 465 | } 466 | 467 | /** 468 | * Find a proper starting point in the map, i.e. any place that is not an obstacle 469 | * @param grid 470 | * @return 471 | */ 472 | Point_t findStart(std::vector > const& grid) 473 | { 474 | unsigned int seed = time(NULL); 475 | int y_size = grid.size(); 476 | int x_size = grid[0].size(); 477 | 478 | Point_t start = {rand_r(&seed) % x_size, rand_r(&seed) % y_size}; // Start in some random place 479 | while (grid[start.y][start.x]) 480 | { 481 | // Try to find a better starting point that is not inside an obstacle 482 | start = {rand_r(&seed) % x_size, rand_r(&seed) % y_size}; // Start in some random place 483 | } 484 | return start; 485 | } 486 | 487 | /* 488 | * Create a NxM map, spawn X random obstacles (so that some percentage of the map is covered by obstacles) 489 | Run coverage planning on that map and check that all reachable cells are covered (by using OpenCV Floodfill) 490 | 491 | 1. Create map with obstacles 492 | 2. Convert this to an image called mapImg 493 | 3. Copy mapImg as floodfilledImg 494 | 3. Copy mapImg as pathImg 495 | 4. Perform floodfill from start position on floodfilledImg 496 | 5. On each coordinate in path, fill pixel at that coordinate in pathImg 497 | 6. pathImg and floodfilledImg should be identical 498 | */ 499 | TEST(TestSpiralStc, testRandomMap) 500 | { 501 | // Seed pseudorandom sequence to create *reproducible test 502 | unsigned int seed = 12345; 503 | int failures = 0; 504 | int success = 0; 505 | int tests = 0; 506 | for (int i = 0; i < 5; ++i) 507 | // Or use https://github.com/google/googletest/blob/master/googletest/docs/advanced.md#repeating-the-tests 508 | { 509 | tests++; 510 | int x_size = rand_r(&seed) % 100 + 1; 511 | int y_size = rand_r(&seed) % 100 + 1; 512 | std::vector > grid = makeTestGrid(x_size, y_size, false); 513 | randomFillTestGrid(grid, 20); // ...% fill of obstacles 514 | 515 | cv::Mat mapImg = drawMap(grid); 516 | Point_t start = findStart(grid); 517 | int multiple_pass_counter, visited_counter; 518 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 519 | start, 520 | multiple_pass_counter, 521 | visited_counter); 522 | 523 | cv::Mat pathImg = mapImg.clone(); 524 | cv::Mat pathViz = drawPath(mapImg, pathImg, start, path); 525 | int differentPixelCount = calcDifference(mapImg, pathImg, start); 526 | if (differentPixelCount) 527 | { 528 | cv::imwrite("/tmp/" + std::to_string(i) + "_path_viz.png", pathViz); 529 | failures++; 530 | } 531 | else 532 | { 533 | success++; 534 | } 535 | EXPECT_EQ(0, differentPixelCount); 536 | } 537 | 538 | ASSERT_EQ(0, failures); 539 | ASSERT_EQ(tests, success); 540 | } 541 | 542 | // Run all the tests that were declared with TEST() 543 | int main(int argc, char **argv) 544 | { 545 | testing::InitGoogleTest(&argc, argv); 546 | return RUN_ALL_TESTS(); 547 | } 548 | --------------------------------------------------------------------------------