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

Jump to bottom

Optimizations for the navfn planner in large map scenarios #4244

Open
wants to merge 1 commit into
base: humble
Choose a base branch
from
Open

Optimizations for the navfn planner in large map scenarios #4244

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
27 changes: 19 additions & 8 deletions nav2_navfn_planner/src/navfn.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -209,7 +209,13 @@ NavFn::setNavArr(int xs, int ys)

nx = xs;
ny = ys;
ns = nx * ny;
auto new_ns = nx * ny;

if (new_ns == ns) {
return;
Copy link
Member

Choose a reason for hiding this comment

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

Should we not still set the costarr and pending to 0?

}

ns = new_ns;

if (costarr) {
delete[] costarr;
Expand Down Expand Up @@ -341,13 +347,16 @@ void
NavFn::setupNavFn(bool keepit)
{
// reset values in propagation arrays
for (int i = 0; i < ns; i++) {
potarr[i] = POT_HIGH;
if (!keepit) {
costarr[i] = COST_NEUTRAL;
std::fill(potarr, potarr + ns, POT_HIGH);
if (!keepit) {
if constexpr (std::is_same<COSTTYPE, unsigned char>::value || std::is_same<COSTTYPE, char>::value) {
std::memset(costarr, COST_NEUTRAL, ns * sizeof(COSTTYPE));
} else {
std::fill(costarr, costarr + ns, COST_NEUTRAL);
}
gradx[i] = grady[i] = 0.0;
}
std::memset(gradx, 0, ns * sizeof(float));
std::memset(grady, 0, ns * sizeof(float));

// outer bounds of cost array
COSTTYPE * pc;
Expand Down Expand Up @@ -382,6 +391,7 @@ NavFn::setupNavFn(bool keepit)
int k = goal[0] + goal[1] * nx;
initCost(k, 0);

#ifdef DEBUG
// find # of obstacle cells
pc = costarr;
int ntot = 0;
Expand All @@ -391,6 +401,7 @@ NavFn::setupNavFn(bool keepit)
}
}
nobs = ntot;
#endif // DEBUG
}


Expand Down Expand Up @@ -635,12 +646,12 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart)
}
}
}

#ifdef DEBUG
RCLCPP_DEBUG(
rclcpp::get_logger("rclcpp"),
"[NavFn] Used %d cycles, %d cells visited (%d%%), priority buf max %d\n",
cycle, nc, (int)((nc * 100.0) / (ns - nobs)), nwv);

#endif // DEBUG
return (cycle < cycles) ? true : false;
}

Expand Down