From ae79feb86cefcf0b9d2569887d8b14d7806ceed1 Mon Sep 17 00:00:00 2001 From: Ethan Kim Date: Fri, 18 Jun 2021 11:53:47 -0600 Subject: [PATCH 01/12] move base sim --- maps/square.png | Bin 0 -> 3243 bytes test/full_coverage_path_planner/fcpp.rviz | 92 +++++++++--------- .../move_base_sim.launch | 62 ++++++++++++ .../test_full_coverage_path_planner.launch | 10 +- 4 files changed, 113 insertions(+), 51 deletions(-) create mode 100644 maps/square.png create mode 100644 test/full_coverage_path_planner/move_base_sim.launch diff --git a/maps/square.png b/maps/square.png new file mode 100644 index 0000000000000000000000000000000000000000..e747bdf69115969264858ff22eb9876465766317 GIT binary patch literal 3243 zcmeAS@N?(olHy`uVBq!ia0y~yVAKI&4mO}jWo=(60|QTyr;B4qMcmt)j(IVjLM(wF z|Fu73*ei9$VJ{c^DXsZ@54!TFnFwAu{~ieB>jfMb8W|ep4{}8mDKT;Uady~X!@?qP zLO@_mk5;=^wsaA<2tNDZ+R zVPs)BEe$k|hw40J2E%H)y1;d%VEQ^iucD@U-vCt zs>gV+YlrZf4vlm_poJpu@9s#ryK&1ERfPz_@+MKir)M-756*hDS6Y0H^|WcbfNA8B z-v*^cb1Z=hg7=g^ZYn)~DQJ2_g6kciqo)`zXJN_i`koiF;ax=3E1;V?tVbO^8q%Z5 zV>Ba%M+bgR4=@$1a?9gJYD@<);T3K0RVs+N!S1Y literal 0 HcmV?d00001 diff --git a/test/full_coverage_path_planner/fcpp.rviz b/test/full_coverage_path_planner/fcpp.rviz index c052c0b..7fd802a 100644 --- a/test/full_coverage_path_planner/fcpp.rviz +++ b/test/full_coverage_path_planner/fcpp.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 554 + Tree Height: 548 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -62,6 +62,7 @@ Visualization Manager: Value: true odom: Value: true + Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true @@ -86,6 +87,14 @@ Visualization Manager: Unreliable: false Use Timestamp: false Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /path_coverage_marker + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path @@ -103,10 +112,35 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: Arrows + Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.019999999552965164 Shaft Length: 0.05000000074505806 - Topic: /move_base/SpiralSTC/plan + Topic: /move_base/TrajectoryPlannerROS/global_plan + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/TrajectoryPlannerROS/local_plan Unreliable: false Value: true - Angle Tolerance: 0.10000000149011612 @@ -130,6 +164,7 @@ Visualization Manager: Keep: 10000 Name: Odometry Position Tolerance: 0.10000000149011612 + Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 @@ -143,50 +178,16 @@ Visualization Manager: Topic: /odom Unreliable: false Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /visualization_marker - Name: Tracking_pid local planner - Namespaces: - axle point: true - control point: true - goal point: true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /move_base_flex/tracking_pid/interpolator - Name: Interpolator (mbf) - Namespaces: - {} - Queue Size: 100 - Value: true - Alpha: 0.30000001192092896 Class: rviz/Map Color Scheme: map Draw Behind: false - Enabled: true + Enabled: false Name: Coverage progress map Topic: /coverage_grid Unreliable: false Use Timestamp: false - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /move_base/TrackingPidLocalPlanner/interpolator - Name: Interpolator (mb) - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Class: rviz/Polygon - Color: 25; 255; 0 - Enabled: true - Name: Footprint - Topic: /move_base_flex/global_costmap/footprint - Unreliable: false - Value: true + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 @@ -224,19 +225,18 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 97.19247436523438 + Scale: 107.97452545166016 Target Frame: - Value: TopDownOrtho (rviz) - X: -0.14492475986480713 - Y: 1.0135213136672974 + X: -0.3638369143009186 + Y: 0.05843987315893173 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1023 + Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000361fc0200000008fb0000001200530065006c0065006300740069006f006e010000003d000000a60000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000e9000002b5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f0000003efc0100000002fb0000000800540069006d006501000000000000039f000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000022f0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035afc0200000008fb0000001200530065006c0065006300740069006f006e010000003d000000a50000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000e8000002af000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003830000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -245,6 +245,6 @@ Window Geometry: collapsed: true Views: collapsed: true - Width: 927 - X: 67 + Width: 1267 + X: 72 Y: 27 diff --git a/test/full_coverage_path_planner/move_base_sim.launch b/test/full_coverage_path_planner/move_base_sim.launch new file mode 100644 index 0000000..20e21ac --- /dev/null +++ b/test/full_coverage_path_planner/move_base_sim.launch @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch index ffa9c89..2c96519 100644 --- a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch +++ b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch @@ -14,15 +14,15 @@ - - + - - + - + From 3af0387f347368b57e0d0932d962aec95f6c3d34 Mon Sep 17 00:00:00 2001 From: ethanckim Date: Mon, 21 Jun 2021 15:54:38 -0600 Subject: [PATCH 02/12] move to verticies prototype --- maps/square.png | Bin 0 -> 3243 bytes maps/square.yaml | 6 ++ src/spiral_stc.cpp | 98 ++++++++++++++++++ .../move_base_sim.launch | 62 +++++++++++ .../test_full_coverage_path_planner.launch | 4 +- ...t_full_coverage_path_planner_plugin.launch | 2 +- 6 files changed, 169 insertions(+), 3 deletions(-) create mode 100644 maps/square.png create mode 100644 maps/square.yaml create mode 100644 test/full_coverage_path_planner/move_base_sim.launch diff --git a/maps/square.png b/maps/square.png new file mode 100644 index 0000000000000000000000000000000000000000..e747bdf69115969264858ff22eb9876465766317 GIT binary patch literal 3243 zcmeAS@N?(olHy`uVBq!ia0y~yVAKI&4mO}jWo=(60|QTyr;B4qMcmt)j(IVjLM(wF z|Fu73*ei9$VJ{c^DXsZ@54!TFnFwAu{~ieB>jfMb8W|ep4{}8mDKT;Uady~X!@?qP zLO@_mk5;=^wsaA<2tNDZ+R zVPs)BEe$k|hw40J2E%H)y1;d%VEQ^iucD@U-vCt zs>gV+YlrZf4vlm_poJpu@9s#ryK&1ERfPz_@+MKir)M-756*hDS6Y0H^|WcbfNA8B z-v*^cb1Z=hg7=g^ZYn)~DQJ2_g6kciqo)`zXJN_i`koiF;ax=3E1;V?tVbO^8q%Z5 zV>Ba%M+bgR4=@$1a?9gJYD@<);T3K0RVs+N!S1Y literal 0 HcmV?d00001 diff --git a/maps/square.yaml b/maps/square.yaml new file mode 100644 index 0000000..30b54e8 --- /dev/null +++ b/maps/square.yaml @@ -0,0 +1,6 @@ +image: square.png +resolution: 0.05 +origin: [-5, -5, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/spiral_stc.cpp b/src/spiral_stc.cpp index 33e9c33..24680ee 100644 --- a/src/spiral_stc.cpp +++ b/src/spiral_stc.cpp @@ -102,6 +102,79 @@ std::list SpiralSTC::spiral(std::vector > const& g return pathNodes; } + +// /** +// * Uses a wavefront algorithm to determine the nearest open verticies/corner on the grid. +// * The mountain pattern (bourstrophedon) should idealy begin from the corner of the grid. +// * @param grid internal map representation +// * @param corner_list Output map in which all cells except the corners are marked as visited (true). +// * @param start_x The x position of the corner to begin search +// * @param start_y The y position of the corner to begin search +// */ +// void findNearestOpenVerticies(std::vector> const& grid, +// std::vector>& corner_list, +// int start_x, +// int start_y) +// { +// std::queue> nodes; +// int bound_x = -1, bound_y = -1; +// bool processed[grid.size()][grid[0].size()] = {false}; // store node indicies which are already processed +// nodes.push({start_x, start_y}); // add first node to queue + +// while(!nodes.empty()) { + +// // get current node +// int x = std::get<0>(nodes.front()); +// int y = std::get<1>(nodes.front()); +// nodes.pop(); + +// // check node +// // To accomodate box case such as: +// // 0000000000000 +// // 0011111111100 +// // 0011000001100 +// // 0011000001100 +// // 0011111111100 +// // 0000000000000 +// if ((bound_x == -1 || bound_y == -1) && grid[x][y] == eNodeVisited) { +// bound_x == x; +// bound_y = y; +// } else { +// if((x < bound_x || y < bound_y) && grid[x][y] == eNodeVisited) { +// bound_x = dmin(x, bound_x); +// bound_y = dmin(y, bound_y); +// } +// if (grid[x][y] == eNodeOpen) { +// if (bound_x == -1 || bound_y == -1) { +// } +// corner_list[x][y] = eNodeOpen; +// return; +// } +// } + + +// // If node is not open, add unprocessed adjacent nodes to queue +// if (x+1 < grid.size() && processed[x+1][y] == false) { +// nodes.push({x+1, y}); +// processed[x+1][y] == true; +// } // East +// if (x-1 > 0 && processed[x-1][y] == false) { +// nodes.push({x-1, y}); +// processed[x-1][y] == true; +// } // West +// if (y+1 < grid[0].size() && processed[x][y+1] == false) { +// nodes.push({x, y+1}); +// processed[x][y+1] == true; +// } // North +// if (y-1 > 0 && processed[x][y-1] == false) { +// nodes.push({x, y-1}); +// processed[x][y-1] == true; +// } // South + +// } + +// } + std::list SpiralSTC::spiral_stc(std::vector > const& grid, Point_t& init, int &multiple_pass_counter, @@ -135,6 +208,30 @@ std::list SpiralSTC::spiral_stc(std::vector > const& #endif pathNodes = SpiralSTC::spiral(grid, pathNodes, visited); // First spiral fill + // std::list map_verticies; + // map_verticies.push_back({0, 0}); + // map_verticies.push_back({nCols, 0}); + // map_verticies.push_back({nCols, nRows}); + // map_verticies.push_back({0, nRows}); + // std::vector> corner_list(nCols, std::vector(nRows, eNodeVisited)); + // findNearestOpenVerticies(grid, corner_list, 0, 0); + // findNearestOpenVerticies(grid, corner_list, 0, nRows-1); + // findNearestOpenVerticies(grid, corner_list, nCols-1, 0); + // findNearestOpenVerticies(grid, corner_list, nCols-1, nRows-1); + + // for (int i = 0; i < nRows; ++i) + // { + // for (int j = 0; j < nCols; ++j) + // { + // std::cout << grid[i][j] << ' '; + // } + // std::cout << std::endl; + // } + + // if (a_star_to_open_space(grid, pathNodes.back(), 1, corner_list, map_verticies, pathNodes)) { + // ROS_INFO("A_star_to_open_space is resigning"); + // } + std::list goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints // Add points to full path std::list::iterator it; @@ -191,6 +288,7 @@ std::list SpiralSTC::spiral_stc(std::vector > const& #endif // Spiral fill from current position + // TODO: Convert to U-turn pattern pathNodes = spiral(grid, pathNodes, visited); #ifdef DEBUG_PLOT diff --git a/test/full_coverage_path_planner/move_base_sim.launch b/test/full_coverage_path_planner/move_base_sim.launch new file mode 100644 index 0000000..20e21ac --- /dev/null +++ b/test/full_coverage_path_planner/move_base_sim.launch @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch index ffa9c89..43033f6 100644 --- a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch +++ b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch @@ -1,14 +1,14 @@ - + - + diff --git a/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch b/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch index 97d0d9d..b113155 100644 --- a/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch +++ b/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch @@ -1,7 +1,7 @@ - + From 7cf3594bf3997b7b36a40941be8d3f1b87d3c66d Mon Sep 17 00:00:00 2001 From: ethanckim Date: Tue, 22 Jun 2021 12:38:25 -0600 Subject: [PATCH 03/12] mountain pattern algorithm implementation --- include/full_coverage_path_planner/common.h | 28 ++ src/common.cpp | 33 ++ src/spiral_stc.cpp | 331 ++++++++++---------- 3 files changed, 223 insertions(+), 169 deletions(-) diff --git a/include/full_coverage_path_planner/common.h b/include/full_coverage_path_planner/common.h index 7d46c21..2bfc9c0 100644 --- a/include/full_coverage_path_planner/common.h +++ b/include/full_coverage_path_planner/common.h @@ -61,6 +61,15 @@ enum eNodeVisited = true }; +enum +{ + point = 0, + east = 1, + west = 2, + north = 3, + south = 4 +}; + /** * Find the distance from poi to the closest point in goals * @param poi Starting point @@ -124,4 +133,23 @@ void printGrid(std::vector > const& grid); * @return a list of points that have the given value_to_search */ std::list map_2_goals(std::vector > const& grid, bool value_to_search); + +/** + * Prints pathNodes in the terminal + */ +void printPathNodes(std::list pathNodes); + +/** + * returns true only if the desired move is valid + */ +bool validMove(int x2, int y2, int nCols, int nRows, + std::vector > const& grid, + std::vector > const& visited); + +/** + * Adds node in (x2, y2) into the list of pathNodes, and marks the node as visited + */ +bool addNodeToList(int x2, int y2, gridNode_t prev, std::list pathNodes, + std::vector>& visited); + #endif // FULL_COVERAGE_PATH_PLANNER_COMMON_H diff --git a/src/common.cpp b/src/common.cpp index bc134a7..2c6fcce 100644 --- a/src/common.cpp +++ b/src/common.cpp @@ -309,3 +309,36 @@ std::list map_2_goals(std::vector > const& grid, bool } return goals; } + + +void printPathNodes(std::list pathNodes) +{ + for (gridNode_t node : pathNodes) { + std::cout << "(" << node.pos.x << ", " << node.pos.y << ")" << ": " << node.cost << " " << node.he << std::endl; + } + std:: cout << "--------------------------------------" << std::endl; + +} + +bool validMove(int x2, int y2, int nCols, int nRows, + std::vector> const& grid, + std::vector> const& visited) +{ + return (x2 >= 0 && x2 < nCols && y2 >= 0 && y2 < nRows) // path node is within the map + && (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen); // the path node is unvisited +} + +void addNodeToList(int x2, int y2, gridNode_t prev, std::list& pathNodes, + std::vector>& visited) { + Point_t new_point = { x2, y2 }; + gridNode_t new_node = + { + new_point, // Point: x,y + 0, // Cost + 0, // Heuristic + }; + prev = pathNodes.back(); + pathNodes.push_back(new_node); + visited[y2][x2] = eNodeVisited; // Close node +} + diff --git a/src/spiral_stc.cpp b/src/spiral_stc.cpp index 24680ee..a84830a 100644 --- a/src/spiral_stc.cpp +++ b/src/spiral_stc.cpp @@ -40,44 +40,50 @@ void SpiralSTC::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_r std::list SpiralSTC::spiral(std::vector > const& grid, std::list& init, std::vector >& visited) { - int dx, dy, dx_prev, x2, y2, i, nRows = grid.size(), nCols = grid[0].size(); - // Spiral filling of the open space + int dx, dy, x2, y2, i, nRows = grid.size(), nCols = grid[0].size(); + // Mountain pattern filling of the open space // Copy incoming list to 'end' - std::list pathNodes(init); - // Create iterator for gridNode_t list and let it point to the last element of end - std::list::iterator it = --(pathNodes.end()); - if (pathNodes.size() > 1) // if list is length 1, keep iterator at end - it--; // Let iterator point to second to last element + std::list pathNodes(init); + + // set initial direction towards longest side + int robot_dir = point; + int pattern_dir = point; + if (nRows >= nCols) { + if (pathNodes.back().pos.y < nRows / 2) { + robot_dir = north; + dy = 1; + } + else { + robot_dir = south; + dy = -1; + } + } else { + if (pathNodes.back().pos.x < nCols / 2) { + robot_dir = east; + dx = 1; + } + else { + robot_dir = west; + dx = -1; + } + } - gridNode_t prev = *(it); bool done = false; while (!done) { - if (it != pathNodes.begin()) - { - // turn ccw - dx = pathNodes.back().pos.x - prev.pos.x; - dy = pathNodes.back().pos.y - prev.pos.y; - dx_prev = dx; - dx = -dy; - dy = dx_prev; - } - else - { - // Initialize spiral direction towards y-axis - dx = 0; - dy = 1; - } - done = true; - - for (int i = 0; i < 4; ++i) - { + // 1. drive straight until hit wall + bool hitWall = false; + while(!hitWall) { x2 = pathNodes.back().pos.x + dx; y2 = pathNodes.back().pos.y + dy; - if (x2 >= 0 && x2 < nCols && y2 >= 0 && y2 < nRows) + if (!validMove(x2, y2, nCols, nRows, grid, visited)) { - if (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen) - { + hitWall = true; + x2 = pathNodes.back().pos.x; + y2 = pathNodes.back().pos.y; + break; + } + if (!hitWall) { Point_t new_point = { x2, y2 }; gridNode_t new_node = { @@ -85,96 +91,120 @@ std::list SpiralSTC::spiral(std::vector > const& g 0, // Cost 0, // Heuristic }; - prev = pathNodes.back(); pathNodes.push_back(new_node); - it = --(pathNodes.end()); visited[y2][x2] = eNodeVisited; // Close node - done = false; - break; + } + } + + // 2. check left and right after hitting wall, then change direction + if (robot_dir == north || robot_dir == south) + { + if (!validMove(x2 + 1, y2, nCols, nRows, grid, visited) + && !validMove(x2 - 1, y2, nCols, nRows, grid, visited)) { + // dead end, exit + done = true; + break; + } else if (!validMove(x2 + 1, y2, nCols, nRows, grid, visited)) { + // east is occupied, travel towards west + x2--; + pattern_dir = west; + } else if (!validMove(x2 - 1, y2, nCols, nRows, grid, visited)) { + // west is occupied, travel towards east + x2++; + pattern_dir = east; + } else { + // both sides are opened. If don't have a prefered turn direction, travel towards farthest edge + if (!(pattern_dir == east || pattern_dir == west)) { + if (x2 < nCols / 2) { // east + pattern_dir = east; + } else { // west + pattern_dir = west; + } + } + if (pattern_dir = east) { + x2++; + } else if (pattern_dir = west) { + x2--; } } - // try next direction cw - dx_prev = dx; - dx = dy; - dy = -dx_prev; + + // add Nodes to List + Point_t new_point = { x2, y2 }; + gridNode_t new_node = + { + new_point, // Point: x,y + 0, // Cost + 0, // Heuristic + }; + pathNodes.push_back(new_node); + visited[y2][x2] = eNodeVisited; // Close node + + // change direction 180 deg + if (robot_dir == north) { + robot_dir = south; + dy = -1; + } else if (robot_dir == south) { + robot_dir = north; + dy = 1; + } } + else if (robot_dir == east || robot_dir == west) + { + if (!validMove(x2, y2 + 1, nCols, nRows, grid, visited) + && !validMove(x2, y2 - 1, nCols, nRows, grid, visited)) { + // dead end, exit + done = true; + break; + } else if (!validMove(x2, y2 + 1, nCols, nRows, grid, visited)) { + // north is occupied, travel towards south + y2--; + pattern_dir = south; + } else if (!validMove(x2, y2 - 1, nCols, nRows, grid, visited)) { + // south is occupied, travel towards north + y2++; + pattern_dir = north; + } else { + // both sides are opened. If don't have a prefered turn direction, travel towards farthest edge + if (!(pattern_dir == north || pattern_dir == south)) { + if (y2 < nRows / 2) { // north + pattern_dir = north; + } else { // south + pattern_dir = south; + } + } + if (pattern_dir = north) { + y2++; + } else if (pattern_dir = south) { + y2--; + } + } + + // add Nodes to List + Point_t new_point = { x2, y2 }; + gridNode_t new_node = + { + new_point, // Point: x,y + 0, // Cost + 0, // Heuristic + }; + pathNodes.push_back(new_node); + visited[y2][x2] = eNodeVisited; // Close node + + // change direction 180 deg + if (robot_dir == east) { + robot_dir = west; + dx = -1; + } else if (robot_dir == west) { + robot_dir = east; + dx = 1; + } + } + // Log + printPathNodes(pathNodes); } return pathNodes; } - -// /** -// * Uses a wavefront algorithm to determine the nearest open verticies/corner on the grid. -// * The mountain pattern (bourstrophedon) should idealy begin from the corner of the grid. -// * @param grid internal map representation -// * @param corner_list Output map in which all cells except the corners are marked as visited (true). -// * @param start_x The x position of the corner to begin search -// * @param start_y The y position of the corner to begin search -// */ -// void findNearestOpenVerticies(std::vector> const& grid, -// std::vector>& corner_list, -// int start_x, -// int start_y) -// { -// std::queue> nodes; -// int bound_x = -1, bound_y = -1; -// bool processed[grid.size()][grid[0].size()] = {false}; // store node indicies which are already processed -// nodes.push({start_x, start_y}); // add first node to queue - -// while(!nodes.empty()) { - -// // get current node -// int x = std::get<0>(nodes.front()); -// int y = std::get<1>(nodes.front()); -// nodes.pop(); - -// // check node -// // To accomodate box case such as: -// // 0000000000000 -// // 0011111111100 -// // 0011000001100 -// // 0011000001100 -// // 0011111111100 -// // 0000000000000 -// if ((bound_x == -1 || bound_y == -1) && grid[x][y] == eNodeVisited) { -// bound_x == x; -// bound_y = y; -// } else { -// if((x < bound_x || y < bound_y) && grid[x][y] == eNodeVisited) { -// bound_x = dmin(x, bound_x); -// bound_y = dmin(y, bound_y); -// } -// if (grid[x][y] == eNodeOpen) { -// if (bound_x == -1 || bound_y == -1) { -// } -// corner_list[x][y] = eNodeOpen; -// return; -// } -// } - - -// // If node is not open, add unprocessed adjacent nodes to queue -// if (x+1 < grid.size() && processed[x+1][y] == false) { -// nodes.push({x+1, y}); -// processed[x+1][y] == true; -// } // East -// if (x-1 > 0 && processed[x-1][y] == false) { -// nodes.push({x-1, y}); -// processed[x-1][y] == true; -// } // West -// if (y+1 < grid[0].size() && processed[x][y+1] == false) { -// nodes.push({x, y+1}); -// processed[x][y+1] == true; -// } // North -// if (y-1 > 0 && processed[x][y-1] == false) { -// nodes.push({x, y-1}); -// processed[x][y-1] == true; -// } // South - -// } - -// } - std::list SpiralSTC::spiral_stc(std::vector > const& grid, Point_t& init, int &multiple_pass_counter, @@ -190,6 +220,7 @@ std::list SpiralSTC::spiral_stc(std::vector > const& x = init.x; y = init.y; + // add initial point to pathNodes Point_t new_point = { x, y }; gridNode_t new_node = { @@ -202,55 +233,34 @@ std::list SpiralSTC::spiral_stc(std::vector > const& pathNodes.push_back(new_node); visited[y][x] = eNodeVisited; + std::list goals = map_2_goals(visited, eNodeOpen); // Retrieve all goalpoints (Cells not visited) + std::list::iterator it; + #ifdef DEBUG_PLOT ROS_INFO("Grid before walking is: "); printGrid(grid, visited, fullPath); #endif - pathNodes = SpiralSTC::spiral(grid, pathNodes, visited); // First spiral fill - // std::list map_verticies; - // map_verticies.push_back({0, 0}); - // map_verticies.push_back({nCols, 0}); - // map_verticies.push_back({nCols, nRows}); - // map_verticies.push_back({0, nRows}); - // std::vector> corner_list(nCols, std::vector(nRows, eNodeVisited)); - // findNearestOpenVerticies(grid, corner_list, 0, 0); - // findNearestOpenVerticies(grid, corner_list, 0, nRows-1); - // findNearestOpenVerticies(grid, corner_list, nCols-1, 0); - // findNearestOpenVerticies(grid, corner_list, nCols-1, nRows-1); - - // for (int i = 0; i < nRows; ++i) - // { - // for (int j = 0; j < nCols; ++j) - // { - // std::cout << grid[i][j] << ' '; - // } - // std::cout << std::endl; - // } - - // if (a_star_to_open_space(grid, pathNodes.back(), 1, corner_list, map_verticies, pathNodes)) { - // ROS_INFO("A_star_to_open_space is resigning"); - // } - - std::list goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints - // Add points to full path - std::list::iterator it; - for (it = pathNodes.begin(); it != pathNodes.end(); ++it) + while (goals.size() != 0) { - Point_t newPoint = { it->pos.x, it->pos.y }; - visited_counter++; - fullPath.push_back(newPoint); - } - // Remove all elements from pathNodes list except last element - pathNodes.erase(pathNodes.begin(), --(pathNodes.end())); + // Spiral fill from current position + // TODO: Convert to U-turn pattern + pathNodes = spiral(grid, pathNodes, visited); #ifdef DEBUG_PLOT - ROS_INFO("Current grid after first spiral is"); - printGrid(grid, visited, fullPath); - ROS_INFO("There are %d goals remaining", goals.size()); + ROS_INFO("Visited grid updated after spiral:"); + printGrid(grid, visited, pathNodes, SpiralStart, pathNodes.back()); #endif - while (goals.size() != 0) - { + + goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints + + for (it = pathNodes.begin(); it != pathNodes.end(); ++it) + { + Point_t newPoint = { it->pos.x, it->pos.y }; + visited_counter++; + fullPath.push_back(newPoint); + } + // Remove all elements from pathNodes list except last element. // The last point is the starting point for a new search and A* extends the path from there on pathNodes.erase(pathNodes.begin(), --(pathNodes.end())); @@ -286,24 +296,7 @@ std::list SpiralSTC::spiral_stc(std::vector > const& gridNode_t SpiralStart = pathNodes.back(); printGrid(grid, visited, pathNodes, pathNodes.front(), pathNodes.back()); #endif - - // Spiral fill from current position - // TODO: Convert to U-turn pattern - pathNodes = spiral(grid, pathNodes, visited); - -#ifdef DEBUG_PLOT - ROS_INFO("Visited grid updated after spiral:"); - printGrid(grid, visited, pathNodes, SpiralStart, pathNodes.back()); -#endif - - goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints - - for (it = pathNodes.begin(); it != pathNodes.end(); ++it) - { - Point_t newPoint = { it->pos.x, it->pos.y }; - visited_counter++; - fullPath.push_back(newPoint); - } + } return fullPath; From 4f38a1f95a782ad36faec3689934a4dbdcbbef7e Mon Sep 17 00:00:00 2001 From: ethanckim Date: Tue, 22 Jun 2021 13:04:05 -0600 Subject: [PATCH 04/12] add gitignore --- .gitignore | 71 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 71 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..4cb63b5 --- /dev/null +++ b/.gitignore @@ -0,0 +1,71 @@ +# Created by https://www.toptal.com/developers/gitignore/api/cmake,visualstudiocode,c++ +# Edit at https://www.toptal.com/developers/gitignore?templates=cmake,visualstudiocode,c++ + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### CMake ### +CMakeLists.txt.user +CMakeCache.txt +CMakeFiles +CMakeScripts +Testing +Makefile +cmake_install.cmake +install_manifest.txt +compile_commands.json +CTestTestfile.cmake +_deps + +### CMake Patch ### +# External projects +*-prefix/ + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +*.code-workspace + +# Local History for Visual Studio Code +.history/ + +### VisualStudioCode Patch ### +# Ignore all local history of files +.history +.ionide + +# End of https://www.toptal.com/developers/gitignore/api/cmake,visualstudiocode,c++ \ No newline at end of file From 60649b18c50070c4b2ff226232b6998adf68d199 Mon Sep 17 00:00:00 2001 From: ethanckim Date: Tue, 22 Jun 2021 13:07:56 -0600 Subject: [PATCH 05/12] update gitignore --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 4cb63b5..0cbc99c 100644 --- a/.gitignore +++ b/.gitignore @@ -68,4 +68,5 @@ _deps .history .ionide -# End of https://www.toptal.com/developers/gitignore/api/cmake,visualstudiocode,c++ \ No newline at end of file +# End of https://www.toptal.com/developers/gitignore/api/cmake,visualstudiocode,c++ +.vscode/settings.json From a8bc76ba68518d2d2622021b555fc0e3b6944489 Mon Sep 17 00:00:00 2001 From: ethanckim Date: Tue, 22 Jun 2021 13:17:42 -0600 Subject: [PATCH 06/12] restored visualization environment --- test/full_coverage_path_planner/fcpp.rviz | 94 +++++++++---------- .../test_full_coverage_path_planner.launch | 10 +- 2 files changed, 52 insertions(+), 52 deletions(-) diff --git a/test/full_coverage_path_planner/fcpp.rviz b/test/full_coverage_path_planner/fcpp.rviz index 7fd802a..8c1ded1 100644 --- a/test/full_coverage_path_planner/fcpp.rviz +++ b/test/full_coverage_path_planner/fcpp.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 548 + Tree Height: 554 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -62,7 +62,6 @@ Visualization Manager: Value: true odom: Value: true - Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true @@ -87,14 +86,6 @@ Visualization Manager: Unreliable: false Use Timestamp: false Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /path_coverage_marker - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path @@ -112,35 +103,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: Arrows - Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.019999999552965164 Shaft Length: 0.05000000074505806 - Topic: /move_base/TrajectoryPlannerROS/global_plan - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /move_base/TrajectoryPlannerROS/local_plan + Topic: /move_base/SpiralSTC/plan Unreliable: false Value: true - Angle Tolerance: 0.10000000149011612 @@ -164,7 +130,6 @@ Visualization Manager: Keep: 10000 Name: Odometry Position Tolerance: 0.10000000149011612 - Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 @@ -178,16 +143,50 @@ Visualization Manager: Topic: /odom Unreliable: false Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /visualization_marker + Name: Tracking_pid local planner + Namespaces: + axle point: true + control point: true + goal point: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /move_base_flex/tracking_pid/interpolator + Name: Interpolator (mbf) + Namespaces: + {} + Queue Size: 100 + Value: true - Alpha: 0.30000001192092896 Class: rviz/Map Color Scheme: map Draw Behind: false - Enabled: false + Enabled: true Name: Coverage progress map Topic: /coverage_grid Unreliable: false Use Timestamp: false - Value: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /move_base/TrackingPidLocalPlanner/interpolator + Name: Interpolator (mb) + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Footprint + Topic: /move_base_flex/global_costmap/footprint + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -225,18 +224,19 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 107.97452545166016 + Scale: 97.19247436523438 Target Frame: - X: -0.3638369143009186 - Y: 0.05843987315893173 + Value: TopDownOrtho (rviz) + X: -0.14492475986480713 + Y: 1.0135213136672974 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 1023 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035afc0200000008fb0000001200530065006c0065006300740069006f006e010000003d000000a50000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000e8000002af000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003830000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000361fc0200000008fb0000001200530065006c0065006300740069006f006e010000003d000000a60000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000e9000002b5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f0000003efc0100000002fb0000000800540069006d006501000000000000039f000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000022f0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -245,6 +245,6 @@ Window Geometry: collapsed: true Views: collapsed: true - Width: 1267 - X: 72 - Y: 27 + Width: 927 + X: 67 + Y: 27 \ No newline at end of file diff --git a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch index e7eaeaa..43033f6 100644 --- a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch +++ b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch @@ -14,15 +14,15 @@ - + + - + + - + From 174ea81cc4df1c300bf40faf2d7a9c0b098bc3e6 Mon Sep 17 00:00:00 2001 From: ethanckim Date: Fri, 23 Jul 2021 08:55:07 -0600 Subject: [PATCH 07/12] Set initial heading based on obstacles, not map coordinates --- src/spiral_stc.cpp | 75 ++++++++++++++++++++++++++++++++++------------ 1 file changed, 56 insertions(+), 19 deletions(-) diff --git a/src/spiral_stc.cpp b/src/spiral_stc.cpp index a84830a..b4aad82 100644 --- a/src/spiral_stc.cpp +++ b/src/spiral_stc.cpp @@ -45,27 +45,64 @@ std::list SpiralSTC::spiral(std::vector > const& g // Copy incoming list to 'end' std::list pathNodes(init); - // set initial direction towards longest side - int robot_dir = point; - int pattern_dir = point; - if (nRows >= nCols) { - if (pathNodes.back().pos.y < nRows / 2) { - robot_dir = north; - dy = 1; - } - else { - robot_dir = south; - dy = -1; - } - } else { - if (pathNodes.back().pos.x < nCols / 2) { - robot_dir = east; - dx = 1; + // this array stores how far the robot can travel in a straight line for each direction + int free_space_in_dir[5] = {0}; + // for each direction + for (int i = 1; i < 5; i++) { + // start from starting pos + x2 = pathNodes.back().pos.x; + y2 = pathNodes.back().pos.y; + // loop until hits wall + while (validMove(x2, y2, nCols, nRows, grid, visited)) { + switch (i) { + case east: + x2++; + break; + case west: + x2--; + break; + case north: + y2++; + break; + case south: + y2--; + break; + default: + break; + } + free_space_in_dir[i]++; } - else { - robot_dir = west; + } + // set initial direction towards direction with most travel possible + int robot_dir = 0; + int pattern_dir = point; + int indexValue = 0; + for (int i = 1; i <= 4; i++) { + if (free_space_in_dir[i] > indexValue) { + robot_dir = i; + indexValue = free_space_in_dir[i]; + } + } + // set dx and dy based on robot_dir + switch(robot_dir) { + case east: // 1 + dx = +1; + dy = 0; + break; + case west: // 2 dx = -1; - } + dy = 0; + break; + case north: // 3 + dx = 0; + dy = +1; + break; + case south: // 4 + dx = 0; + dy = -1; + break; + default: + break; } bool done = false; From 6fed446ff78750542b4be7ec1914fc2c01f35845 Mon Sep 17 00:00:00 2001 From: ethanckim Date: Tue, 3 Aug 2021 15:40:09 -0600 Subject: [PATCH 08/12] Fix infinite loop error and improved heading decision of path planning algorithm --- include/full_coverage_path_planner/common.h | 18 +- .../full_coverage_path_planner.h | 19 ++ src/common.cpp | 48 ++++- src/full_coverage_path_planner.cpp | 66 +++++++ src/spiral_stc.cpp | 179 +++++++----------- 5 files changed, 213 insertions(+), 117 deletions(-) diff --git a/include/full_coverage_path_planner/common.h b/include/full_coverage_path_planner/common.h index 2bfc9c0..376dc9f 100644 --- a/include/full_coverage_path_planner/common.h +++ b/include/full_coverage_path_planner/common.h @@ -136,11 +136,18 @@ std::list map_2_goals(std::vector > const& grid, bool /** * Prints pathNodes in the terminal + * @param pathNodes pathNodes to be printed in the terminal */ void printPathNodes(std::list pathNodes); /** * returns true only if the desired move is valid + * @param x2 x coordinate of desired position + * @param y2 y coordinate of desired position + * @param nCols + * @param nRows + * @param grid internal map representation - 2D grid of bools. true == occupied/blocked/obstacle + * @param visited 2D grid of bools. true == visited */ bool validMove(int x2, int y2, int nCols, int nRows, std::vector > const& grid, @@ -149,7 +156,16 @@ bool validMove(int x2, int y2, int nCols, int nRows, /** * Adds node in (x2, y2) into the list of pathNodes, and marks the node as visited */ -bool addNodeToList(int x2, int y2, gridNode_t prev, std::list pathNodes, +void addNodeToList(int x2, int y2, std::list& pathNodes, std::vector>& visited); +/** + * Returns direction in which most free space is visible when given the robot's current location + * @param ignoreDir ignores a single direction specified. Pass 0 (point) to consider all four directions. + */ +int dirWithMostSpace(int x2, int y2, int nCols, int nRows, + std::vector > const& grid, + std::vector > const& visited, + int ignoreDir); + #endif // FULL_COVERAGE_PATH_PLANNER_COMMON_H diff --git a/include/full_coverage_path_planner/full_coverage_path_planner.h b/include/full_coverage_path_planner/full_coverage_path_planner.h index 763d098..026f862 100644 --- a/include/full_coverage_path_planner/full_coverage_path_planner.h +++ b/include/full_coverage_path_planner/full_coverage_path_planner.h @@ -85,6 +85,22 @@ class FullCoveragePathPlanner void parsePointlist2Plan(const geometry_msgs::PoseStamped& start, std::list const& goalpoints, std::vector& plan); + /** + * Convert ROS Occupancy grid to internal grid representation, given the size of a single tile + * @param costmap_grid_ Costmap representation. Cells higher that 65 are considered occupied + * @param grid internal map representation + * @param tileSize size (in meters) of a cell. This can be the robot's size + * @param realStart Start position of the robot (in meters) + * @param scaledStart Start position of the robot on the grid + * @return success + */ + bool parseCostmap(costmap_2d::Costmap2D* costmap_grid_, + std::vector >& grid, + float robotRadius, + float toolRadius, + geometry_msgs::PoseStamped const& realStart, + Point_t& scaledStart); + /** * Convert ROS Occupancy grid to internal grid representation, given the size of a single tile * @param cpp_grid_ ROS occupancy grid representation. Cells higher that 65 are considered occupied @@ -103,6 +119,9 @@ class FullCoveragePathPlanner ros::Publisher plan_pub_; ros::ServiceClient cpp_grid_client_; nav_msgs::OccupancyGrid cpp_grid_; + // Using costmap instead of Occupancy Grid from map server as the costmap updates periodically. + costmap_2d::Costmap2DROS* costmap_ros_; + costmap_2d::Costmap2D* costmap_; float robot_radius_; float tool_radius_; float plan_resolution_; diff --git a/src/common.cpp b/src/common.cpp index 2c6fcce..74077f2 100644 --- a/src/common.cpp +++ b/src/common.cpp @@ -328,7 +328,7 @@ bool validMove(int x2, int y2, int nCols, int nRows, && (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen); // the path node is unvisited } -void addNodeToList(int x2, int y2, gridNode_t prev, std::list& pathNodes, +void addNodeToList(int x2, int y2, std::list& pathNodes, std::vector>& visited) { Point_t new_point = { x2, y2 }; gridNode_t new_node = @@ -337,8 +337,52 @@ void addNodeToList(int x2, int y2, gridNode_t prev, std::list& pathN 0, // Cost 0, // Heuristic }; - prev = pathNodes.back(); pathNodes.push_back(new_node); visited[y2][x2] = eNodeVisited; // Close node + return; } +int dirWithMostSpace(int x_init, int y_init, int nCols, int nRows, + std::vector > const& grid, + std::vector > const& visited, + int ignoreDir) { + // this array stores how far the robot can travel in a straight line for each direction + int free_space_in_dir[5] = {0}; + // for each direction + for (int i = 1; i < 5; i++) { + // start from starting pos + int x2 = x_init; + int y2 = y_init; + do { // loop until hit wall + switch (i) { + case east: + x2++; + break; + case west: + x2--; + break; + case north: + y2++; + break; + case south: + y2--; + break; + default: + break; + } + free_space_in_dir[i]++; + } while (validMove(x2, y2, nCols, nRows, grid, visited)); + } + + // set initial direction towards direction with most travel possible + int robot_dir = 0; + int indexValue = 0; + for (int i = 1; i <= 4; i++) { + // std::cout << "free space in " << i << ": " << free_space_in_dir[i] << std::endl; + if (free_space_in_dir[i] > indexValue && i != ignoreDir) { + robot_dir = i; + indexValue = free_space_in_dir[i]; + } + } + return robot_dir; +} diff --git a/src/full_coverage_path_planner.cpp b/src/full_coverage_path_planner.cpp index 0048682..21500fe 100644 --- a/src/full_coverage_path_planner.cpp +++ b/src/full_coverage_path_planner.cpp @@ -185,6 +185,67 @@ void FullCoveragePathPlanner::parsePointlist2Plan(const geometry_msgs::PoseStamp ROS_INFO("Plan ready containing %lu goals!", plan.size()); } +bool FullCoveragePathPlanner::parseCostmap(costmap_2d::Costmap2D* costmap_grid_, + std::vector >& grid, + float robotRadius, + float toolRadius, + geometry_msgs::PoseStamped const& realStart, + Point_t& scaledStart) +{ + int ix, iy, nodeRow, nodeCol; + uint32_t nodeSize = dmax(floor(toolRadius / costmap_grid_->getResolution()), 1); // Size of node in pixels/units + uint32_t nRows = costmap_grid_->getSizeInCellsY(), nCols = costmap_grid_->getSizeInCellsX(); + ROS_INFO("nRows: %u nCols: %u nodeSize: %d", nRows, nCols, nodeSize); + + if (nRows == 0 || nCols == 0) + { + return false; + } + + // Save map origin and scaling + tile_size_ = nodeSize * costmap_grid_->getResolution(); // Size of a tile in meters + grid_origin_.x = costmap_grid_->getOriginX(); // x-origin in meters + grid_origin_.y = costmap_grid_->getOriginY(); // y-origin in meters + ROS_INFO("costmap resolution: %g", costmap_grid_->getResolution()); + ROS_INFO("tile size: %g", tile_size_); + ROS_INFO("grid origin: (%g, %g)", grid_origin_.x, grid_origin_.y); + + // Scale starting point + scaledStart.x = static_cast(clamp((realStart.pose.position.x - grid_origin_.x) / tile_size_, 0.0, + floor(nCols / tile_size_))); + scaledStart.y = static_cast(clamp((realStart.pose.position.y - grid_origin_.y) / tile_size_, 0.0, + floor(nRows / tile_size_))); + ROS_INFO("real start: (%g, %g)", realStart.pose.position.x, realStart.pose.position.y); + ROS_INFO("scaled start: (%u, %u)", scaledStart.x, scaledStart.y); + + // Scale grid + for (iy = 0; iy < nRows; iy = iy + nodeSize) + { + std::vector gridRow; + for (ix = 0; ix < nCols; ix = ix + nodeSize) + { + bool nodeOccupied = false; + for (nodeRow = 0; (nodeRow < nodeSize) && ((iy + nodeRow) < nRows) && (nodeOccupied == false); ++nodeRow) + { + for (nodeCol = 0; (nodeCol < nodeSize) && ((ix + nodeCol) < nCols); ++nodeCol) + { + double mx = grid_origin_.x + ix + nodeCol; + double my = grid_origin_.y + iy + nodeRow; + if (costmap_grid_->getCost(mx, my) > costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + { + nodeOccupied = true; + ROS_INFO("(%f, %f) marked occupied", mx, my); + break; + } + } + } + gridRow.push_back(nodeOccupied); + } + grid.push_back(gridRow); + } + return true; +} + bool FullCoveragePathPlanner::parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_, std::vector >& grid, float robotRadius, @@ -207,12 +268,17 @@ bool FullCoveragePathPlanner::parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_ tile_size_ = nodeSize * cpp_grid_.info.resolution; // Size of a tile in meters grid_origin_.x = cpp_grid_.info.origin.position.x; // x-origin in meters grid_origin_.y = cpp_grid_.info.origin.position.y; // y-origin in meters + ROS_INFO("costmap resolution: %g", cpp_grid_.info.resolution); + ROS_INFO("tile size: %g", tile_size_); + ROS_INFO("grid origin: (%g, %g)", grid_origin_.x, grid_origin_.y); // Scale starting point scaledStart.x = static_cast(clamp((realStart.pose.position.x - grid_origin_.x) / tile_size_, 0.0, floor(cpp_grid_.info.width / tile_size_))); scaledStart.y = static_cast(clamp((realStart.pose.position.y - grid_origin_.y) / tile_size_, 0.0, floor(cpp_grid_.info.height / tile_size_))); + ROS_INFO("real start: (%g, %g)", realStart.pose.position.x, realStart.pose.position.y); + ROS_INFO("scaled start: (%u, %u)", scaledStart.x, scaledStart.y); // Scale grid for (iy = 0; iy < nRows; iy = iy + nodeSize) diff --git a/src/spiral_stc.cpp b/src/spiral_stc.cpp index b4aad82..d64a71f 100644 --- a/src/spiral_stc.cpp +++ b/src/spiral_stc.cpp @@ -13,6 +13,8 @@ // register this planner as a BaseGlobalPlanner plugin PLUGINLIB_EXPORT_CLASS(full_coverage_path_planner::SpiralSTC, nav_core::BaseGlobalPlanner) +int pattern_dir_ = point; + namespace full_coverage_path_planner { void SpiralSTC::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) @@ -26,6 +28,9 @@ void SpiralSTC::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_r plan_pub_ = private_named_nh.advertise("plan", 1); // Try to request the cpp-grid from the cpp_grid map_server cpp_grid_client_ = nh.serviceClient("static_map"); + // Get the cost map: + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros->getCostmap(); // Define robot radius (radius) parameter float robot_radius_default = 0.5f; @@ -43,46 +48,13 @@ std::list SpiralSTC::spiral(std::vector > const& g int dx, dy, x2, y2, i, nRows = grid.size(), nCols = grid[0].size(); // Mountain pattern filling of the open space // Copy incoming list to 'end' - std::list pathNodes(init); - - // this array stores how far the robot can travel in a straight line for each direction - int free_space_in_dir[5] = {0}; - // for each direction - for (int i = 1; i < 5; i++) { - // start from starting pos - x2 = pathNodes.back().pos.x; - y2 = pathNodes.back().pos.y; - // loop until hits wall - while (validMove(x2, y2, nCols, nRows, grid, visited)) { - switch (i) { - case east: - x2++; - break; - case west: - x2--; - break; - case north: - y2++; - break; - case south: - y2--; - break; - default: - break; - } - free_space_in_dir[i]++; - } - } - // set initial direction towards direction with most travel possible - int robot_dir = 0; - int pattern_dir = point; - int indexValue = 0; - for (int i = 1; i <= 4; i++) { - if (free_space_in_dir[i] > indexValue) { - robot_dir = i; - indexValue = free_space_in_dir[i]; - } - } + std::list pathNodes(init); + + // Set starting pos + x2 = pathNodes.back().pos.x; + y2 = pathNodes.back().pos.y; + // set initial direction based on space visible from initial pos + int robot_dir = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, point); // set dx and dy based on robot_dir switch(robot_dir) { case east: // 1 @@ -102,18 +74,22 @@ std::list SpiralSTC::spiral(std::vector > const& g dy = -1; break; default: + ROS_ERROR("Full Coverage Path Planner: NO INITIAL ROBOT DIRECTION CALCULATED. This is a logic error that must be fixed by editing spiral_stc.cpp. Will travel east for now."); + robot_dir = east; + dx = +1; + dy = 0; break; } bool done = false; while (!done) { - // 1. drive straight until hit wall + // 1. drive straight until not a valid move (hit occupied cell or at end of map) bool hitWall = false; while(!hitWall) { - x2 = pathNodes.back().pos.x + dx; - y2 = pathNodes.back().pos.y + dy; - if (!validMove(x2, y2, nCols, nRows, grid, visited)) + x2 += dx; + y2 += dy; + if (!validMove(x2, y2, nCols, nRows, grid, visited)) { hitWall = true; x2 = pathNodes.back().pos.x; @@ -121,15 +97,7 @@ std::list SpiralSTC::spiral(std::vector > const& g break; } if (!hitWall) { - Point_t new_point = { x2, y2 }; - gridNode_t new_node = - { - new_point, // Point: x,y - 0, // Cost - 0, // Heuristic - }; - pathNodes.push_back(new_node); - visited[y2][x2] = eNodeVisited; // Close node + addNodeToList(x2, y2, pathNodes, visited); } } @@ -144,37 +112,30 @@ std::list SpiralSTC::spiral(std::vector > const& g } else if (!validMove(x2 + 1, y2, nCols, nRows, grid, visited)) { // east is occupied, travel towards west x2--; - pattern_dir = west; + pattern_dir_ = west; } else if (!validMove(x2 - 1, y2, nCols, nRows, grid, visited)) { // west is occupied, travel towards east x2++; - pattern_dir = east; + pattern_dir_ = east; } else { - // both sides are opened. If don't have a prefered turn direction, travel towards farthest edge - if (!(pattern_dir == east || pattern_dir == west)) { - if (x2 < nCols / 2) { // east - pattern_dir = east; - } else { // west - pattern_dir = west; + // both sides are opened. If don't have a prefered turn direction, travel towards most open direction + if (!(pattern_dir_ == east || pattern_dir_ == west)) { + if (validMove(x2, y2 + 1, nCols, nRows, grid, visited)) { + pattern_dir_ = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, north); + } else { + pattern_dir_ = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, south); } + ROS_INFO("rotation dir with most space successful"); } - if (pattern_dir = east) { + if (pattern_dir_ = east) { x2++; - } else if (pattern_dir = west) { + } else if (pattern_dir_ = west) { x2--; } } - // add Nodes to List - Point_t new_point = { x2, y2 }; - gridNode_t new_node = - { - new_point, // Point: x,y - 0, // Cost - 0, // Heuristic - }; - pathNodes.push_back(new_node); - visited[y2][x2] = eNodeVisited; // Close node + // add Node to List + addNodeToList(x2, y2, pathNodes, visited); // change direction 180 deg if (robot_dir == north) { @@ -195,37 +156,29 @@ std::list SpiralSTC::spiral(std::vector > const& g } else if (!validMove(x2, y2 + 1, nCols, nRows, grid, visited)) { // north is occupied, travel towards south y2--; - pattern_dir = south; + pattern_dir_ = south; } else if (!validMove(x2, y2 - 1, nCols, nRows, grid, visited)) { // south is occupied, travel towards north y2++; - pattern_dir = north; + pattern_dir_ = north; } else { // both sides are opened. If don't have a prefered turn direction, travel towards farthest edge - if (!(pattern_dir == north || pattern_dir == south)) { - if (y2 < nRows / 2) { // north - pattern_dir = north; - } else { // south - pattern_dir = south; + if (!(pattern_dir_ == north || pattern_dir_ == south)) { + if (validMove(x2 + 1, y2, nCols, nRows, grid, visited)) { + pattern_dir_ = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, east); + } else { + pattern_dir_ = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, west); } } - if (pattern_dir = north) { + if (pattern_dir_ = north) { y2++; - } else if (pattern_dir = south) { + } else if (pattern_dir_ = south) { y2--; } } - // add Nodes to List - Point_t new_point = { x2, y2 }; - gridNode_t new_node = - { - new_point, // Point: x,y - 0, // Cost - 0, // Heuristic - }; - pathNodes.push_back(new_node); - visited[y2][x2] = eNodeVisited; // Close node + // add Node to List + addNodeToList(x2, y2, pathNodes, visited); // change direction 180 deg if (robot_dir == east) { @@ -236,9 +189,9 @@ std::list SpiralSTC::spiral(std::vector > const& g dx = 1; } } - // Log - printPathNodes(pathNodes); } + // Log + printPathNodes(pathNodes); return pathNodes; } @@ -248,29 +201,23 @@ std::list SpiralSTC::spiral_stc(std::vector > const& int &visited_counter) { int x, y, nRows = grid.size(), nCols = grid[0].size(); + pattern_dir_ = point; // Initial node is initially set as visited so it does not count multiple_pass_counter = 0; visited_counter = 0; - std::vector > visited; + std::vector> visited; visited = grid; // Copy grid matrix x = init.x; y = init.y; // add initial point to pathNodes - Point_t new_point = { x, y }; - gridNode_t new_node = - { - new_point, // Point: x,y - 0, // Cost - 0, // Heuristic - }; std::list pathNodes; std::list fullPath; - pathNodes.push_back(new_node); - visited[y][x] = eNodeVisited; + addNodeToList(x, y, pathNodes, visited); std::list goals = map_2_goals(visited, eNodeOpen); // Retrieve all goalpoints (Cells not visited) + std::cout << "Goals Left: " << goals.size() << std::endl; std::list::iterator it; #ifdef DEBUG_PLOT @@ -280,17 +227,13 @@ std::list SpiralSTC::spiral_stc(std::vector > const& while (goals.size() != 0) { - // Spiral fill from current position - // TODO: Convert to U-turn pattern + // boustrophedon pattern from current position pathNodes = spiral(grid, pathNodes, visited); - #ifdef DEBUG_PLOT ROS_INFO("Visited grid updated after spiral:"); printGrid(grid, visited, pathNodes, SpiralStart, pathNodes.back()); #endif - goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints - for (it = pathNodes.begin(); it != pathNodes.end(); ++it) { Point_t newPoint = { it->pos.x, it->pos.y }; @@ -298,6 +241,8 @@ std::list SpiralSTC::spiral_stc(std::vector > const& fullPath.push_back(newPoint); } + goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints + // Remove all elements from pathNodes list except last element. // The last point is the starting point for a new search and A* extends the path from there on pathNodes.erase(pathNodes.begin(), --(pathNodes.end())); @@ -308,9 +253,7 @@ std::list SpiralSTC::spiral_stc(std::vector > const& bool resign = a_star_to_open_space(grid, pathNodes.back(), 1, visited, goals, pathNodes); if (resign) { -#ifdef DEBUG_PLOT - ROS_INFO("A_star_to_open_space is resigning", goals.size()); -#endif + ROS_WARN("A_star_to_open_space is resigning! This may be due to the open cells outside of the obstacle boundary. Goals Left: %u", goals.size()); break; } @@ -333,7 +276,7 @@ std::list SpiralSTC::spiral_stc(std::vector > const& gridNode_t SpiralStart = pathNodes.back(); printGrid(grid, visited, pathNodes, pathNodes.front(), pathNodes.back()); #endif - + } return fullPath; @@ -352,24 +295,32 @@ bool SpiralSTC::makePlan(const geometry_msgs::PoseStamped& start, const geometry ROS_INFO("Initialized!"); } + //clear the plan, just in case + plan.clear(); + costmap_ = costmap_ros_->getCostmap(); + clock_t begin = clock(); Point_t startPoint; /********************** Get grid from server **********************/ std::vector > grid; nav_msgs::GetMap grid_req_srv; - ROS_INFO("Requesting grid!!"); + ROS_INFO("Requesting grid..."); if (!cpp_grid_client_.call(grid_req_srv)) { ROS_ERROR("Could not retrieve grid from map_server"); return false; } + ROS_INFO("grid recieved!!"); +//parseCostmap(costmap_, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint) + ROS_INFO("Parsing grid to internal representation..."); if (!parseGrid(grid_req_srv.response.map, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint)) { ROS_ERROR("Could not parse retrieved grid"); return false; } + ROS_INFO("grid parsed!!"); #ifdef DEBUG_PLOT ROS_INFO("Start grid is:"); From b40196927a77da01c19518698d259911666aaffb Mon Sep 17 00:00:00 2001 From: ethanckim Date: Tue, 3 Aug 2021 16:07:10 -0600 Subject: [PATCH 09/12] Use costmap instead of static map --- src/full_coverage_path_planner.cpp | 6 +++--- src/spiral_stc.cpp | 5 ++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/full_coverage_path_planner.cpp b/src/full_coverage_path_planner.cpp index 21500fe..1449e58 100644 --- a/src/full_coverage_path_planner.cpp +++ b/src/full_coverage_path_planner.cpp @@ -229,12 +229,12 @@ bool FullCoveragePathPlanner::parseCostmap(costmap_2d::Costmap2D* costmap_grid_, { for (nodeCol = 0; (nodeCol < nodeSize) && ((ix + nodeCol) < nCols); ++nodeCol) { - double mx = grid_origin_.x + ix + nodeCol; - double my = grid_origin_.y + iy + nodeRow; + double mx = ix + nodeCol; + double my = iy + nodeRow; if (costmap_grid_->getCost(mx, my) > costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { nodeOccupied = true; - ROS_INFO("(%f, %f) marked occupied", mx, my); + // ROS_INFO("(%f, %f) marked occupied", mx, my); break; } } diff --git a/src/spiral_stc.cpp b/src/spiral_stc.cpp index d64a71f..f3bd4cc 100644 --- a/src/spiral_stc.cpp +++ b/src/spiral_stc.cpp @@ -191,7 +191,7 @@ std::list SpiralSTC::spiral(std::vector > const& g } } // Log - printPathNodes(pathNodes); + // printPathNodes(pathNodes); return pathNodes; } @@ -313,9 +313,8 @@ bool SpiralSTC::makePlan(const geometry_msgs::PoseStamped& start, const geometry } ROS_INFO("grid recieved!!"); -//parseCostmap(costmap_, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint) ROS_INFO("Parsing grid to internal representation..."); - if (!parseGrid(grid_req_srv.response.map, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint)) + if (!parseCostmap(costmap_, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint)) { ROS_ERROR("Could not parse retrieved grid"); return false; From 8a446b3cc52caa6fc34af224df9e0e0a16fe5bf4 Mon Sep 17 00:00:00 2001 From: ethanckim Date: Tue, 3 Aug 2021 16:07:44 -0600 Subject: [PATCH 10/12] Changed test launch files due to using costmaps --- .../param/costmap_common_params.yaml | 19 +++++++++++++++++++ .../param/global_costmap_params.yaml | 7 +++++++ .../param/local_costmap_params.yaml | 4 ++-- .../test_full_coverage_path_planner.launch | 5 +++++ 4 files changed, 33 insertions(+), 2 deletions(-) create mode 100644 test/full_coverage_path_planner/param/costmap_common_params.yaml create mode 100644 test/full_coverage_path_planner/param/global_costmap_params.yaml diff --git a/test/full_coverage_path_planner/param/costmap_common_params.yaml b/test/full_coverage_path_planner/param/costmap_common_params.yaml new file mode 100644 index 0000000..a689051 --- /dev/null +++ b/test/full_coverage_path_planner/param/costmap_common_params.yaml @@ -0,0 +1,19 @@ +transform_tolerance: 0.2 + +map_topic: map + +footprint: [[0.46, 0.28], [-0.50, 0.28], [-0.50, -0.28], [0.46, -0.28]] +footprint_padding: 0.05 +inflation_radius: 0.55 + +#observation_sources: point_cloud_sensor +#obstacle_range: 2.5 +#raytrace_range: 3.0 +#point_cloud_sensor: { sensor_frame: d435i_link, +# data_type: PointCloud2, +# topic: /points_filtered, +# marking: true, +# clearing: true, +# expected_update_rate: 1.0 +# max_obstacle_height: 0.75 +# min_obstacle_height: 0.05 } \ No newline at end of file diff --git a/test/full_coverage_path_planner/param/global_costmap_params.yaml b/test/full_coverage_path_planner/param/global_costmap_params.yaml new file mode 100644 index 0000000..e9a0668 --- /dev/null +++ b/test/full_coverage_path_planner/param/global_costmap_params.yaml @@ -0,0 +1,7 @@ +global_costmap: + global_frame: map + robot_base_frame: base_link + update_frequency: 5.0 + publish_frequency: 1.0 + static_map: true + always_send_full_costmap: true \ No newline at end of file diff --git a/test/full_coverage_path_planner/param/local_costmap_params.yaml b/test/full_coverage_path_planner/param/local_costmap_params.yaml index 5147ed1..821db1b 100644 --- a/test/full_coverage_path_planner/param/local_costmap_params.yaml +++ b/test/full_coverage_path_planner/param/local_costmap_params.yaml @@ -1,10 +1,10 @@ local_costmap: global_frame: odom robot_base_frame: base_link - update_frequency: 10.0 + update_frequency: 5.0 publish_frequency: 5.0 width: 5.0 height: 5.0 resolution: 0.05 static_map: false - rolling_window: true + rolling_window: true \ No newline at end of file diff --git a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch index 43033f6..017c053 100644 --- a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch +++ b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch @@ -15,10 +15,14 @@ + + + + @@ -29,6 +33,7 @@ + From f694051d198c1871d6d2e922e2a84fd71bb1dda6 Mon Sep 17 00:00:00 2001 From: ethanckim Date: Wed, 4 Aug 2021 10:23:43 -0600 Subject: [PATCH 11/12] Rename Spiral to Boustrophedon and Updated README.md accordingly --- CMakeLists.txt | 10 ++-- README.md | 41 +++++++------- doc/fcpp_modified_boustrophedon.png | Bin 0 -> 27977 bytes fcpp_plugin.xml | 8 +-- .../{spiral_stc.h => boustrophedon_stc.h} | 24 ++++---- .../full_coverage_path_planner.h | 6 +- package.xml | 8 ++- src/{spiral_stc.cpp => boustrophedon_stc.cpp} | 42 +++++++------- test/README.md | 4 +- test/full_coverage_path_planner/fcpp.rviz | 2 +- .../move_base_sim.launch | 2 +- .../param/planners.yaml | 4 +- .../test_full_coverage_path_planner.launch | 10 ++-- ...t_full_coverage_path_planner_plugin.launch | 6 +- ...ral_stc.cpp => test_boustrophedon_stc.cpp} | 52 +++++++++--------- 15 files changed, 112 insertions(+), 107 deletions(-) create mode 100644 doc/fcpp_modified_boustrophedon.png rename include/full_coverage_path_planner/{spiral_stc.h => boustrophedon_stc.h} (64%) rename src/{spiral_stc.cpp => boustrophedon_stc.cpp} (85%) rename test/src/{test_spiral_stc.cpp => test_boustrophedon_stc.cpp} (89%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8842b0b..38ce075 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,7 +35,7 @@ catkin_package( add_library(${PROJECT_NAME} src/common.cpp src/${PROJECT_NAME}.cpp - src/spiral_stc.cpp + src/boustrophedon_stc.cpp ) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} @@ -65,13 +65,13 @@ catkin_install_python( if (CATKIN_ENABLE_TESTING) catkin_add_gtest(test_common test/src/test_common.cpp test/src/util.cpp src/common.cpp) - 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) - add_dependencies(test_spiral_stc ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - target_link_libraries(test_spiral_stc ${catkin_LIBRARIES}) + catkin_add_gtest(test_boustrophedon_stc test/src/test_boustrophedon_stc.cpp test/src/util.cpp src/boustrophedon_stc.cpp src/common.cpp src/${PROJECT_NAME}.cpp) + add_dependencies(test_boustrophedon_stc ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + target_link_libraries(test_boustrophedon_stc ${catkin_LIBRARIES}) find_package(OpenCV) include_directories(${OpenCV_INCLUDE_DIRS}) - target_link_libraries(test_spiral_stc ${OpenCV_LIBRARIES}) + target_link_libraries(test_boustrophedon_stc ${OpenCV_LIBRARIES}) add_rostest(test/${PROJECT_NAME}/test_${PROJECT_NAME}.test) diff --git a/README.md b/README.md index a4b23ea..c0c9e56 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,19 @@ -# Full Coverage Path Planner (FCPP) +# Boustrophedon Full Coverage Path Planner (Modified) +## Acknowledgement + +This package is a modification of the Full Coverage Path Planner package from Nobleo. + +It is modified such that a Boustrophedon Pattern is used to plan the path rather than a Spiral algorithm in the original package. + +Refer to the original package here: http://wiki.ros.org/full_coverage_path_planner ## Overview -This package provides an implementation of a Full Coverage Path Planner (FCPP) using the Backtracking Spiral Algorithm (BSA), see [1]. +This package provides an implementation of a Full Coverage Path Planner (FCPP) using the Boustrophedon Pattern. see [1] and [2]. This packages acts as a global planner plugin to the Move Base package (http://wiki.ros.org/move_base). -![BSA](doc/fcpp_robot_0_5m_plus_tool_0_2m.png) +![BSA](doc/fcpp_modified_boustrophedon.png) The user can configure robot radius and tool radius separately: @@ -15,17 +22,19 @@ The user can configure robot radius and tool radius separately: **Keywords:** coverage path planning, move base -### License +### Authors Apache 2.0 +**Package modified by Ethan Kim, ethanc.kim@uwaterloo.ca, MapaRobo Inc.** + **Author(s): Yury Brodskiy, Ferry Schoenmakers, Tim Clephas, Jerrel Unkel, Loy van Beek, Cesar lopez** **Maintainer: Cesar Lopez, cesar.lopez@nobleo.nl** **Affiliation: Nobleo Projects BV, Eindhoven, the Netherlands** -The Full Coverage Path Planner package has been tested under [ROS] Melodic and Ubuntu 18.04. +The Modified package has been tested under [ROS] Noetic and Ubuntu 20.04. ## Installation @@ -57,8 +66,8 @@ All tests can be run using: #### test_common Unit test that checks the basic functions used by the repository -#### test_spiral_stc -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. +#### test_boustrophedon_stc +Unit test that checks the basis boustrophedon 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. #### test_full_coverage_path_planner.test 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. @@ -131,8 +140,8 @@ The CoverageProgressNode keeps track of coverage progress. It does this by perio ## Plugins -### full_coverage_path_planner/SpiralSTC -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. +### full_coverage_path_planner/BoustrophedonSTC +For use in move_base(\_flex) as "base_global_planner"="full_coverage_path_planner/BoustrophedonSTC". It uses global_cost_map and global_costmap/robot_radius. #### Parameters @@ -142,18 +151,10 @@ For use in move_base(\_flex) as "base_global_planner"="full_coverage_path_planne ## References -[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. - -## Bugs & Feature Requests - -Please report bugs and request features using the [Issue Tracker](https://github.com/nobleo/full_coverage_path_planner/issues). - - -[ROS]: http://www.ros.org -[rviz]: http://wiki.ros.org/rviz -[MBF]: http://wiki.ros.org/move_base_flex +[1] Choset, Howie, and Philippe Pignon. "Coverage path planning: The boustrophedon cellular decomposition." Field and service robotics. Springer, London, 1998. +[2] Zelinsky, Alexander, et al. "Planning paths of complete coverage of an unstructured environment by a mobile robot." Proceedings of international conference on advanced robotics. Vol. 13. 1993. -## Acknowledgments +## Acknowledgments from Original Authors + diff --git a/test/full_coverage_path_planner/param/planners.yaml b/test/full_coverage_path_planner/param/planners.yaml index a59da1f..d47b710 100644 --- a/test/full_coverage_path_planner/param/planners.yaml +++ b/test/full_coverage_path_planner/param/planners.yaml @@ -1,3 +1,3 @@ planners: - - name: 'SpiralSTC' - type: 'full_coverage_path_planner/SpiralSTC' + - name: 'BoustrophedonSTC' + type: 'full_coverage_path_planner/BoustrophedonSTC' diff --git a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch index 017c053..91d81c2 100644 --- a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch +++ b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch @@ -19,20 +19,20 @@ - - + + - + - + @@ -54,7 +54,7 @@ - + diff --git a/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch b/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch index b113155..d61efd5 100644 --- a/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch +++ b/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch @@ -11,13 +11,13 @@ - - + + - + diff --git a/test/src/test_spiral_stc.cpp b/test/src/test_boustrophedon_stc.cpp similarity index 89% rename from test/src/test_spiral_stc.cpp rename to test/src/test_boustrophedon_stc.cpp index 8335255..5d23a07 100644 --- a/test/src/test_spiral_stc.cpp +++ b/test/src/test_boustrophedon_stc.cpp @@ -24,7 +24,7 @@ #include #include -#include +#include #include cv::Mat drawMap(std::vector > const& grid); @@ -35,15 +35,15 @@ cv::Mat drawPath(const cv::Mat &mapImg, std::list &path); /* - * On a map with nothing on it, spiral_stc should cover all the nodes of the map + * On a map with nothing on it, boustrophedon_stc should cover all the nodes of the map */ -TEST(TestSpiralStc, testFillEmptyMap) +TEST(TestBoustrophedonStc, testFillEmptyMap) { std::vector > grid = makeTestGrid(4, 4, false); Point_t start = {0, 0}; int multiple_pass_counter, visited_counter; - std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, + std::list path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid, start, multiple_pass_counter, visited_counter); @@ -54,7 +54,7 @@ TEST(TestSpiralStc, testFillEmptyMap) /* * On a map with a single obstacle, all the other nodes should still be visited. */ -TEST(TestSpiralStc, testFillMapWithOneObstacle) +TEST(TestBoustrophedonStc, testFillMapWithOneObstacle) { /* * [s 0 0 0] @@ -67,7 +67,7 @@ TEST(TestSpiralStc, testFillMapWithOneObstacle) Point_t start = {0, 0}; int multiple_pass_counter, visited_counter; - std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, + std::list path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid, start, multiple_pass_counter, visited_counter); @@ -82,7 +82,7 @@ TEST(TestSpiralStc, testFillMapWithOneObstacle) /* * In a map with 2 obstacles, still the complete map should be covered except for those 2 obstacles */ -TEST(TestSpiralStc, testFillMapWith2Obstacles) +TEST(TestBoustrophedonStc, testFillMapWith2Obstacles) { /* * [s 0 0 0] @@ -96,7 +96,7 @@ TEST(TestSpiralStc, testFillMapWith2Obstacles) Point_t start = {0, 0}; int multiple_pass_counter, visited_counter; - std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, + std::list path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid, start, multiple_pass_counter, visited_counter); @@ -111,7 +111,7 @@ TEST(TestSpiralStc, testFillMapWith2Obstacles) /* * On a 4x4 map where the opposite right half of the map is blocked, we can cover only the 4x2 reachable nodes */ -TEST(TestSpiralStc, testFillMapWithHalfBlocked) +TEST(TestBoustrophedonStc, testFillMapWithHalfBlocked) { /* * [s 0 1 0] @@ -127,7 +127,7 @@ TEST(TestSpiralStc, testFillMapWithHalfBlocked) Point_t start = {0, 0}; int multiple_pass_counter, visited_counter; - std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, + std::list path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid, start, multiple_pass_counter, visited_counter); @@ -141,9 +141,9 @@ TEST(TestSpiralStc, testFillMapWithHalfBlocked) /* * On a map with a wall almost blocking off a half of the map, but leaving a gap to the other side, - * spiral_stc should still cover all reachable nodes + * boustrophedon_stc should still cover all reachable nodes */ -TEST(TestSpiralStc, testFillMapWithWall) +TEST(TestBoustrophedonStc, testFillMapWithWall) { /* * [s 0 1 0] @@ -158,7 +158,7 @@ TEST(TestSpiralStc, testFillMapWithWall) Point_t start = {0, 0}; int multiple_pass_counter, visited_counter; - std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, + std::list path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid, start, multiple_pass_counter, visited_counter); @@ -173,7 +173,7 @@ TEST(TestSpiralStc, testFillMapWithWall) /* * This test case features a very short dead-end */ -TEST(TestSpiralStc, testDeadEnd1) +TEST(TestBoustrophedonStc, testDeadEnd1) { /* * [0 0 1 0] @@ -189,7 +189,7 @@ TEST(TestSpiralStc, testDeadEnd1) Point_t start = {1, 2}; int multiple_pass_counter, visited_counter; - std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, + std::list path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid, start, multiple_pass_counter, visited_counter); @@ -208,10 +208,10 @@ TEST(TestSpiralStc, testDeadEnd1) /* * This test case is an extension of testDeadEnd1, where the top row is also covered as an obstacle. * The top row is covered but the obstacle from testDeadEnd1 is shifted downwards - * in an attempt to see if SpiralSTC also fails when a dead-end is not on the edge of the map + * in an attempt to see if BoustrophedonSTC also fails when a dead-end is not on the edge of the map * (but below a row of obstacles) */ -TEST(TestSpiralStc, testDeadEnd1WithTopRow) +TEST(TestBoustrophedonStc, testDeadEnd1WithTopRow) { /* * [1 1 1 1] @@ -233,7 +233,7 @@ TEST(TestSpiralStc, testDeadEnd1WithTopRow) Point_t start = {1, 3}; int multiple_pass_counter, visited_counter; - std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, + std::list path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid, start, multiple_pass_counter, visited_counter); @@ -252,7 +252,7 @@ TEST(TestSpiralStc, testDeadEnd1WithTopRow) /* * This test case features a very short dead-end */ -TEST(TestSpiralStc, testDeadEnd2) +TEST(TestBoustrophedonStc, testDeadEnd2) { /* * [1 0 0 0] @@ -268,7 +268,7 @@ TEST(TestSpiralStc, testDeadEnd2) Point_t start = {3, 0}; int multiple_pass_counter, visited_counter; - std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, + std::list path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid, start, multiple_pass_counter, visited_counter); @@ -287,7 +287,7 @@ TEST(TestSpiralStc, testDeadEnd2) /* * This test case features a very short dead-end */ -TEST(TestSpiralStc, testDeadEnd3) +TEST(TestBoustrophedonStc, testDeadEnd3) { /* * [0 0 0 0 0 0 1 0 0] @@ -309,7 +309,7 @@ TEST(TestSpiralStc, testDeadEnd3) Point_t start = {5, 2}; int multiple_pass_counter, visited_counter; - std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, + std::list path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid, start, multiple_pass_counter, visited_counter); @@ -328,7 +328,7 @@ TEST(TestSpiralStc, testDeadEnd3) /* * This test case features a very short dead-end */ -TEST(TestSpiralStc, testDeadEnd3WithTopRow) +TEST(TestBoustrophedonStc, testDeadEnd3WithTopRow) { /* * [1 1 1 1 1 1 1 1 1] @@ -359,7 +359,7 @@ TEST(TestSpiralStc, testDeadEnd3WithTopRow) Point_t start = {5, 3}; // NOLINT int multiple_pass_counter, visited_counter; - std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, + std::list path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid, start, multiple_pass_counter, visited_counter); @@ -496,7 +496,7 @@ Point_t findStart(std::vector > const& grid) 5. On each coordinate in path, fill pixel at that coordinate in pathImg 6. pathImg and floodfilledImg should be identical */ -TEST(TestSpiralStc, testRandomMap) +TEST(TestBoustrophedonStc, testRandomMap) { // Seed pseudorandom sequence to create *reproducible test unsigned int seed = 12345; @@ -515,7 +515,7 @@ TEST(TestSpiralStc, testRandomMap) cv::Mat mapImg = drawMap(grid); Point_t start = findStart(grid); int multiple_pass_counter, visited_counter; - std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, + std::list path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid, start, multiple_pass_counter, visited_counter); From e5a89652bf0f5a1999a614528d7e19ae88628f0f Mon Sep 17 00:00:00 2001 From: Nunchakus Lei Date: Mon, 4 Sep 2023 09:51:50 -0600 Subject: [PATCH 12/12] Let parse costmap function consider robot radius --- src/full_coverage_path_planner.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/full_coverage_path_planner.cpp b/src/full_coverage_path_planner.cpp index 1449e58..c3c873b 100644 --- a/src/full_coverage_path_planner.cpp +++ b/src/full_coverage_path_planner.cpp @@ -193,9 +193,13 @@ bool FullCoveragePathPlanner::parseCostmap(costmap_2d::Costmap2D* costmap_grid_, Point_t& scaledStart) { int ix, iy, nodeRow, nodeCol; - uint32_t nodeSize = dmax(floor(toolRadius / costmap_grid_->getResolution()), 1); // Size of node in pixels/units - uint32_t nRows = costmap_grid_->getSizeInCellsY(), nCols = costmap_grid_->getSizeInCellsX(); + int nodeSize = dmax(floor(toolRadius / costmap_grid_->getResolution()), 1); // Size of node in pixels/units + int robotNodeSize = dmax(floor(robotRadius / costmap_grid_->getResolution()), 1); // RobotRadius in pixels/units + int nodePaddingSize = dmax(ceil(static_cast(robotNodeSize - nodeSize) / 2.0), 0); + int nRows = costmap_grid_->getSizeInCellsY(), nCols = costmap_grid_->getSizeInCellsX(); ROS_INFO("nRows: %u nCols: %u nodeSize: %d", nRows, nCols, nodeSize); + ROS_INFO("parseCostmap -> robotNodeSize: %u nodePaddingSize: %u", robotNodeSize, nodePaddingSize); + ROS_INFO("parseCostmap -> robotRadius: %f toolRadius: %f", robotRadius, toolRadius); if (nRows == 0 || nCols == 0) { @@ -225,12 +229,14 @@ bool FullCoveragePathPlanner::parseCostmap(costmap_2d::Costmap2D* costmap_grid_, for (ix = 0; ix < nCols; ix = ix + nodeSize) { bool nodeOccupied = false; - for (nodeRow = 0; (nodeRow < nodeSize) && ((iy + nodeRow) < nRows) && (nodeOccupied == false); ++nodeRow) + for (nodeRow = 0; (nodeRow < robotNodeSize) && ((iy - nodePaddingSize + nodeRow) < nRows) && (nodeOccupied == false); ++nodeRow) { - for (nodeCol = 0; (nodeCol < nodeSize) && ((ix + nodeCol) < nCols); ++nodeCol) + if ((iy - nodePaddingSize + nodeRow) < 0) {continue;} + for (nodeCol = 0; (nodeCol < robotNodeSize) && ((ix - nodePaddingSize + nodeCol) < nCols); ++nodeCol) { - double mx = ix + nodeCol; - double my = iy + nodeRow; + if ((ix - nodePaddingSize + nodeCol) < 0) {continue;} + double mx = ix - nodePaddingSize + nodeCol; + double my = iy - nodePaddingSize + nodeRow; if (costmap_grid_->getCost(mx, my) > costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { nodeOccupied = true;