Handle non-traversible goal

include/modules/impl/path_planner_impl.hpp

@@ -160,6 +160,24 @@ bool PathPlanner<P, M>::solvePath(
         }
     }
 
+    // check for unreachable destination or move goal to closest traversible point
+    this->kdtree.radiusSearch(goal_pt, this->search_radius, tmp_indices, tmp_dists);
+    bool all_invalid = true;
+    for(pcl::index_t i : tmp_indices)
+    {
+        if(meta_cloud.points[i].trav_weight() < 1.f)
+        {
+            goal_pt = loc_cloud.points[i];
+            all_invalid = false;
+            break;
+        }
+    }
+    if(all_invalid)
+    {
+        DEBUG_COUT("Error - goal is unreachable!");
+        return false;
+    }
+
     this->nodes.clear();
     this->nodes.reserve(loc_cloud.points.size());
 
@@ -186,6 +204,7 @@ bool PathPlanner<P, M>::solvePath(
     }
     else
     {
+        DEBUG_COUT("Error - could not initialize starting location!");
         return false;
     }
 

src/core/threads/path_planning_worker.cpp

@@ -55,6 +55,7 @@
 
 #include <util.hpp>
 #include <geometry.hpp>
+// #include <modules/map_octree.hpp>
 
 
 using namespace util::geom::cvt::ops;
@@ -65,6 +66,41 @@ using PathMsg = nav_msgs::msg::Path;
 using PointCloudMsg = sensor_msgs::msg::PointCloud2;
 
 
+// namespace csm::perception
+// {
+
+// template<typename Point_T, typename Meta_T>
+// class PlannningMap :
+//     public MapOctree<pcl::PointXYZI, MAP_OCTREE_DEFAULT, PlanningMap>
+// {
+//     static_assert(pcl::traits::has_xyz<Point_T>::value);
+//     static_assert(util::traits::has_trav_weight<Meta_T>::value);
+
+// private:
+//     using PointT = Point_T;
+//     using MetaT = Meta_T;
+//     using PointCloudT = pcl::PointCloud<PointT>;
+//     using MetaCloudT = pcl::PointCloud<MetaT>;
+
+//     using Vec3f = Eigen::Vector3f;
+
+// public:
+//     PlanningMap(double voxel_res);
+//     ~PlanningMap() = default;
+
+// public:
+//     void update(
+//         const PointCloudT& points,
+//         const MetaCloudT& points_meta,
+//         const Vec3f& bound_min,
+//         const Vec3f& bound_max);
+
+// protected:
+// };
+
+// };  // namespace csm::perception
+
+
 namespace csm
 {
 namespace perception