Fix MAX_NON_OBSTACLE_COST in theta star planner (#5865) (#5868)

(cherry picked from commit 018dca9)

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
Co-authored-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com>

Author: mergify[bot]

nav2_theta_star_planner/README.md

@@ -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
@@ -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
 ```

nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp

@@ -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
 {
@@ -92,14 +92,13 @@ class ThetaStar
   bool generatePath(std::vector<coordsW> & raw_path, std::function<bool()> cancel_checker);
 
   /**
-   * @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;
   }
 
   /**
@@ -110,8 +109,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
   {
@@ -193,16 +192,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;
@@ -226,7 +228,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;
   }
 
   /**