Skip to content

Commit

Permalink
review theta_star and lazy_theta_star planner
Browse files Browse the repository at this point in the history
  • Loading branch information
@ZhanyuGuo
ZhanyuGuo committed Aug 23, 2024
1 parent 6295cbc commit 5ad7ee4
Show file tree
Hide file tree
Showing 5 changed files with 116 additions and 176 deletions.
21 changes: 6 additions & 15 deletions src/core/global_planner/graph_planner/include/lazy_theta_star.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,6 @@
#ifndef LAZY_THETA_STAR_H
#define LAZY_THETA_STAR_H

#include <vector>
#include <queue>
#include "theta_star.h"

namespace global_planner
Expand All @@ -31,28 +29,21 @@ class LazyThetaStar : public ThetaStar
public:
/**
* @brief Construct a new LaztThetaStar object
* @param costmap the environment for path planning
* @param costmap the environment for path planning
*/
LazyThetaStar(costmap_2d::Costmap2D* costmap);

/**
* @brief Lazy Theta* implementation
* @param start start node
* @param goal goal node
* @param path optimal path consists of Node
* @param expand containing the node been search during the process
* @param start start node
* @param goal goal node
* @param path optimal path consists of Node
* @param expand containing the node been search during the process
* @return true if path found, else false
*/
bool plan(const Node& start, const Node& goal, std::vector<Node>& path, std::vector<Node>& expand);

protected:
/**
* @brief update the g value of child node
* @param parent
* @param child
*/
void _updateVertex(const Node& parent, Node& child);

/**
* @brief check if the parent of vertex need to be updated. if so, update it
* @param node
Expand All @@ -61,7 +52,7 @@ class LazyThetaStar : public ThetaStar

private:
std::unordered_map<int, Node> closed_list_; // closed list
std::vector<Node> motion_; // possible motions
std::vector<Node> motions_; // possible motions
};
} // namespace global_planner
#endif
29 changes: 10 additions & 19 deletions src/core/global_planner/graph_planner/include/theta_star.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,6 @@
#ifndef THETA_STAR_H
#define THETA_STAR_H

#include <vector>
#include <queue>
#include "global_planner.h"

namespace global_planner
Expand All @@ -31,35 +29,28 @@ class ThetaStar : public GlobalPlanner
public:
/**
* @brief Construct a new ThetaStar object
* @param costmap the environment for path planning
* @param costmap the environment for path planning
*/
ThetaStar(costmap_2d::Costmap2D* costmap);

/**
* @brief Theta* implementation
* @param start start node
* @param goal goal node
* @param path optimal path consists of Node
* @param expand containing the node been search during the process
* @return true if path found, else false
* @param start start node
* @param goal goal node
* @param path optimal path consists of Node
* @param expand containing the node been search during the process
* @return true if path found, else false
*/
bool plan(const Node& start, const Node& goal, std::vector<Node>& path, std::vector<Node>& expand);

protected:
/**
* @brief update the g value of child node
* @param parent
* @param child
*/
void _updateVertex(const Node& parent, Node& child);

