Updates to Nav2 Theta Star Planner docs

nav2_theta_star_planner/README.md

@@ -13,7 +13,7 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav
 For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m -
 ![example.png](img/00-37.png)
 
-The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5`
+The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5`
 
 ## Cost Function Details
 ### Symbols and their meanings
@@ -51,8 +51,7 @@ The parameters of the planner are :
 - ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8
 - ` .w_euc_cost ` : weight applied on the length of the path. 
 - ` .w_traversal_cost ` : it tunes how harshly the nodes of high cost are penalised. From the above g(neigh) equation you can see that the cost-aware component of the cost function forms a parabolic curve, thus this parameter would, on increasing its value, make that curve steeper allowing for a greater differentiation (as the delta of costs would increase, when the graph becomes steep) among the nodes of different costs.
-- ` .w_heuristic_cost ` : it has been provided to have an admissible heuristic
-
+- `w_heuristic_cost ` : it has been provided to have an admissible heuristic; it is **not** a user configurable parameter
 Below are the default values of the parameters :
 ```
 planner_server:
@@ -64,7 +63,6 @@ planner_server:
       how_many_corners: 8
       w_euc_cost: 1.0
       w_traversal_cost: 2.0
-      w_heuristic_cost: 1.0
 ```
 
 ## Usage Notes
@@ -78,8 +76,6 @@ Providing a gentle potential field over the region allows the planner to exchang
 
 To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversly make the paths less taut. So it is recommend ed that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time.
 
-Usually set `w_heuristic_cost` at the same value as `w_euc_cost` or 1.0 (whichever is lower). Though as a last resort you may increase the value of `w_heuristic_cost` to speed up the planning process, this is not recommended as it might not always lead to shorter query times, but would definitely give you sub optimal paths
-
 Also note that changes to `w_traversal_cost` might cause slow downs, in case the number of node expanisions increase thus tuning it along with `w_euc_cost` is the way to go to.
 
 While tuning the planner's parameters you can also change the `inflation_layer`'s parameters (of the global costmap) to tune the behavior of the paths.
@@ -91,5 +87,5 @@ This planner is recommended to be used with local planners like DWB or TEB (or o
 
 While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended to do so because it might increase the query times from the planner. Test the planners performance on the higher cm/px costmaps before making a switch to finer costmaps.
 
-### Possible Applications
-This planner could be used in scenarios where planning speed matters over an extremely smooth path, which could anyways be handled by using a local planner/controller as mentioned above. Because of the any-angled nature of paths you can traverse environments diagonally (wherever it is allowed, eg: in a wide open region).
+### When to use this planner?
+This planner could be used in scenarios where planning speed matters over an extremely smooth path, which could anyways be handled by using a local planner/controller as mentioned above. Because of the any-angled nature of paths you can traverse environments diagonally (wherever it is allowed, eg: in a wide open region). Another use case would be when you have corridors that are misaligned in the map image, in such a case this planner would be able to give straight-line like paths as it considers line of sight and thus avoid giving jagged paths which arises with other planners because of the finite directions of motion they consider.

nav2_theta_star_planner/src/theta_star_planner.cpp

@@ -52,6 +52,7 @@ void ThetaStarPlanner::configure(
     node, name_ + ".w_traversal_cost", rclcpp::ParameterValue(2.0));
   node->get_parameter(name_ + ".w_traversal_cost", planner_->w_traversal_cost_);
 
+  planner_->w_heuristic_cost_ = planner_->w_euc_cost_ < 1.0 ? planner_->w_euc_cost_ : 1.0;
   nav2_util::declare_parameter_if_not_declared(
     node, name_ + ".w_heuristic_cost", rclcpp::ParameterValue(1.0));
   node->get_parameter(name_ + ".w_heuristic_cost", planner_->w_heuristic_cost_);
@@ -87,7 +88,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan(
   // Corner case of start and goal beeing on the same cell
   unsigned int mx_start, my_start, mx_goal, my_goal;
   planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start);
-  planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_goal, my_goal);
+  planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal);
   if (mx_start == mx_goal && my_start == my_goal) {
     if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) {
       RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles");
@@ -138,7 +139,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan(
 
   auto stop_time = std::chrono::steady_clock::now();
   auto dur = std::chrono::duration_cast<std::chrono::microseconds>(stop_time - start_time);
-  RCLCPP_DEBUG(logger_, "the time taken for pointy is : %i", static_cast<int>(dur.count()));
+  RCLCPP_DEBUG(logger_, "the time taken is : %i", static_cast<int>(dur.count()));
   RCLCPP_DEBUG(logger_, "the nodes_opened are:  %i", planner_->nodes_opened);
   return global_path;
 }