Merge pull request ros-navigation#21 from botsandus/AUTO-854_viz_planner

AUTO-854 visualize planner espansions

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

@@ -26,6 +26,8 @@
 #include <vector>
 
 #include "nav2_smac_planner/a_star.hpp"
+#include <geometry_msgs/msg/pose_array.hpp>
+
 using namespace std::chrono;  // NOLINT
 
 namespace nav2_smac_planner
@@ -221,7 +223,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 +276,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(
@@ -328,9 +342,14 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   NodeHybrid::CoordinateVector path;
   int num_iterations = 0;
   std::string error;
+  std::unique_ptr<std::vector<std::tuple<float, float>>> expansions = nullptr;
   try {
+    if (_viz_expansions) {
+      expansions = std::make_unique<std::vector<std::tuple<float, float>>>();
+    }
     if (!_a_star->createPath(
-        path, num_iterations, _tolerance / static_cast<float>(costmap->getResolution())))
+        path, num_iterations, _tolerance / static_cast<float>(costmap->getResolution()),
+        expansions.get()))
     {
       if (num_iterations < _a_star->getMaxIterations()) {
         error = std::string("no valid path found");
@@ -343,6 +362,19 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
     error += e.what();
   }
 
+  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);
+  }
+
   if (!error.empty()) {
     RCLCPP_WARN(
       _logger,