Add viz_expansions parameter for debug

nav2_smac_planner/README.md

@@ -126,6 +126,7 @@ planner_server:
  cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. 
  allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. 
  smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes.
+ viz_expansions: True # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning). WARNING: heavy to compute and to display, for debug only as it degrades the performance. 
  smoother:
  max_iterations: 1000
  w_smooth: 0.3
@@ -206,26 +207,4 @@ As such, it is recommended if you have sparse SLAM maps, gaps or holes in your m
 
 One interesting thing to note from the second figure is that you see a number of expansions in open space. This is due to travel / heuristic values being so similar, tuning values of the penalty weights can have a decent impact there. The defaults are set as a good middle ground between large open spaces and confined aisles (environment specific tuning could be done to reduce the number of expansions for a specific map, speeding up the planner). The planner actually runs substantially faster the more confined the areas of search / environments are -- but still plenty fast for even wide open areas!
 
-Sometimes visualizing the expansions is very useful to debug potential concerns (why does this goal take longer to compute, why can't I find a path, etc), should you on rare occasion run into an issue. The following snippet is what I used to visualize the expansion in the images above which may help you in future endevours.
-
-``` cpp
-// In createPath()
-static auto node = std::make_shared<rclcpp::Node>("test");
-static auto pub = node->create_publisher<geometry_msgs::msg::PoseArray>("expansions", 1);
-geometry_msgs::msg::PoseArray msg;
-geometry_msgs::msg::Pose msg_pose;
-msg.header.stamp = node->now();
-msg.header.frame_id = "map";
-
-...
-
-// Each time we expand a new node 
-msg_pose.position.x = _costmap->getOriginX() + (current_node->pose.x * _costmap->getResolution());
-msg_pose.position.y = _costmap->getOriginY() + (current_node->pose.y * _costmap->getResolution());
-msg.poses.push_back(msg_pose);
-
-... 
-
-// On backtrace or failure
-pub->publish(msg);
-```
+Sometimes visualizing the expansions is very useful to debug potential concerns (why does this goal take longer to compute, why can't I find a path, etc), should you on rare occasion run into an issue. You can enable the publication of the expansions on the `/expansions` topic for SmacHybrid with the parameter `viz_expansions: True`, beware that it should be used only for debug as it increases a lot the CPU usage.

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -22,6 +22,7 @@
 #include <memory>
 #include <queue>
 #include <utility>
+#include <tuple>
 #include "Eigen/Core"
 
 #include "nav2_costmap_2d/costmap_2d.hpp"
@@ -104,9 +105,12 @@ class AStarAlgorithm
  * @param path Reference to a vector of indicies of generated path
  * @param num_iterations Reference to number of iterations to create plan
  * @param tolerance Reference to tolerance in costmap nodes
+ * @param expansions_log Optional expansions logged for debug
  * @return if plan was successful
  */
- bool createPath(CoordinateVector & path, int & num_iterations, const float & tolerance);
+ bool createPath(
+ CoordinateVector & path, int & num_iterations, const float & tolerance,
+ std::vector<std::tuple<float, float>> * expansions_log = nullptr);
 
  /**
  * @brief Sets the collision checker to use

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -29,6 +29,7 @@
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_array.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "tf2/utils.h"
@@ -116,9 +117,12 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
  double _max_planning_time;
  double _lookup_table_size;
  double _minimum_turning_radius_global_coords;
+ bool _viz_expansions;
  std::string _motion_model_for_search;
  MotionModel _motion_model;
  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
+ rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
+ _expansions_publisher;
  std::mutex _mutex;
  rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
 

nav2_smac_planner/src/a_star.cpp

@@ -221,7 +221,8 @@ bool AStarAlgorithm<NodeT>::areInputsValid()
 template<typename NodeT>
 bool AStarAlgorithm<NodeT>::createPath(
  CoordinateVector & path, int & iterations,
- const float & tolerance)
+ const float & tolerance,
+ std::vector<std::tuple<float, float>> * expansions_log)
 {
  steady_clock::time_point start_time = steady_clock::now();
  _tolerance = tolerance;
@@ -273,6 +274,17 @@ bool AStarAlgorithm<NodeT>::createPath(
  // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue
  current_node = getNextNode();
 
+ // Save current node coordinates for debug
+ if (expansions_log) {
+ Coordinates coords = current_node->getCoords(
+ current_node->getIndex(), getSizeX(), getSizeDim3());
+ expansions_log->push_back(
+ std::make_tuple<float, float>(
+ _costmap->getOriginX() + (coords.x * _costmap->getResolution()),
+ _costmap->getOriginY() + (coords.y * _costmap->getResolution()))
+ );
+ }
+
  // We allow for nodes to be queued multiple times in case
  // shorter paths result in it, but we can visit only once
  if (current_node->wasVisited()) {

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -133,6 +133,10 @@ void SmacPlannerHybrid::configure(
  node, name + ".lookup_table_size", rclcpp::ParameterValue(20.0));
  node->get_parameter(name + ".lookup_table_size", _lookup_table_size);
 
+ nav2_util::declare_parameter_if_not_declared(
+ node, name + ".viz_expansions", rclcpp::ParameterValue(false));
+ node->get_parameter(name + ".viz_expansions", _viz_expansions);
+
  nav2_util::declare_parameter_if_not_declared(
  node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN")));
  node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search);
@@ -215,6 +219,9 @@ void SmacPlannerHybrid::configure(
  }
 
  _raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
+ if (_viz_expansions) {
+ _expansions_publisher = node->create_publisher<geometry_msgs::msg::PoseArray>("expansions", 1);
+ }
 
  RCLCPP_INFO(
  _logger, "Configured plugin %s of type SmacPlannerHybrid with "
@@ -231,6 +238,9 @@ void SmacPlannerHybrid::activate()
  _logger, "Activating plugin %s of type SmacPlannerHybrid",
  _name.c_str());
  _raw_plan_publisher->on_activate();
+ if (_viz_expansions) {
+ _expansions_publisher->on_activate();
+ }
  if (_costmap_downsampler) {
  _costmap_downsampler->on_activate();
  }
@@ -246,6 +256,9 @@ void SmacPlannerHybrid::deactivate()
  _logger, "Deactivating plugin %s of type SmacPlannerHybrid",
  _name.c_str());
  _raw_plan_publisher->on_deactivate();
+ if (_viz_expansions) {
+ _expansions_publisher->on_deactivate();
+ }
  if (_costmap_downsampler) {
  _costmap_downsampler->on_deactivate();
  }
@@ -264,6 +277,7 @@ void SmacPlannerHybrid::cleanup()
  _costmap_downsampler.reset();
  }
  _raw_plan_publisher.reset();
+ _expansions_publisher.reset();
 }
 
 nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
@@ -336,16 +350,34 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
  // Compute plan
  NodeHybrid::CoordinateVector path;
  int num_iterations = 0;
-
+ std::string error;
+ std::unique_ptr<std::vector<std::tuple<float, float>>> expansions = nullptr;
+ if (_viz_expansions) {
+ expansions = std::make_unique<std::vector<std::tuple<float, float>>>();
+ }
  // Note: All exceptions thrown are handled by the planner server and returned to the action
- if (!_a_star->createPath(path, num_iterations, 0)) {
+ if (!_a_star->createPath(path, num_iterations, 0, expansions.get())) {
  if (num_iterations < _a_star->getMaxIterations()) {
  throw nav2_core::NoValidPathCouldBeFound("no valid path found");
  } else {
  throw nav2_core::PlannerTimedOut("exceeded maximum iterations");
  }
  }
 
+ // Publish expansions for debug
+ if (_viz_expansions) {
+ geometry_msgs::msg::PoseArray msg;
+ geometry_msgs::msg::Pose msg_pose;
+ msg.header.stamp = _clock->now();
+ msg.header.frame_id = _global_frame;
+ for (auto & e : *expansions) {
+ msg_pose.position.x = std::get<0>(e);
+ msg_pose.position.y = std::get<1>(e);
+ msg.poses.push_back(msg_pose);
+ }
+ _expansions_publisher->publish(msg);
+ }
+
  // Convert to world coordinates
  plan.poses.reserve(path.size());
  for (int i = path.size() - 1; i >= 0; --i) {