Skip to content

Fix MAX_NON_OBSTACLE_COST in theta star planner (backport #5865) #5869

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

Merged
SteveMacenski merged 1 commit into from
Jan 15, 2026
Merged

Fix MAX_NON_OBSTACLE_COST in theta star planner (backport #5865) #5869

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
5 changes: 1 addition & 4 deletions nav2_theta_star_planner/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,9 +25,6 @@ The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0` and the

**f(a)** - total cost (g(a) + h(a)) for the node 'a'

**LETHAL_COST** - a value of the costmap traversal cost that inscribes an obstacle with
respect to a function, value = 253

**curr** - represents the node whose neighbours are being added to the list

**neigh** - one of the neighboring nodes of curr
Expand All @@ -36,7 +33,7 @@ respect to a function, value = 253

**euc_cost(a,b)** - euclidean distance between the node type 'a' and type 'b'

**costmap_cost(a,b)** - the costmap traversal cost (ranges from 0 - 252, 254 = unknown value) between the node type 'a' and type 'b'
**costmap_cost(a,b)** - the costmap traversal cost (ranges from 0 - 252, 255 = unknown value) between the node type 'a' and type 'b'

### Cost function
```
Expand Down
29 changes: 16 additions & 13 deletions nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,8 @@

const double INF_COST = DBL_MAX;
const int UNKNOWN_COST = 255;
const int OBS_COST = 254;
const int LETHAL_COST = 252;
const int OCCUPIED_COST = 254;
const int MAX_NON_OBSTACLE_COST = 252;

struct coordsM
{
Expand Down Expand Up @@ -90,14 +90,13 @@ class ThetaStar
bool generatePath(std::vector<coordsW> & raw_path);

/**
* @brief this function checks whether the cost of a point(cx, cy) on the costmap is less than the LETHAL_COST
* @brief this function checks whether the cost of a point(cx, cy) on the costmap is less than or equal to the MAX_NON_OBSTACLE_COST
* @return the result of the check
*/
inline bool isSafe(const int & cx, const int & cy) const
{
return (costmap_->getCost(
cx,
cy) == UNKNOWN_COST && allow_unknown_) || costmap_->getCost(cx, cy) < LETHAL_COST;
return (costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) ||
costmap_->getCost(cx, cy) <= MAX_NON_OBSTACLE_COST;
}

/**
Expand All @@ -108,8 +107,8 @@ class ThetaStar
const geometry_msgs::msg::PoseStamped & goal);

/**
* @brief checks whether the start and goal points have costmap costs greater than LETHAL_COST
* @return true if the cost of any one of the points is greater than LETHAL_COST
* @brief checks whether the start and goal points have costmap costs greater than MAX_NON_OBSTACLE_COST
* @return true if the cost of any one of the points is greater than MAX_NON_OBSTACLE_COST
*/
bool isUnsafeToPlan() const
{
Expand Down Expand Up @@ -186,16 +185,19 @@ class ThetaStar
/**
* @brief it is an overloaded function to ease the cost calculations while performing the LOS check
* @param cost denotes the total straight line traversal cost; it adds the traversal cost for the node (cx, cy) at every instance; it is also being returned
* @return false if the traversal cost is greater than / equal to the LETHAL_COST and true otherwise
* @return false if the traversal cost is greater than the MAX_NON_OBSTACLE_COST and true otherwise
*/
bool isSafe(const int & cx, const int & cy, double & cost) const
{
double curr_cost = getCost(cx, cy);
if ((costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) || curr_cost < LETHAL_COST) {
if ((costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) ||
curr_cost <= MAX_NON_OBSTACLE_COST)
{
if (costmap_->getCost(cx, cy) == UNKNOWN_COST) {
curr_cost = OBS_COST - 1;
curr_cost = OCCUPIED_COST - 1;
}
cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST;
cost += w_traversal_cost_ * curr_cost * curr_cost / MAX_NON_OBSTACLE_COST /
MAX_NON_OBSTACLE_COST;
return true;
} else {
return false;
Expand All @@ -219,7 +221,8 @@ class ThetaStar
inline double getTraversalCost(const int & cx, const int & cy)
{
double curr_cost = getCost(cx, cy);
return w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST;
return w_traversal_cost_ * curr_cost * curr_cost / MAX_NON_OBSTACLE_COST /
MAX_NON_OBSTACLE_COST;
}

/**
Expand Down