/**
* @brief Bresenham algorithm to check if there is any obstacle between parent and child
* @param parent
* @param child
* @brief Bresenham algorithm to check if there is any obstacle between node1 and node2
* @param node1 node1
* @param node2 node2
* @return true if no obstacle, else false
*/
bool _lineOfSight(const Node& parent, const Node& child);
bool _lineOfSight(const Node& node1, const Node& node2);
};
} // namespace global_planner
#endif
88 changes: 30 additions & 58 deletions src/core/global_planner/graph_planner/src/lazy_theta_star.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,11 +20,11 @@ namespace global_planner
{
/**
* @brief Construct a new LazyThetaStar object
* @param costmap the environment for path planning
* @param costmap the environment for path planning
*/
LazyThetaStar::LazyThetaStar(costmap_2d::Costmap2D* costmap) : ThetaStar(costmap)
{
motion_ = Node::getMotion();
motions_ = Node::getMotion();
};

/**
Expand All @@ -38,28 +38,29 @@ LazyThetaStar::LazyThetaStar(costmap_2d::Costmap2D* costmap) : ThetaStar(costmap
bool LazyThetaStar::plan(const Node& start, const Node& goal, std::vector<Node>& path, std::vector<Node>& expand)
{
// initialize
closed_list_.clear();
path.clear();
expand.clear();

// push the start node into open list
// open list and closed list
std::priority_queue<Node, std::vector<Node>, Node::compare_cost> open_list;
closed_list_.clear();

open_list.push(start);

// main process
while (!open_list.empty())
{
// pop current node from open list
Node current = open_list.top();
auto current = open_list.top();
open_list.pop();

_setVertex(current);

if (current.g() >= std::numeric_limits<double>::max())
continue;

// current node does not exist in closed list
if (closed_list_.find(current.id()) != closed_list_.end())
// current node exist in closed list, continue
if (closed_list_.count(current.id()))
continue;

closed_list_.insert(std::make_pair(current.id(), current));
Expand All @@ -73,12 +74,10 @@ bool LazyThetaStar::plan(const Node& start, const Node& goal, std::vector<Node>&
}

// explore neighbor of current node
for (const auto& m : motion_)
for (const auto& motion : motions_)
{
// explore a new node
// path 1
Node node_new = current + m; // add the .x(), .y(), g_
node_new.set_h(helper::dist(node_new, goal));
auto node_new = current + motion; // including current.g + motion.g
node_new.set_id(grid2Index(node_new.x(), node_new.y()));
node_new.set_pid(current.id());

Expand All @@ -92,77 +91,51 @@ bool LazyThetaStar::plan(const Node& start, const Node& goal, std::vector<Node>&
costmap_->getCharMap()[node_new.id()] >= costmap_->getCharMap()[current.id()]))
continue;

node_new.set_h(helper::dist(node_new, goal));

// path 1: same to a_star
open_list.push(node_new);

// get parent node
Node parent;
parent.set_id(current.pid());
int tmp_x, tmp_y;
index2Grid(parent.id(), tmp_x, tmp_y);
parent.set_x(tmp_x);
parent.set_y(tmp_y);
auto find_parent = closed_list_.find(parent.id());
if (find_parent != closed_list_.end())
auto current_parent_it = closed_list_.find(current.pid());
if (current_parent_it != closed_list_.end())
{
parent = find_parent->second;
// path 2
_updateVertex(parent, node_new);
auto current_parent = current_parent_it->second;
node_new.set_g(current_parent.g() + helper::dist(current_parent, node_new));
node_new.set_pid(current_parent.id());
open_list.push(node_new);
}

open_list.push(node_new);
}
}

return false;
}

/**
* @brief update the g value of child node
* @param parent
* @param child
*/
void LazyThetaStar::_updateVertex(const Node& parent, Node& child)
{
// path 2
if (parent.g() + helper::dist(parent, child) < child.g())
{
child.set_g(parent.g() + helper::dist(parent, child));
child.set_pid(parent.id());
}
}

/**
* @brief check if the parent of vertex need to be updated. if so, update it
* @param node
*/
void LazyThetaStar::_setVertex(Node& node)
{
// get the coordinate of parent node
Node parent;
parent.set_id(node.pid());
int tmp_x, tmp_y;
index2Grid(parent.id(), tmp_x, tmp_y);
parent.set_x(tmp_x);
parent.set_y(tmp_y);

// if no parent, no need to check the line of sight
auto find_parent = closed_list_.find(parent.id());
if (find_parent == closed_list_.end())
auto parent_it = closed_list_.find(node.pid());
if (parent_it == closed_list_.end())
return;
parent = find_parent->second;

auto parent = parent_it->second;

if (!_lineOfSight(parent, node))
{
// path 1
node.set_g(std::numeric_limits<double>::max());
for (const auto& m : motion_)
for (const auto& motion : motions_)
{
Node parent_new = node + m;
auto parent_new = node + motion;
parent_new.set_id(grid2Index(parent_new.x(), parent_new.y()));
auto find_parent_new = closed_list_.find(parent_new.id());

if (find_parent_new != closed_list_.end())
auto parent_new_it = closed_list_.find(parent_new.id());
if (parent_new_it != closed_list_.end())
{
// parent_new exists in closed list
parent_new = find_parent_new->second;
parent_new = parent_new_it->second;
if (parent_new.g() + helper::dist(parent_new, node) < node.g())
{
node.set_g(parent_new.g() + helper::dist(parent_new, node));
Expand All @@ -172,5 +145,4 @@ void LazyThetaStar::_setVertex(Node& node)
}
}
}

} // namespace global_planner
Loading

0 comments on commit 5ad7ee4

Please sign in to comment.