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,