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 7 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
19 changes: 11 additions & 8 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 Down
44 changes: 19 additions & 25 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,9 +62,8 @@ 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;
float orientation = dir::none;
RCLCPP_INFO(rclcpp::get_logger("FullCoveragePathPlanner"), "Received goalpoints with length: %lu", goalpoints.size());
if (goalpoints.size() > 1)
Copy link
Contributor Author

Choose a reason for hiding this comment

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

Can you elaborate more on your previous comment?

Copy link
Member

Choose a reason for hiding this comment

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

Sure!

Early-outs make the code more readable by handling the exceptions in a short block at the top.

function do_awesome()
{
  if (everything_okay_as_expected)
  {
    Do 
    lots 
    of 
    indented stuff
  }
  else
  {
    warn about it
  }
}

Can be replaced by:

function do_awesome()
{
  if (!everything_okay_as_expected)
  { 
    warn about it
    return;
  }

  Do 
  lots 
  of 
  stuff
}

Copy link
Member

Choose a reason for hiding this comment

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

And here it looks like goalpoints.size() <= 1 is exceptional.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Got it... in our case the function does not return right after the else condition, so we can not early-out. I can switch the order to at least show the especial case first.

Copy link
Member

Choose a reason for hiding this comment

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

Aaah, I thought it did!
Then you may leave it like this as well. Up to you.

Copy link
Member

Choose a reason for hiding this comment

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

On a similar note. I think the node dies if goalpoints.size() == 0. Since that case isn't handled.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Yep.. will add that one

{
Expand All @@ -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())
{
Expand All @@ -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() ||
Expand All @@ -118,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;
}
Expand Down Expand Up @@ -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
Expand All @@ -217,18 +211,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)
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