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

fixed dijkstra horizontal/vertical bias #940

Open
wants to merge 1 commit into
base: melodic-devel
Choose a base branch
from
Open

fixed dijkstra horizontal/vertical bias #940

Show file tree
Hide file tree
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
1 change: 1 addition & 0 deletions global_planner/include/global_planner/dijkstra.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,7 @@ class DijkstraExpansion : public Expander {
int currentEnd_, nextEnd_, overEnd_; /**< end points of arrays */
bool *pending_; /**< pending_ cells during propagation */
bool precise_;
int tick_count_; // counter for selecting cell push_back ordering

/** block priority thresholds */
float threshold_; /**< current threshold */
Expand Down
68 changes: 47 additions & 21 deletions global_planner/src/dijkstra.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@
namespace global_planner {

DijkstraExpansion::DijkstraExpansion(PotentialCalculator* p_calc, int nx, int ny) :
Expander(p_calc, nx, ny), pending_(NULL), precise_(false) {
Expander(p_calc, nx, ny), pending_(NULL), precise_(false), tick_count_(0) {
// priority buffers
buffer1_ = new int[PRIORITYBUFSIZE];
buffer2_ = new int[PRIORITYBUFSIZE];
Expand Down Expand Up @@ -198,6 +198,7 @@ inline void DijkstraExpansion::updateCell(unsigned char* costs, float* potential
return;

float pot = p_calc_->calculatePotential(potential, c, n);
tick_count_ ^= 1; // Toggle tick_count bit

// now add affected neighbors to priority blocks
if (pot < potential[n]) {
Expand All @@ -207,26 +208,51 @@ inline void DijkstraExpansion::updateCell(unsigned char* costs, float* potential
float de = INVSQRT2 * (float)getCost(costs, n + nx_);
potential[n] = pot;
//ROS_INFO("UPDATE %d %d %d %f", n, n%nx, n/nx, potential[n]);
if (pot < threshold_) // low-cost buffer block
{
if (potential[n - 1] > pot + le)
push_next(n-1);
if (potential[n + 1] > pot + re)
push_next(n+1);
if (potential[n - nx_] > pot + ue)
push_next(n-nx_);
if (potential[n + nx_] > pot + de)
push_next(n+nx_);
} else // overflow block
{
if (potential[n - 1] > pot + le)
push_over(n-1);
if (potential[n + 1] > pot + re)
push_over(n+1);
if (potential[n - nx_] > pot + ue)
push_over(n-nx_);
if (potential[n + nx_] > pot + de)
push_over(n+nx_);
if (tick_count_==0) { // Push back horizontals first
if (pot < threshold_) // low-cost buffer block
{
if (potential[n - 1] > pot + le)
push_next(n-1);
if (potential[n + 1] > pot + re)
push_next(n+1);
if (potential[n - nx_] > pot + ue)
push_next(n-nx_);
if (potential[n + nx_] > pot + de)
push_next(n+nx_);
} else // overflow block
{
if (potential[n - 1] > pot + le)
push_over(n-1);
if (potential[n + 1] > pot + re)
push_over(n+1);
if (potential[n - nx_] > pot + ue)
push_over(n-nx_);
if (potential[n + nx_] > pot + de)
push_over(n+nx_);
}
}
else { // Push back verticals first
if (pot < threshold_) // low-cost buffer block
{
if (potential[n - nx_] > pot + ue)
push_next(n-nx_);
if (potential[n + nx_] > pot + de)
push_next(n+nx_);
if (potential[n - 1] > pot + le)
push_next(n-1);
if (potential[n + 1] > pot + re)
push_next(n+1);
} else // overflow block
{
if (potential[n - nx_] > pot + ue)
push_over(n-nx_);
if (potential[n + nx_] > pot + de)
push_over(n+nx_);
if (potential[n - 1] > pot + le)
push_over(n-1);
if (potential[n + 1] > pot + re)
push_over(n+1);
}
}
}
}
Expand Down