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,6 @@ 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
-
 Below are the default values of the parameters :
 ```
 planner_server:
@@ -64,7 +62,6 @@ planner_server:
       how_many_corners: 8
       w_euc_cost: 1.0
       w_traversal_cost: 2.0
-      w_heuristic_cost: 1.0
 ```
 
 ## Usage Notes
@@ -76,9 +73,9 @@ This planner uses the costs associated with each cell from the `global_costmap`
 
 Providing a gentle potential field over the region allows the planner to exchange the increase in path lengths for a decrease in the accumulated traversal cost, thus leading to an increase in the distance from the obstacles. This around a corner allows for naturally smoothing the turns and removes the requirement for an external path smoother.
 
-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.
+`w_heuristic_cost` is an internal setting that is not user changable. It has been provided to have an admissible heuristic, restricting its value to the minimum of `w_euc_cost` and `1.0` to make sure the heuristic and travel costs are admissible and consistent.
 
-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
+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.
 
 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.
 
@@ -91,5 +88,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");
@@ -115,6 +116,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan(
     logger_, "Got the src and dst... (%i, %i) && (%i, %i)",
     planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y);
   getPlan(global_path);
+  global_path.poses.back().pose.orientation = goal.pose.orientation;
 
   // If use_final_approach_orientation=true, interpolate the last pose orientation from the
   // previous pose to set the orientation to the 'final approach' orientation of the robot so
@@ -138,7 +140,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;
 }
@@ -165,15 +167,14 @@ nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation(
 {
   nav_msgs::msg::Path pa;
 
+  geometry_msgs::msg::PoseStamped p1;
   for (unsigned int j = 0; j < raw_path.size() - 1; j++) {
-    geometry_msgs::msg::PoseStamped p;
     coordsW pt1 = raw_path[j];
-    p.pose.position.x = pt1.x;
-    p.pose.position.y = pt1.y;
-    pa.poses.push_back(p);
+    p1.pose.position.x = pt1.x;
+    p1.pose.position.y = pt1.y;
+    pa.poses.push_back(p1);
 
     coordsW pt2 = raw_path[j + 1];
-    geometry_msgs::msg::PoseStamped p1;
     double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y);
     int loops = static_cast<int>(distance / dist_bw_points);
     double sin_alpha = (pt2.y - pt1.y) / distance;
@@ -184,6 +185,7 @@ nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation(
       pa.poses.push_back(p1);
     }
   }
+
   return pa;
 }