Skip to content

Commit

Permalink
Impl SetStartAndEndConfiguration
Browse files Browse the repository at this point in the history
  • Loading branch information
@nkuwenjian
nkuwenjian committed Oct 23, 2023
1 parent 92ce302 commit 4a72625
Show file tree
Hide file tree
Showing 4 changed files with 52 additions and 40 deletions.
5 changes: 3 additions & 2 deletions include/lattice_path_planner/grid_search/grid_search.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,13 +87,14 @@ class GridSearch {
private:
bool SetStart(int start_x, int start_y);
bool SetEnd(int end_x, int end_y);
bool SetStartAndEndConfiguration(int sx, int sy, int ex, int ey);
Node2d* GetNode(int grid_x, int grid_y);
int CalcHeuCost(int grid_x, int grid_y) const;
bool IsWithinMap(int grid_x, int grid_y) const;
bool IsValidCell(int grid_x, int grid_y) const;
int CalcGridXYIndex(int grid_x, int grid_y) const;
int GetKey(Node2d* node) const;
void UpdateSuccs(const Node2d& curr_node);
int GetKey(const Node2d* node) const;
void UpdateSuccs(const Node2d* curr_node);
void ComputeGridSearchActions();
int GetActionCost(int curr_x, int curr_y, int action_id) const;
void LoadGridAStarResult(GridAStarResult* result) const;
Expand Down
4 changes: 2 additions & 2 deletions include/lattice_path_planner/lattice_a_star/lattice_a_star.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,15 +83,15 @@ class LatticeAStar {
double EuclidHeuCost(int x1, int y1, int x2, int y2) const;

// check collision and validity
bool ValidityCheck(const Node3d& node) const;
bool ValidityCheck(const Node3d* node) const;
bool IsValidCell(int grid_x, int grid_y) const;

bool WorldToGrid(double x, double y, double phi, int* grid_x, int* grid_y,
int* grid_phi) const;
bool GridToWorld(int grid_x, int grid_y, int grid_phi, double* x, double* y,
double* phi) const;
bool IsWithinMap(int grid_x, int grid_y) const;
void UpdateSuccs(const Node3d& curr_node);
void UpdateSuccs(const Node3d* curr_node);
int GetActionCost(int curr_x, int curr_y,
const primitive_generator::Primitive& action) const;
Node3d* GetNode(int grid_x, int grid_y, int grid_phi);
Expand Down
53 changes: 31 additions & 22 deletions src/grid_search/grid_search.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,22 +83,9 @@ bool GridSearch::GenerateGridPath(
iterations_++;

// check the validity of start/goal
if (!SetStart(sx, sy)) {
LOG(ERROR) << "GridSearch is called on invalid start (" << sx << "," << sy
<< ")";
if (!SetStartAndEndConfiguration(sx, sy, ex, ey)) {
return false;
}
CHECK_NOTNULL(start_node_);
// since the goal has not been set yet, the start node's h value is set to 0
CHECK_EQ(start_node_->h(), 0);

if (!SetEnd(ex, ey)) {
LOG(ERROR) << "GridSearch is called on invalid end (" << ex << "," << ey
<< ")";
return false;
}
CHECK_NOTNULL(end_node_);
CHECK_EQ(end_node_->h(), 0);

// initialize start node and insert it into heap
start_node_->set_g(0);
Expand All @@ -121,7 +108,7 @@ bool GridSearch::GenerateGridPath(

// new expand
++explored_node_num;
UpdateSuccs(*node);
UpdateSuccs(node);
}

const auto end_timestamp = std::chrono::system_clock::now();
Expand Down Expand Up @@ -163,7 +150,8 @@ int GridSearch::CalcGridXYIndex(const int grid_x, const int grid_y) const {
return grid_x + grid_y * max_grid_x_;
}

int GridSearch::GetKey(Node2d* node) const {
int GridSearch::GetKey(const Node2d* node) const {
CHECK_NOTNULL(node);
return termination_condition_ ==
TerminationCondition::TERM_CONDITION_OPTPATHFOUND
? node->g() + node->h()
Expand Down Expand Up @@ -327,6 +315,26 @@ bool GridSearch::SetEnd(const int end_x, const int end_y) {
return true;
}

bool GridSearch::SetStartAndEndConfiguration(int sx, int sy, int ex, int ey) {
if (!SetStart(sx, sy)) {
LOG(ERROR) << "VoronoiPlanner is called on invalid start (" << sx << ","
<< sy << ")";
return false;
}
CHECK_NOTNULL(start_node_);
// since the goal has not been set yet, the start node's h value is set to 0
CHECK_EQ(start_node_->h(), 0);

if (!SetEnd(ex, ey)) {
LOG(ERROR) << "VoronoiPlanner is called on invalid end (" << ex << "," << ey
<< ")";
return false;
}
CHECK_NOTNULL(end_node_);
CHECK_EQ(end_node_->h(), 0);
return true;
}

Node2d* GridSearch::GetNode(const int grid_x, const int grid_y) {
DCHECK(IsWithinMap(grid_x, grid_y));
Node2d* node = &dp_lookup_table_[grid_x][grid_y];
Expand All @@ -340,9 +348,10 @@ Node2d* GridSearch::GetNode(const int grid_x, const int grid_y) {
return node;
}

void GridSearch::UpdateSuccs(const Node2d& curr_node) {
const int curr_x = curr_node.grid_x();
const int curr_y = curr_node.grid_y();
void GridSearch::UpdateSuccs(const Node2d* curr_node) {
CHECK_NOTNULL(curr_node);
const int curr_x = curr_node->grid_x();
const int curr_y = curr_node->grid_y();
for (int action_id = 0; action_id < common::kNumOfGridSearchActions;
++action_id) {
const int succ_x = curr_x + actions_.dx[action_id];
Expand All @@ -363,9 +372,9 @@ void GridSearch::UpdateSuccs(const Node2d& curr_node) {
Node2d* succ_node = GetNode(succ_x, succ_y);
// see if we can decrease the value of successive node taking into account
// the cost of action
if (succ_node->g() > curr_node.g() + action_cost) {
succ_node->set_g(curr_node.g() + action_cost);
succ_node->set_pre_node(&curr_node);
if (succ_node->g() > curr_node->g() + action_cost) {
succ_node->set_g(curr_node->g() + action_cost);
succ_node->set_pre_node(curr_node);

// re-insert into heap if not closed yet
if (succ_node->heap_index() == 0) {
Expand Down
30 changes: 16 additions & 14 deletions src/lattice_a_star/lattice_a_star.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ bool LatticeAStar::SetStart(double start_x, double start_y, double start_phi) {

// create start node
start_node_ = GetNode(start_grid_x, start_grid_y, start_grid_phi);
if (!ValidityCheck(*start_node_)) {
if (!ValidityCheck(start_node_)) {
LOG(ERROR) << "Invalid start node: (" << start_grid_x << "," << start_grid_y
<< "," << start_grid_phi << ")";
return false;
Expand All @@ -100,7 +100,7 @@ bool LatticeAStar::SetEnd(double end_x, double end_y, double end_phi) {

// create end node.
end_node_ = GetNode(end_grid_x, end_grid_y, end_grid_phi);
if (!ValidityCheck(*end_node_)) {
if (!ValidityCheck(end_node_)) {
LOG(ERROR) << "Invalid end node: (" << end_grid_x << "," << end_grid_y
<< "," << end_grid_phi << ")";
return false;
Expand Down Expand Up @@ -172,7 +172,7 @@ bool LatticeAStar::Plan(double start_x, double start_y, double start_phi,
node->grid_x(), node->grid_y())] = common::NodeStatus::CLOSED;
// new expand
++explored_node_num;
UpdateSuccs(*node);
UpdateSuccs(node);

if (explored_node_num % 100000 == 0 && explored_node_num > 0) {
LOG(INFO) << "expands so far=" << explored_node_num;
Expand Down Expand Up @@ -255,10 +255,11 @@ bool LatticeAStar::IsWithinMap(const int grid_x, const int grid_y) const {
grid_y < env_cfg_.max_grid_y;
}

bool LatticeAStar::ValidityCheck(const Node3d& node) const {
int grid_x = node.grid_x();
int grid_y = node.grid_y();
int grid_phi = node.grid_phi();
bool LatticeAStar::ValidityCheck(const Node3d* node) const {
CHECK_NOTNULL(node);
int grid_x = node->grid_x();
int grid_y = node->grid_y();
int grid_phi = node->grid_phi();
if (!IsValidCell(grid_x, grid_y)) {
return false;
}
Expand All @@ -284,10 +285,11 @@ bool LatticeAStar::ValidityCheck(const Node3d& node) const {
return true;
}

void LatticeAStar::UpdateSuccs(const Node3d& curr_node) {
const int curr_x = curr_node.grid_x();
const int curr_y = curr_node.grid_y();
const int curr_phi = curr_node.grid_phi();
void LatticeAStar::UpdateSuccs(const Node3d* curr_node) {
CHECK_NOTNULL(curr_node);
const int curr_x = curr_node->grid_x();
const int curr_y = curr_node->grid_y();
const int curr_phi = curr_node->grid_phi();
const std::vector<primitive_generator::Primitive>& actions =
motion_primitive_generator_->motion_primitives()[curr_phi];

Expand All @@ -312,9 +314,9 @@ void LatticeAStar::UpdateSuccs(const Node3d& curr_node) {
Node3d* succ_node = GetNode(succ_x, succ_y, succ_phi);
// see if we can decrease the value of successive node taking into account
// the cost of action
if (succ_node->g() > curr_node.g() + action_cost) {
succ_node->set_g(curr_node.g() + action_cost);
succ_node->set_pre_node(&curr_node);
if (succ_node->g() > curr_node->g() + action_cost) {
succ_node->set_g(curr_node->g() + action_cost);
succ_node->set_pre_node(curr_node);
succ_node->set_action_idx({curr_phi, action.id});

// re-insert into heap if not closed yet
Expand Down

0 comments on commit 4a72625

Please sign in to comment.