debug visualizations for lattice planner (ros-navigation#4034)

* debug visualizations for lattice planner

Signed-off-by: Steve Macenski <[email protected]>

* fix a typo

* fix test

* Update planner_server.cpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Steve Macenski <[email protected]>

nav2_planner/src/planner_server.cpp

@@ -644,8 +644,8 @@ void PlannerServer::isPathValid(
 
     /**
      * The lethal check starts at the closest point to avoid points that have already been passed
-     * and may have become occupied. The method for collision detection is based on the shape of 
-     * the footprint. 
+     * and may have become occupied. The method for collision detection is based on the shape of
+     * the footprint.
      */
     std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
     unsigned int mx = 0;

nav2_smac_planner/README.md

@@ -126,7 +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.
-      debug_visualizations: True                # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. 
+      debug_visualizations: True                # For Hybrid/Lattice nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. 
       smoother:
         max_iterations: 1000
         w_smooth: 0.3

nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp

@@ -27,6 +27,7 @@
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
+#include "geometry_msgs/msg/pose_array.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_util/node_utils.hpp"
@@ -111,6 +112,11 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
   double _max_planning_time;
   double _lookup_table_size;
+  bool _debug_visualizations;
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    _planned_footprints_publisher;
+  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
+    _expansions_publisher;
   std::mutex _mutex;
   rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
 

nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -126,6 +126,9 @@ void SmacPlannerLattice::configure(
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".allow_reverse_expansion", rclcpp::ParameterValue(false));
   node->get_parameter(name + ".allow_reverse_expansion", _search_info.allow_reverse_expansion);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".debug_visualizations", rclcpp::ParameterValue(false));
+  node->get_parameter(name + ".debug_visualizations", _debug_visualizations);
 
   _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath);
   _search_info.minimum_turning_radius =
@@ -193,6 +196,12 @@ void SmacPlannerLattice::configure(
     _smoother->initialize(_metadata.min_turning_radius);
   }
 
+  if (_debug_visualizations) {
+    _expansions_publisher = node->create_publisher<geometry_msgs::msg::PoseArray>("expansions", 1);
+    _planned_footprints_publisher = node->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "planned_footprints", 1);
+  }
+
   RCLCPP_INFO(
     _logger, "Configured plugin %s of type SmacPlannerLattice with "
     "maximum iterations %i, max on approach iterations %i, "
@@ -208,6 +217,10 @@ void SmacPlannerLattice::activate()
     _logger, "Activating plugin %s of type SmacPlannerLattice",
     _name.c_str());
   _raw_plan_publisher->on_activate();
+  if (_debug_visualizations) {
+    _expansions_publisher->on_activate();
+    _planned_footprints_publisher->on_activate();
+  }
   auto node = _node.lock();
   // Add callback for dynamic parameters
   _dyn_params_handler = node->add_on_set_parameters_callback(
@@ -220,6 +233,10 @@ void SmacPlannerLattice::deactivate()
     _logger, "Deactivating plugin %s of type SmacPlannerLattice",
     _name.c_str());
   _raw_plan_publisher->on_deactivate();
+  if (_debug_visualizations) {
+    _expansions_publisher->on_deactivate();
+    _planned_footprints_publisher->on_deactivate();
+  }
   _dyn_params_handler.reset();
 }
 
@@ -282,11 +299,30 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   NodeLattice::CoordinateVector path;
   int num_iterations = 0;
   std::string error;
+  std::unique_ptr<std::vector<std::tuple<float, float, float>>> expansions = nullptr;
+  if (_debug_visualizations) {
+    expansions = std::make_unique<std::vector<std::tuple<float, float, float>>>();
+  }
 
   // Note: All exceptions thrown are handled by the planner server and returned to the action
   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 (_debug_visualizations) {
+      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_pose.orientation = getWorldOrientation(std::get<2>(e));
+        msg.poses.push_back(msg_pose);
+      }
+      _expansions_publisher->publish(msg);
+    }
+
     // Note: If the start is blocked only one iteration will occur before failure
     if (num_iterations == 1) {
       throw nav2_core::StartOccupied("Start occupied");
@@ -324,6 +360,38 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
     _raw_plan_publisher->publish(plan);
   }
 
+  if (_debug_visualizations) {
+    // Publish expansions for debug
+    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_pose.orientation = getWorldOrientation(std::get<2>(e));
+      msg.poses.push_back(msg_pose);
+    }
+    _expansions_publisher->publish(msg);
+
+    // plot footprint path planned for debug
+    if (_planned_footprints_publisher->get_subscription_count() > 0) {
+      auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
+      for (size_t i = 0; i < plan.poses.size(); i++) {
+        const std::vector<geometry_msgs::msg::Point> edge =
+          transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint());
+        marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now()));
+      }
+
+      if (marker_array->markers.empty()) {
+        visualization_msgs::msg::Marker clear_all_marker;
+        clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
+        marker_array->markers.push_back(clear_all_marker);
+      }
+      _planned_footprints_publisher->publish(std::move(marker_array));
+    }
+  }
+
   // Find how much time we have left to do smoothing
   steady_clock::time_point b = steady_clock::now();
   duration<double> time_span = duration_cast<duration<double>>(b - a);

nav2_smac_planner/test/test_smac_hybrid.cpp

@@ -45,6 +45,7 @@ TEST(SmacTest, test_smac_se2)
 {
   rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 =
     std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSE2Test");
+  nodeSE2->declare_parameter("test.debug_visualizations", rclcpp::ParameterValue(true));
 
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
     std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");

nav2_smac_planner/test/test_smac_lattice.cpp

@@ -54,6 +54,7 @@ TEST(SmacTest, test_smac_lattice)
 {
   rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice =
     std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacLatticeTest");
+  nodeLattice->declare_parameter("test.debug_visualizations", rclcpp::ParameterValue(true));
 
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
     std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");