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..b41ba9e 100644 --- a/include/full_coverage_path_planner/full_coverage_path_planner.hpp +++ b/include/full_coverage_path_planner/full_coverage_path_planner.hpp @@ -38,17 +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: @@ -66,6 +68,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: /** @@ -88,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); @@ -107,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 569decf..d99ec67 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,11 +62,23 @@ 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; + 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) { @@ -82,6 +87,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 +111,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() || @@ -118,19 +127,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; } @@ -150,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 @@ -186,13 +188,11 @@ 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) { - 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,18 +217,18 @@ 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) { 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) + 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; break; diff --git a/src/spiral_stc.cpp b/src/spiral_stc.cpp index 38adca5..9884750 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) + // loop over the four possible directions: up, right, down, left + for (size_t 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) @@ -143,6 +143,7 @@ namespace full_coverage_path_planner it = --(pathNodes.end()); visited[y2][x2] = eNodeVisited; // Close node done = false; + // brake for loop break; } } @@ -160,15 +161,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 - x = init.x; - y = init.y; + int x = init.x; + int y = init.y; Point_t new_point = {x, y}; gridNode_t new_node = @@ -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); } @@ -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; }