Add viz_expansions parameter for debug (ros-navigation#3577)

* viz_expansions

* lint

* switch to unique_ptr

* readme update

---------

Co-authored-by: Guillaume Doisy <[email protected]>
Signed-off-by: enricosutera <[email protected]>

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) {