Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Galactic code review #29

Merged
merged 9 commits into from
Jun 8, 2023
Merged

Galactic code review #29

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
b47bc13
Remove std::cout spiralcpp
Aug 3, 2021
5abd2dc
use auto for loop
Aug 3, 2021
ea7caac
move variable dafinitions spiral function
Aug 3, 2021
25fcd01
small fixes spiral_stc.cpp
Aug 3, 2021
6c6a686
Use const to define coverage cost
Aug 3, 2021
1317919
simplify gui_path pucblish
Aug 3, 2021
406ce4a
use c++ enum
Aug 3, 2021
6353245
Use double instead of float
Aug 3, 2021
a343360
Handle empty point list case
Aug 3, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 17 additions & 14 deletions include/full_coverage_path_planner/full_coverage_path_planner.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

enum class dir

{
none = 0,
right = 1,
up = 2,
left = -1,
down = -2,
};

class FullCoveragePathPlanner
{
public:
Expand All @@ -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:
/**
Expand All @@ -88,8 +91,8 @@ namespace full_coverage_path_planner
*/
bool parseGrid(nav2_costmap_2d::Costmap2D const * cpp_costmap,
std::vector<std::vector<bool>> &grid,
float robotRadius,
float toolRadius,
double robotRadius,
double toolRadius,
geometry_msgs::msg::PoseStamped const &realStart,
Point_t &scaledStart);

Expand All @@ -107,10 +110,10 @@ namespace full_coverage_path_planner
}
nav2_util::LifecycleNode::SharedPtr node_;
rclcpp::Publisher<nav_msgs::msg::Path>::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_;
Expand Down
74 changes: 37 additions & 37 deletions src/full_coverage_path_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -69,11 +62,23 @@ namespace full_coverage_path_planner
{
geometry_msgs::msg::PoseStamped new_goal;
std::list<Point_t>::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)
{
Expand All @@ -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())
{
Expand All @@ -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() ||
Expand All @@ -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;
}
Expand All @@ -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
Expand Down Expand Up @@ -186,13 +188,11 @@ namespace full_coverage_path_planner

bool FullCoveragePathPlanner::parseGrid(nav2_costmap_2d::Costmap2D const * cpp_costmap,
std::vector<std::vector<bool>> &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
Expand All @@ -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<bool> 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<float>(robotNodeSize - nodeSize) / 2.0)) * nCols + (ix + nodeColl - ceil(static_cast<float>(robotNodeSize - nodeSize) / 2.0)), 0);
if (cpp_costmap_data[index_grid] > 65)
int index_grid = dmax((iy + nodeRow - ceil(static_cast<double>(robotNodeSize - nodeSize) / 2.0)) * nCols + (ix + nodeColl - ceil(static_cast<double>(robotNodeSize - nodeSize) / 2.0)), 0);
if (cpp_costmap_data[index_grid] > COVERAGE_COST)
{
nodeOccupied = true;
break;
Expand Down
44 changes: 22 additions & 22 deletions src/spiral_stc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,8 @@ namespace full_coverage_path_planner
std::list<gridNode_t> SpiralSTC::spiral(std::vector<std::vector<bool>> const &grid, std::list<gridNode_t> &init,
std::vector<std::vector<bool>> &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<gridNode_t> pathNodes(init);
Expand All @@ -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
Expand All @@ -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)
Expand All @@ -143,6 +143,7 @@ namespace full_coverage_path_planner
it = --(pathNodes.end());
visited[y2][x2] = eNodeVisited; // Close node
done = false;
// brake for loop
break;
}
}
Expand All @@ -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<std::vector<bool>> 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 =
Expand All @@ -191,9 +191,9 @@ namespace full_coverage_path_planner
std::list<Point_t> goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints
// Add points to full path
std::list<gridNode_t>::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);
}
Expand Down Expand Up @@ -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)
{
Expand All @@ -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);
}
Expand Down Expand Up @@ -320,7 +320,7 @@ namespace full_coverage_path_planner

clock_t end = clock();
double elapsed_secs = static_cast<double>(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;
}
Expand Down