feat: add visualisations to debug NO_PATH_FOUND

include/smac_planner/node_hybrid.hpp

@@ -25,6 +25,7 @@
 #include <utility>
 #include <limits>
 
+#include "nav_msgs/OccupancyGrid.h"
 #include "ompl/base/StateSpace.h"
 
 #include "smac_planner/constants.hpp"
@@ -421,6 +422,8 @@ class NodeHybrid
     const Coordinates & goal_coords,
     const float & obstacle_heuristic);
 
+  static void initializeFootprintCollisionMap(const std::shared_ptr<costmap_2d::Costmap2DROS>& costmap_ros);
+
   /**
    * @brief reset the obstacle heuristic state
    * @param costmap_ros Costmap to use
@@ -462,6 +465,7 @@ class NodeHybrid
   static ObstacleHeuristicQueue obstacle_heuristic_queue;
 
   static costmap_2d::Costmap2DROS* costmap_ros;
+  static nav_msgs::OccupancyGrid footprint_collision_map;
   // Dubin / Reeds-Shepp lookup and size for dereferencing
   static LookupTable dist_heuristic_lookup_table;
   static float size_lookup;

include/smac_planner/node_lattice.hpp

@@ -27,6 +27,7 @@
 #include <limits>
 #include <string>
 
+#include "costmap_2d/costmap_2d_ros.h"
 #include "nlohmann/json.hpp"
 #include "ompl/base/StateSpace.h"
 #include "angles/angles.h"
@@ -347,6 +348,10 @@ class NodeLattice
     const unsigned int & dim_3_size,
     const SearchInfo & search_info);
 
+  static void initializeFootprintCollisionMap(const std::shared_ptr<costmap_2d::Costmap2DROS>& costmap_ros) {
+    NodeHybrid::initializeFootprintCollisionMap(costmap_ros);
+  }
+
   /**
    * @brief Compute the wavefront heuristic
    * @param costmap Costmap to use

include/smac_planner/smac_planner_hybrid.hpp

@@ -154,6 +154,7 @@ class SmacPlannerHybrid : public mbf_costmap_core::CostmapPlanner
   ros::Publisher _expansions_publisher;
   ros::Publisher _waypoint_publisher;
   ros::Publisher _collision_pub;
+  ros::Publisher _footprint_collision_pub;
   std::mutex _mutex;
 
   /**

src/a_star.cpp

@@ -25,6 +25,7 @@
 
 #include <geometry_msgs/Pose.h>
 #include <geometry_msgs/Point.h>
+#include "costmap_2d/costmap_2d_ros.h"
 #include "mbf_msgs/GetPathResult.h"
 #include "smac_planner/utils.hpp"
 
@@ -117,6 +118,7 @@ void AStarAlgorithm<NodeT>::setCollisionChecker(GridCollisionChecker * collision
     _y_size = y_size;
     NodeT::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info);
   }
+
   _expander->setCollisionChecker(_collision_checker);
 }
 
@@ -253,9 +255,9 @@ void AStarAlgorithm<NodeT>::setGoal(
     if (!_start) {
       throw std::runtime_error("Start must be set before goal.");
     }
-
+    const std::shared_ptr<costmap_2d::Costmap2DROS> costmap_2d_ros = _collision_checker->getCostmapROS();
     NodeT::resetObstacleHeuristic(
-      _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my);
+      costmap_2d_ros, _start->pose.x, _start->pose.y, mx, my);
   }
 
   _goal_coordinates = goal_coords;

src/node_hybrid.cpp

@@ -36,6 +36,7 @@ HybridMotionTable NodeHybrid::motion_table;
 float NodeHybrid::size_lookup = 25;
 LookupTable NodeHybrid::dist_heuristic_lookup_table;
 costmap_2d::Costmap2DROS* NodeHybrid::costmap_ros = nullptr;
+nav_msgs::OccupancyGrid NodeHybrid::footprint_collision_map;
 
 ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue;
 
@@ -365,13 +366,27 @@ bool NodeHybrid::isNodeValid(
   const bool & traverse_unknown,
   GridCollisionChecker * collision_checker)
 {
-  if (collision_checker->inCollision(
-      this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown))
-  {
+  unsigned int map_x = static_cast<unsigned int>(this->pose.x + 0.5f);
+  unsigned int map_y = static_cast<unsigned int>(this->pose.y + 0.5f);
+  bool coordinates_in_bounds = (map_x < NodeHybrid::footprint_collision_map.info.width &&
+                                map_y < NodeHybrid::footprint_collision_map.info.height);
+
+  unsigned int index = map_y * NodeHybrid::footprint_collision_map.info.width + map_x;
+  bool index_valid = coordinates_in_bounds && (index < NodeHybrid::footprint_collision_map.data.size());
+
+  if (collision_checker->inCollision(this->pose.x, this->pose.y, this->pose.theta, traverse_unknown)) {
+    if (index_valid) {
+      NodeHybrid::footprint_collision_map.data[index] = 100; // Mark as occupied
+    }
     return false;
   }
 
   _cell_cost = collision_checker->getCost();
+
+  if (index_valid && NodeHybrid::footprint_collision_map.data[index] != 0) {
+    NodeHybrid::footprint_collision_map.data[index] = _cell_cost; // Mark as free
+  }
+
   return true;
 }
 
@@ -471,6 +486,26 @@ inline float distanceHeuristic2D(
   return std::sqrt(dx * dx + dy * dy);
 }
 
+void NodeHybrid::initializeFootprintCollisionMap(const std::shared_ptr<costmap_2d::Costmap2DROS>& costmap_ros)
+{
+    const costmap_2d::Costmap2D* costmap = costmap_ros->getCostmap();
+    NodeHybrid::costmap_ros = costmap_ros.get();
+    NodeHybrid::footprint_collision_map.header.stamp = ros::Time::now();
+    NodeHybrid::footprint_collision_map.header.frame_id = costmap_ros->getGlobalFrameID();
+    NodeHybrid::footprint_collision_map.info.resolution = costmap->getResolution();
+    NodeHybrid::footprint_collision_map.info.width = costmap->getSizeInCellsX();
+    NodeHybrid::footprint_collision_map.info.height = costmap->getSizeInCellsY();
+    NodeHybrid::footprint_collision_map.info.origin.position.x = costmap->getOriginX();
+    NodeHybrid::footprint_collision_map.info.origin.position.y = costmap->getOriginY();
+    NodeHybrid::footprint_collision_map.info.origin.position.z = 0.0;
+    NodeHybrid::footprint_collision_map.info.origin.orientation.w = 1.0;
+
+    NodeHybrid::footprint_collision_map.data.assign(
+        NodeHybrid::footprint_collision_map.info.width *
+        NodeHybrid::footprint_collision_map.info.height,
+        -1);  // Initialize with -1 (unknown) initially
+}
+
 void NodeHybrid::resetObstacleHeuristic(
   const std::shared_ptr<costmap_2d::Costmap2DROS> & costmap_ros_i,
   const unsigned int & start_x, const unsigned int & start_y,

src/smac_planner_hybrid.cpp

@@ -24,6 +24,7 @@
 #include "mbf_msgs/GetPathResult.h"
 #include "nav_msgs/Path.h"
 #include "ros/console.h"
+#include "smac_planner/node_hybrid.hpp"
 #include "smac_planner/types.hpp"
 #include "smac_planner/utils.hpp"
 #include <base_local_planner/footprint_helper.h>
@@ -74,6 +75,7 @@ void SmacPlannerHybrid::initialize(
   _expansions_publisher = private_nh.advertise<geometry_msgs::PoseArray>("expansions", 1);
   _waypoint_publisher = private_nh.advertise<visualization_msgs::Marker>("waypoint_pose", 1);
   _collision_pub = private_nh.advertise<nav_msgs::OccupancyGrid>("collision_map", 1);
+  _footprint_collision_pub = private_nh.advertise<nav_msgs::OccupancyGrid>("footprint_collision_map", 1);
   _planned_footprints_publisher = private_nh.advertise<visualization_msgs::MarkerArray>(
       "planned_footprints", 1);
 
@@ -228,7 +230,7 @@ uint32_t SmacPlannerHybrid::makePlan(
   std::string &message)
 {
   _planning_canceled = false;
-
+  NodeHybrid::initializeFootprintCollisionMap(_costmap_ros);
   geometry_msgs::PoseStamped* waypoint_ptr = nullptr;
   BOOST_SCOPE_EXIT(&plan, &waypoint_ptr, this_) {
     this_->publishVisualisations(plan, waypoint_ptr);
@@ -350,6 +352,8 @@ void SmacPlannerHybrid::publishVisualisations(const std::vector<geometry_msgs::P
   if (waypoint_ptr) {
     Utils::publishArrowMarker(_waypoint_publisher, * waypoint_ptr, "goal_align_waypoint", 1);
   }
+
+  _footprint_collision_pub.publish(NodeHybrid::footprint_collision_map);
 }