From b47bc1348db6352b0ed55712d60750c5ec0b5eda Mon Sep 17 00:00:00 2001 From: Cesar Lopez Date: Tue, 3 Aug 2021 09:14:57 +0200 Subject: [PATCH 1/9] Remove std::cout spiralcpp --- src/spiral_stc.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/spiral_stc.cpp b/src/spiral_stc.cpp index 38adca5..2d5c5b7 100644 --- a/src/spiral_stc.cpp +++ b/src/spiral_stc.cpp @@ -320,7 +320,7 @@ namespace full_coverage_path_planner clock_t end = clock(); double elapsed_secs = static_cast(end - begin) / CLOCKS_PER_SEC; - std::cout << "elapsed time: " << elapsed_secs << "\n"; + RCLCPP_INFO(rclcpp::get_logger("FullCoveragePathPlanner"), "Elapsed time: %f", elapsed_secs); return true; } From 5abd2dc96e1a6e024ce549a7f0d9d8cbabe5d857 Mon Sep 17 00:00:00 2001 From: Cesar Lopez Date: Tue, 3 Aug 2021 09:22:26 +0200 Subject: [PATCH 2/9] use auto for loop --- src/spiral_stc.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/spiral_stc.cpp b/src/spiral_stc.cpp index 2d5c5b7..e5131c8 100644 --- a/src/spiral_stc.cpp +++ b/src/spiral_stc.cpp @@ -191,9 +191,9 @@ namespace full_coverage_path_planner 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) + for (const auto pathNode : pathNodes) { - Point_t newPoint = {it->pos.x, it->pos.y}; + Point_t newPoint = {pathNode.pos.x, pathNode.pos.y}; visited_counter++; fullPath.push_back(newPoint); } @@ -224,13 +224,13 @@ namespace full_coverage_path_planner } // Update visited grid - for (it = pathNodes.begin(); it != pathNodes.end(); ++it) + for (const auto pathNode : pathNodes) { - if (visited[it->pos.y][it->pos.x]) + if (visited[pathNode.pos.y][pathNode.pos.x]) { multiple_pass_counter++; } - visited[it->pos.y][it->pos.x] = eNodeVisited; + visited[pathNode.pos.y][pathNode.pos.x] = eNodeVisited; } if (pathNodes.size() > 0) { @@ -253,9 +253,9 @@ namespace full_coverage_path_planner goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints - for (it = pathNodes.begin(); it != pathNodes.end(); ++it) + for (const auto pathNode : pathNodes) { - Point_t newPoint = {it->pos.x, it->pos.y}; + Point_t newPoint = {pathNode.pos.x, pathNode.pos.y}; visited_counter++; fullPath.push_back(newPoint); } From ea7caac927bb40911166e3c555dc85d2c05d3d96 Mon Sep 17 00:00:00 2001 From: Cesar Lopez Date: Tue, 3 Aug 2021 09:51:46 +0200 Subject: [PATCH 3/9] move variable dafinitions spiral function --- src/spiral_stc.cpp | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/spiral_stc.cpp b/src/spiral_stc.cpp index e5131c8..871305a 100644 --- a/src/spiral_stc.cpp +++ b/src/spiral_stc.cpp @@ -93,7 +93,8 @@ namespace full_coverage_path_planner std::list SpiralSTC::spiral(std::vector> const &grid, std::list &init, std::vector> &visited) { - int dx, dy, dx_prev, x2, y2, nRows = grid.size(), nCols = grid[0].size(); + int nRows = grid.size(); + int nCols = grid[0].size(); // Spiral filling of the open space // Copy incoming list to 'end' std::list pathNodes(init); @@ -106,6 +107,10 @@ namespace full_coverage_path_planner bool done = false; while (!done) { + // Initialize spiral direction towards y-axis + int dx = 0; + int dy = 1; + int dx_prev; if (it != pathNodes.begin()) { // turn ccw @@ -115,18 +120,13 @@ namespace full_coverage_path_planner dx = -dy; dy = dx_prev; } - else - { - // Initialize spiral direction towards y-axis - dx = 0; - dy = 1; - } + // This condition might change in the loop bellow done = true; for (int i = 0; i < 4; ++i) { - x2 = pathNodes.back().pos.x + dx; - y2 = pathNodes.back().pos.y + dy; + int x2 = pathNodes.back().pos.x + dx; + int y2 = pathNodes.back().pos.y + dy; if (x2 >= 0 && x2 < nCols && y2 >= 0 && y2 < nRows) { if (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen) @@ -160,13 +160,14 @@ namespace full_coverage_path_planner int &multiple_pass_counter, int &visited_counter) { - int x, y; // Initial node is initially set as visited so it does not count multiple_pass_counter = 0; visited_counter = 0; std::vector> visited; visited = grid; // Copy grid matrix + int x; + int y; x = init.x; y = init.y; From 25fcd014e6356f2558c496284cc5fb470128a4e7 Mon Sep 17 00:00:00 2001 From: Cesar Lopez Date: Tue, 3 Aug 2021 09:57:48 +0200 Subject: [PATCH 4/9] small fixes spiral_stc.cpp --- src/spiral_stc.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/spiral_stc.cpp b/src/spiral_stc.cpp index 871305a..9884750 100644 --- a/src/spiral_stc.cpp +++ b/src/spiral_stc.cpp @@ -122,8 +122,8 @@ namespace full_coverage_path_planner } // This condition might change in the loop bellow done = true; - - for (int i = 0; i < 4; ++i) + // loop over the four possible directions: up, right, down, left + for (size_t i = 0; i < 4; ++i) { int x2 = pathNodes.back().pos.x + dx; int y2 = pathNodes.back().pos.y + dy; @@ -143,6 +143,7 @@ namespace full_coverage_path_planner it = --(pathNodes.end()); visited[y2][x2] = eNodeVisited; // Close node done = false; + // brake for loop break; } } @@ -166,10 +167,8 @@ namespace full_coverage_path_planner std::vector> visited; visited = grid; // Copy grid matrix - int x; - int y; - x = init.x; - y = init.y; + int x = init.x; + int y = init.y; Point_t new_point = {x, y}; gridNode_t new_node = From 6c6a68609d881b71a3624724e6c91426cf3001a5 Mon Sep 17 00:00:00 2001 From: Cesar Lopez Date: Tue, 3 Aug 2021 11:05:22 +0200 Subject: [PATCH 5/9] Use const to define coverage cost --- .../full_coverage_path_planner/full_coverage_path_planner.hpp | 2 ++ src/full_coverage_path_planner.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/include/full_coverage_path_planner/full_coverage_path_planner.hpp b/include/full_coverage_path_planner/full_coverage_path_planner.hpp index 410c88a..71a4053 100644 --- a/include/full_coverage_path_planner/full_coverage_path_planner.hpp +++ b/include/full_coverage_path_planner/full_coverage_path_planner.hpp @@ -49,6 +49,7 @@ enum namespace full_coverage_path_planner { + class FullCoveragePathPlanner { public: @@ -66,6 +67,7 @@ namespace full_coverage_path_planner ~FullCoveragePathPlanner() { } + static const unsigned char COVERAGE_COST = 65; // Cost for checking coverage. Perhaps define this in coverage costmap plugin? protected: /** diff --git a/src/full_coverage_path_planner.cpp b/src/full_coverage_path_planner.cpp index 569decf..fee674b 100644 --- a/src/full_coverage_path_planner.cpp +++ b/src/full_coverage_path_planner.cpp @@ -228,7 +228,7 @@ namespace full_coverage_path_planner for (nodeColl = 0; (nodeColl < robotNodeSize) && ((ix + nodeColl) < nCols); ++nodeColl) { int index_grid = dmax((iy + nodeRow - ceil(static_cast(robotNodeSize - nodeSize) / 2.0)) * nCols + (ix + nodeColl - ceil(static_cast(robotNodeSize - nodeSize) / 2.0)), 0); - if (cpp_costmap_data[index_grid] > 65) + if (cpp_costmap_data[index_grid] > COVERAGE_COST) { nodeOccupied = true; break; From 13179197598b8b133cc93f811323810490f28c21 Mon Sep 17 00:00:00 2001 From: Cesar Lopez Date: Tue, 3 Aug 2021 11:15:44 +0200 Subject: [PATCH 6/9] simplify gui_path pucblish --- src/full_coverage_path_planner.cpp | 30 ++++++++++++------------------ 1 file changed, 12 insertions(+), 18 deletions(-) diff --git a/src/full_coverage_path_planner.cpp b/src/full_coverage_path_planner.cpp index fee674b..a8fcaf4 100644 --- a/src/full_coverage_path_planner.cpp +++ b/src/full_coverage_path_planner.cpp @@ -46,18 +46,11 @@ namespace full_coverage_path_planner // create a message for the plan nav_msgs::msg::Path gui_path; - gui_path.poses.resize(path.size()); - - if (!path.empty()) - { - gui_path.header.frame_id = path[0].header.frame_id; - gui_path.header.stamp = path[0].header.stamp; - } - - // Extract the plan in world co-ordinates, we assume the path is all in the same frame - for (unsigned int i = 0; i < path.size(); i++) + gui_path.header.frame_id = path[0].header.frame_id; + gui_path.header.stamp = path[0].header.stamp; + for (const auto pose : path) { - gui_path.poses[i] = path[i]; + gui_path.poses.push_back(pose); } plan_pub_->publish(gui_path); @@ -69,7 +62,6 @@ namespace full_coverage_path_planner { geometry_msgs::msg::PoseStamped new_goal; std::list::const_iterator it, it_next, it_prev; - int dx_now, dy_now, dx_next, dy_next, move_dir_now = 0, move_dir_next = 0; bool do_publish = false; float orientation = eDirNone; RCLCPP_INFO(rclcpp::get_logger("FullCoveragePathPlanner"), "Received goalpoints with length: %lu", goalpoints.size()); @@ -82,6 +74,10 @@ namespace full_coverage_path_planner it_prev = it; it_prev--; + int dx_now; + int dy_now; + int dx_next; + int dy_next; // Check for the direction of movement if (it == goalpoints.begin()) { @@ -102,8 +98,8 @@ namespace full_coverage_path_planner // 0 + 1*2 = 2 // -1 + 0*2 = -1 // 0 + -1*2 = -2 - move_dir_now = dx_now + dy_now * 2; - move_dir_next = dx_next + dy_next * 2; + int move_dir_now = dx_now + dy_now * 2; + int move_dir_next = dx_next + dy_next * 2; // Check if this points needs to be published (i.e. a change of direction or first or last point in list) do_publish = move_dir_next != move_dir_now || it == goalpoints.begin() || @@ -191,8 +187,6 @@ namespace full_coverage_path_planner geometry_msgs::msg::PoseStamped const &realStart, Point_t &scaledStart) { - size_t ix; - size_t iy; size_t nodeRow; size_t nodeColl; size_t nodeSize = dmax(floor(toolRadius / cpp_costmap->getResolution()), 1); // Size of node in pixels/units @@ -217,10 +211,10 @@ namespace full_coverage_path_planner floor(cpp_costmap->getSizeInCellsY() / tile_size_))); // Scale grid - for (iy = 0; iy < nRows; iy = iy + nodeSize) + for (size_t iy = 0; iy < nRows; iy = iy + nodeSize) { std::vector gridRow; - for (ix = 0; ix < nCols; ix = ix + nodeSize) + for (size_t ix = 0; ix < nCols; ix = ix + nodeSize) { bool nodeOccupied = false; for (nodeRow = 0; (nodeRow < robotNodeSize) && ((iy + nodeRow) < nRows) && (nodeOccupied == false); ++nodeRow) From 406ce4a29e944a0773938af1211a0e5bb9d7f310 Mon Sep 17 00:00:00 2001 From: Cesar Lopez Date: Tue, 3 Aug 2021 11:22:48 +0200 Subject: [PATCH 7/9] use c++ enum --- .../full_coverage_path_planner.hpp | 17 +++++++++-------- src/full_coverage_path_planner.cpp | 12 ++++++------ 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/include/full_coverage_path_planner/full_coverage_path_planner.hpp b/include/full_coverage_path_planner/full_coverage_path_planner.hpp index 71a4053..3ca716f 100644 --- a/include/full_coverage_path_planner/full_coverage_path_planner.hpp +++ b/include/full_coverage_path_planner/full_coverage_path_planner.hpp @@ -38,18 +38,19 @@ using std::string; #define clamp(a, lower, upper) dmax(dmin(a, upper), lower) #endif -enum -{ - eDirNone = 0, - eDirRight = 1, - eDirUp = 2, - eDirLeft = -1, - eDirDown = -2, -}; namespace full_coverage_path_planner { + enum dir + { + none = 0, + right = 1, + up = 2, + left = -1, + down = -2, + }; + class FullCoveragePathPlanner { public: diff --git a/src/full_coverage_path_planner.cpp b/src/full_coverage_path_planner.cpp index a8fcaf4..23c30a4 100644 --- a/src/full_coverage_path_planner.cpp +++ b/src/full_coverage_path_planner.cpp @@ -63,7 +63,7 @@ namespace full_coverage_path_planner geometry_msgs::msg::PoseStamped new_goal; std::list::const_iterator it, it_next, it_prev; bool do_publish = false; - float orientation = eDirNone; + float orientation = dir::none; RCLCPP_INFO(rclcpp::get_logger("FullCoveragePathPlanner"), "Received goalpoints with length: %lu", goalpoints.size()); if (goalpoints.size() > 1) { @@ -114,19 +114,19 @@ namespace full_coverage_path_planner // Calculate desired orientation to be in line with movement direction switch (move_dir_now) { - case eDirNone: + case dir::none: // Keep orientation break; - case eDirRight: + case dir::right: orientation = 0; break; - case eDirUp: + case dir::up: orientation = M_PI / 2; break; - case eDirLeft: + case dir::left: orientation = M_PI; break; - case eDirDown: + case dir::down: orientation = M_PI * 1.5; break; } From 6353245892e8d893db0d4ae12c30523670ff070a Mon Sep 17 00:00:00 2001 From: Cesar Lopez Date: Tue, 3 Aug 2021 14:47:11 +0200 Subject: [PATCH 8/9] Use double instead of float --- .../full_coverage_path_planner.hpp | 12 ++++++------ src/full_coverage_path_planner.cpp | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/include/full_coverage_path_planner/full_coverage_path_planner.hpp b/include/full_coverage_path_planner/full_coverage_path_planner.hpp index 3ca716f..b41ba9e 100644 --- a/include/full_coverage_path_planner/full_coverage_path_planner.hpp +++ b/include/full_coverage_path_planner/full_coverage_path_planner.hpp @@ -91,8 +91,8 @@ namespace full_coverage_path_planner */ bool parseGrid(nav2_costmap_2d::Costmap2D const * cpp_costmap, std::vector> &grid, - float robotRadius, - float toolRadius, + double robotRadius, + double toolRadius, geometry_msgs::msg::PoseStamped const &realStart, Point_t &scaledStart); @@ -110,10 +110,10 @@ namespace full_coverage_path_planner } nav2_util::LifecycleNode::SharedPtr node_; rclcpp::Publisher::SharedPtr plan_pub_; - float robot_radius_; - float tool_radius_; - float plan_resolution_; - float tile_size_; + double robot_radius_; + double tool_radius_; + double plan_resolution_; + double tile_size_; dPoint_t grid_origin_; bool initialized_; geometry_msgs::msg::PoseStamped previous_goal_; diff --git a/src/full_coverage_path_planner.cpp b/src/full_coverage_path_planner.cpp index 23c30a4..10dcc89 100644 --- a/src/full_coverage_path_planner.cpp +++ b/src/full_coverage_path_planner.cpp @@ -63,7 +63,7 @@ namespace full_coverage_path_planner geometry_msgs::msg::PoseStamped new_goal; std::list::const_iterator it, it_next, it_prev; bool do_publish = false; - float orientation = dir::none; + double orientation = dir::none; RCLCPP_INFO(rclcpp::get_logger("FullCoveragePathPlanner"), "Received goalpoints with length: %lu", goalpoints.size()); if (goalpoints.size() > 1) { @@ -182,8 +182,8 @@ namespace full_coverage_path_planner bool FullCoveragePathPlanner::parseGrid(nav2_costmap_2d::Costmap2D const * cpp_costmap, std::vector> &grid, - float robotRadius, - float toolRadius, + double robotRadius, + double toolRadius, geometry_msgs::msg::PoseStamped const &realStart, Point_t &scaledStart) { @@ -221,7 +221,7 @@ namespace full_coverage_path_planner { for (nodeColl = 0; (nodeColl < robotNodeSize) && ((ix + nodeColl) < nCols); ++nodeColl) { - int index_grid = dmax((iy + nodeRow - ceil(static_cast(robotNodeSize - nodeSize) / 2.0)) * nCols + (ix + nodeColl - ceil(static_cast(robotNodeSize - nodeSize) / 2.0)), 0); + int index_grid = dmax((iy + nodeRow - ceil(static_cast(robotNodeSize - nodeSize) / 2.0)) * nCols + (ix + nodeColl - ceil(static_cast(robotNodeSize - nodeSize) / 2.0)), 0); if (cpp_costmap_data[index_grid] > COVERAGE_COST) { nodeOccupied = true; From a3433607565c21ca9b1d5705470419b138db1801 Mon Sep 17 00:00:00 2001 From: Cesar Lopez Date: Tue, 3 Aug 2021 14:51:14 +0200 Subject: [PATCH 9/9] Handle empty point list case --- src/full_coverage_path_planner.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/full_coverage_path_planner.cpp b/src/full_coverage_path_planner.cpp index 10dcc89..d99ec67 100644 --- a/src/full_coverage_path_planner.cpp +++ b/src/full_coverage_path_planner.cpp @@ -65,7 +65,20 @@ namespace full_coverage_path_planner bool do_publish = false; double orientation = dir::none; RCLCPP_INFO(rclcpp::get_logger("FullCoveragePathPlanner"), "Received goalpoints with length: %lu", goalpoints.size()); - if (goalpoints.size() > 1) + if(goalpoints.size()<1) + { + RCLCPP_WARN(rclcpp::get_logger("FullCoveragePathPlanner"), "Empty point list"); + return; + } + else if (goalpoints.size() == 1) + { + new_goal.header.frame_id = "map"; + new_goal.pose.position.x = (goalpoints.begin()->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5; + new_goal.pose.position.y = (goalpoints.begin()->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5; + new_goal.pose.orientation = createQuaternionMsgFromYaw(0); + plan.push_back(new_goal); + } + else { for (it = goalpoints.begin(); it != goalpoints.end(); ++it) { @@ -146,14 +159,7 @@ namespace full_coverage_path_planner } } } - else - { - new_goal.header.frame_id = "map"; - new_goal.pose.position.x = (goalpoints.begin()->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5; - new_goal.pose.position.y = (goalpoints.begin()->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5; - new_goal.pose.orientation = createQuaternionMsgFromYaw(0); - plan.push_back(new_goal); - } + /* Add poses from current position to start of plan */ // Compute angle between current pose and first plan point