diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index ed3f810f095..c888d0a6a45 100644 --- a/nav2_theta_star_planner/README.md +++ b/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. diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 18ab9a7759a..3134315fc83 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/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(stop_time - start_time); - RCLCPP_DEBUG(logger_, "the time taken for pointy is : %i", static_cast(dur.count())); + RCLCPP_DEBUG(logger_, "the time taken is : %i", static_cast(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(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; }