From be68097c811dbc63d778906d43ee58ce05f174bb Mon Sep 17 00:00:00 2001 From: S1ink Date: Fri, 2 Jan 2026 22:18:22 -0600 Subject: [PATCH 1/8] path planning map --- include/modules/accumulator_map.hpp | 80 -------- include/modules/impl/kfc_map_impl.hpp | 20 +- include/modules/impl/map_octree_impl.hpp | 106 +++++++---- include/modules/kfc_map.hpp | 34 ++-- include/modules/map_octree.hpp | 44 +++-- include/modules/path_plan_map.hpp | 227 +++++++++++++++++++++++ include/modules/selection_octree.hpp | 22 +-- include/modules/ue_octree.hpp | 30 ++- src/core/threads/mapping_worker.cpp | 6 +- 9 files changed, 387 insertions(+), 182 deletions(-) delete mode 100644 include/modules/accumulator_map.hpp create mode 100644 include/modules/path_plan_map.hpp diff --git a/include/modules/accumulator_map.hpp b/include/modules/accumulator_map.hpp deleted file mode 100644 index 06256eb..0000000 --- a/include/modules/accumulator_map.hpp +++ /dev/null @@ -1,80 +0,0 @@ -// /******************************************************************************* -// * Copyright (C) 2024-2026 Cardinal Space Mining Club * -// * * -// * ;xxxxxxx: * -// * ;$$$$$$$$$ ...::.. * -// * $$$$$$$$$$x .:::::::::::.. * -// * x$$$$$$$$$$$$$$::::::::::::::::. * -// * :$$$$$&X; .xX:::::::::::::.::... * -// * .$$Xx++$$$$+ :::. :;: .::::::. .... : * -// * :$$$$$$$$$ ;: ;xXXXXXXXx .::. .::::. .:. * -// * :$$$$$$$$: ; ;xXXXXXXXXXXXXx: ..:::::: .::. * -// * ;$$$$$$$$ :: :;XXXXXXXXXXXXXXXXXX+ .::::. .::: * -// * X$$$$$X : +XXXXXXXXXXXXXXXXXXXXXXXX; .:: .::::. * -// * .$$$$ :xXXXXXXXXXXXXXXXXXXXXXXXXXXX. .:::::. * -// * X$$X XXXXXXXXXXXXXXXXXXXXXXXXXXXXx: .::::. * -// * $$$:.XXXXXXXXXXXXXXXXXXXXXXXXXXX ;; ..:. * -// * $$& :XXXXXXXXXXXXXXXXXXXXXXXX; +XX; X$$; * -// * $$$: XXXXXXXXXXXXXXXXXXXXXX; :XXXXX; X$$; * -// * X$$X XXXXXXXXXXXXXXXXXXX; .+XXXXXXX; $$$ * -// * $$$$ ;XXXXXXXXXXXXXXX+ +XXXXXXXXx+ X$$$+ * -// * x$$$$$X ;XXXXXXXXXXX+ :xXXXXXXXX+ .;$$$$$$ * -// * +$$$$$$$$ ;XXXXXXx;;+XXXXXXXXX+ : +$$$$$$$$ * -// * +$$$$$$$$: xXXXXXXXXXXXXXX+ ; X$$$$$$$$ * -// * :$$$$$$$$$. +XXXXXXXXX; ;: x$$$$$$$$$ * -// * ;x$$$$XX$$$$+ .;+X+ :;: :$$$$$xX$$$X * -// * ;;;;;;;;;;X$$$$$$$+ :X$$$$$$&. * -// * ;;;;;;;:;;;;;x$$$$$$$$$$$$$$$$x. * -// * :;;;;;;;;;;;;. :$$$$$$$$$$X * -// * .;;;;;;;;:;; +$$$$$$$$$ * -// * .;;;;;;. X$$$$$$$: * -// * * -// * Unless required by applicable law or agreed to in writing, software * -// * distributed under the License is distributed on an "AS IS" BASIS, * -// * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * -// * See the License for the specific language governing permissions and * -// * limitations under the License. * -// * * -// *******************************************************************************/ - -// #pragma once - -// #include - -// #include - -// #include -// #include - -// #include - - -// namespace csm -// { -// namespace perception -// { - -// template -// class AccumulatorMap -// { -// using PointCloudT = pcl::PointCloud; - -// using Vec3f = Eigen::Vector3f; - -// public: -// AccumulatorMap(double voxel_size = 0.1); -// ~AccumulatorMap() = default; - -// public: -// void clear(); -// void append( -// const PointCloudT& pts, -// const Vec3f& bound_min, -// const Vec3f& bound_max); - -// protected: -// MapT map_octree; -// }; - -// }; // namespace perception -// }; // namespace csm diff --git a/include/modules/impl/kfc_map_impl.hpp b/include/modules/impl/kfc_map_impl.hpp index e68a9fa..0eba754 100644 --- a/include/modules/impl/kfc_map_impl.hpp +++ b/include/modules/impl/kfc_map_impl.hpp @@ -68,14 +68,12 @@ void KFCMap::applyParams( if (voxel_res > 0.) { - this->map_octree.setResolution(voxel_res); + this->map_octree.reconfigure(voxel_res); } } template -void KFCMap::setBounds( - const Vec3f& min, - const Vec3f& max) +void KFCMap::setBounds(const Vec3f& min, const Vec3f& max) { std::unique_lock lock{this->mtx}; @@ -87,12 +85,11 @@ void KFCMap::setBounds( template template -typename KFCMap::UpdateResult - KFCMap::updateMap( - const Vec3f& origin, - const pcl::PointCloud& pts, - const std::vector* inf_rays, - const pcl::Indices* pt_indices) +typename KFCMap::UpdateResult KFCMap::updateMap( + const Vec3f& origin, + const PointCloudT& pts, + const std::vector* inf_rays, + const pcl::Indices* pt_indices) { UpdateResult results{}; if (pt_indices && pt_indices->size() <= 0) @@ -135,8 +132,7 @@ typename KFCMap::UpdateResult // prepare submap range buffer if (!this->submap_ranges) { - this->submap_ranges = - std::make_shared>(); + this->submap_ranges = std::make_shared(); } this->submap_ranges->clear(); this->submap_ranges->reserve(buff.search_indices.size()); diff --git a/include/modules/impl/map_octree_impl.hpp b/include/modules/impl/map_octree_impl.hpp index 8170d3d..8aae79d 100644 --- a/include/modules/impl/map_octree_impl.hpp +++ b/include/modules/impl/map_octree_impl.hpp @@ -123,18 +123,57 @@ const std::vector>& -template -MapOctree::MapOctree(const double vox_res) : - Super_T(vox_res), - cloud_buff{std::make_shared()} +template +MapOctree::MapOctree(double vox_res) : + SuperT(vox_res), + cloud_buff{std::make_shared()} { this->input_ = this->cloud_buff; this->cloud_buff->is_dense = false; } -template -size_t MapOctree::addPoint( +template +void MapOctree::clear() +{ + this->deleteTree(); + this->cloud_buff->clear(); + this->hole_indices.clear(); + if constexpr (HAS_POINT_STAMPS) + { + this->pt_stamps.clear(); + } + if constexpr (HAS_POINT_NORMALS) + { + this->pt_normals.clear(); + } +} + +template +void MapOctree::reconfigure(double vox_res) +{ + this->clear(); + this->setResolution(vox_res); +} + +template +const typename MapOctree::PointCloudT& MapOctree::points() + const +{ + return *this->cloud_buff; +} + +template +typename MapOctree::PointT& MapOctree::pointAt(pcl::index_t pt_idx) +{ + const size_t pt_idx_ = static_cast(pt_idx); + assert(pt_idx_ < this->cloud_buff->size()); + + return (*this->cloud_buff)[pt_idx_]; +} + +template +size_t MapOctree::addPoint( const PointT& pt, uint64_t stamp, bool compute_normal) @@ -194,7 +233,7 @@ size_t MapOctree::addPoint( const size_t idx_ = pt_idx->getPointIndex(); auto& map_point = (*this->cloud_buff)[idx_]; - if (Derived_T::mergePointFields(map_point, pt)) + if (DerivedT::mergePointFields(map_point, pt)) { this->hole_indices.push_back(idx_); pt_idx->reset(); @@ -226,9 +265,9 @@ size_t MapOctree::addPoint( } } -template -void MapOctree::addPoints( - const pcl::PointCloud& pts, +template +void MapOctree::addPoints( + const PointCloudT& pts, const pcl::Indices* indices) { std::vector map_indices; @@ -283,10 +322,8 @@ void MapOctree::addPoints( } } -template -void MapOctree::deletePoint( - const pcl::index_t pt_idx, - bool trim_nodes) +template +void MapOctree::deletePoint(const pcl::index_t pt_idx, bool trim_nodes) { const size_t pt_idx_ = static_cast(pt_idx); assert(pt_idx_ < this->cloud_buff->size()); @@ -317,8 +354,8 @@ void MapOctree::deletePoint( } } -template -void MapOctree::deletePoints( +template +void MapOctree::deletePoints( const pcl::Indices& indices, bool trim_nodes) { @@ -329,8 +366,8 @@ void MapOctree::deletePoints( } -template -void MapOctree::crop( +template +void MapOctree::crop( const Vec3f& min, const Vec3f& max, bool trim_nodes) @@ -372,8 +409,8 @@ void MapOctree::crop( } } -template -void MapOctree::optimizeStorage() +template +void MapOctree::optimizeStorage() { // std::cout << "exhibit a" << std::endl; @@ -405,7 +442,7 @@ void MapOctree::optimizeStorage() { // std::cout << "exhibit d" << std::endl; - LeafContainer_T* pt_idx = nullptr; + LeafContainerT* pt_idx = nullptr; while (end_idx >= 0 && (!pcl::isFinite((*this->cloud_buff)[end_idx]) || !(pt_idx = @@ -470,8 +507,8 @@ void MapOctree::optimizeStorage() -template -bool MapOctree::mergePointFields( +template +bool MapOctree::mergePointFields( PointT& map_point, const PointT& new_point) { @@ -509,8 +546,8 @@ bool MapOctree::mergePointFields( return false; } -template -void MapOctree::computePointNormal(size_t idx) +template +void MapOctree::computePointNormal(size_t idx) { if constexpr (HAS_POINT_NORMALS) { @@ -545,19 +582,18 @@ void MapOctree::computePointNormal(size_t idx) } } -template -typename MapOctree::LeafContainer_T* - MapOctree::getOctreePoint( - const PointT& pt, - pcl::octree::OctreeKey& key) +template +typename MapOctree::LeafContainerT* MapOctree::getOctreePoint( + const PointT& pt, + pcl::octree::OctreeKey& key) { this->genOctreeKeyforPoint(pt, key); return this->findLeaf(key); } -template -typename MapOctree::LeafContainer_T* - MapOctree::getOrCreateOctreePoint( +template +typename MapOctree::LeafContainerT* + MapOctree::getOrCreateOctreePoint( const PointT& pt, pcl::octree::OctreeKey& key) { @@ -567,8 +603,8 @@ typename MapOctree::LeafContainer_T* // generate key this->genOctreeKeyforPoint(pt, key); - typename Super_T::LeafNode* leaf_node; - typename Super_T::BranchNode* parent_branch_of_leaf_node; + typename SuperT::LeafNode* leaf_node; + typename SuperT::BranchNode* parent_branch_of_leaf_node; auto depth_mask = this->createLeafRecursive( key, this->depth_mask_, diff --git a/include/modules/kfc_map.hpp b/include/modules/kfc_map.hpp index 4bb2f9b..191f60e 100644 --- a/include/modules/kfc_map.hpp +++ b/include/modules/kfc_map.hpp @@ -89,14 +89,19 @@ enum /** KDTree Frustum Collision (KFC) mapping implementation */ template< - typename PointT, - typename MapT = MapOctree> + typename Point_T, + typename Map_T = MapOctree> class KFCMap { - static_assert(pcl::traits::has_xyz::value); - static_assert(MapT::HAS_POINT_NORMALS); + static_assert(pcl::traits::has_xyz::value); + static_assert(Map_T::HAS_POINT_NORMALS); + + using PointT = Point_T; + using MapT = Map_T; + using PointCloudT = pcl::PointCloud; using CollisionPointT = pcl::PointXYZLNormal; + using CollisionPointCloudT = pcl::PointCloud; using Arr3f = Eigen::Array3f; using Vec3f = Eigen::Vector3f; @@ -146,7 +151,8 @@ class KFCMap * from the map. * @param add_max_range The maximum range for points that can be added to * the map. - * @param voxel_res The octree resolution. Values <= 0 result in no change. + * @param voxel_res The octree resolution. Values <= 0 result in no change. + * Note that if this is updated, the internal map gets cleared. */ void applyParams( double frustum_search_radius, @@ -162,7 +168,7 @@ class KFCMap template inline UpdateResult updateMap( const Vec3f& origin, - const pcl::PointCloud& pts, + const PointCloudT& pts, const pcl::Indices* indices = nullptr) { return this @@ -175,7 +181,7 @@ class KFCMap typename RayDirT = pcl::Axis> inline UpdateResult updateMap( const Vec3f& origin, - const pcl::PointCloud& pts, + const PointCloudT& pts, const std::vector& inf_rays, const pcl::Indices* pt_indices = nullptr) { @@ -188,29 +194,27 @@ class KFCMap pt_indices); } - inline typename pcl::PointCloud::ConstPtr getPoints() const + inline const PointCloudT& getPoints() const { - return this->map_octree.getInputCloud(); + return this->map_octree.points(); } - - inline const MapT& getMap() const { return this->map_octree; } - inline size_t numPoints() const { - return this->map_octree.getInputCloud()->size(); + return this->map_octree.points().size(); } + inline const MapT& getMap() const { return this->map_octree; } protected: template UpdateResult updateMap( const Vec3f& origin, - const pcl::PointCloud& pts, + const PointCloudT& pts, const std::vector* inf_rays, const pcl::Indices* pt_indices); protected: pcl::KdTreeFLANN collision_kdtree; - typename pcl::PointCloud::Ptr submap_ranges{nullptr}; + typename CollisionPointCloudT::Ptr submap_ranges{nullptr}; MapT map_octree; Arr3f bounds_min = Arr3f::Constant(-std::numeric_limits::infinity()); diff --git a/include/modules/map_octree.hpp b/include/modules/map_octree.hpp index 945c31b..148bbe5 100644 --- a/include/modules/map_octree.hpp +++ b/include/modules/map_octree.hpp @@ -133,11 +133,11 @@ class MapOctreeBase template< - typename PointT, + typename Point_T, int ConfigV = MAP_OCTREE_DEFAULT, - typename ChildT = void> + typename Child_T = void> class MapOctree : - public pcl::octree::OctreePointCloudSearch, + public pcl::octree::OctreePointCloudSearch, public std::conditional< (ConfigV & MAP_OCTREE_STORE_STAMPS), MapOctreeBase::StampStorageBase, @@ -147,21 +147,24 @@ class MapOctree : MapOctreeBase::NormalStorageBase, MapOctreeBase>::type { - static_assert(pcl::traits::has_xyz::value); + static_assert(pcl::traits::has_xyz::value); - using Super_T = pcl::octree::OctreePointCloudSearch; - using LeafContainer_T = typename Super_T::OctreeT::Base::LeafContainer; - using Derived_T = typename std::conditional< + using PointT = Point_T; + using ChildT = Child_T; + + using SuperT = pcl::octree::OctreePointCloudSearch; + using LeafContainerT = typename SuperT::OctreeT::Base::LeafContainer; + using DerivedT = typename std::conditional< std::is_same::value, MapOctree, ChildT>::type; - using typename Super_T::IndicesPtr; - using typename Super_T::IndicesConstPtr; + using PointCloudT = typename SuperT::PointCloud; - using typename Super_T::PointCloud; - using typename Super_T::PointCloudPtr; - using typename Super_T::PointCloudConstPtr; + using typename SuperT::IndicesPtr; + using typename SuperT::IndicesConstPtr; + using typename SuperT::PointCloudPtr; + using typename SuperT::PointCloudConstPtr; using Vec3f = Eigen::Vector3f; using Vec4f = Eigen::Vector4f; @@ -175,7 +178,7 @@ class MapOctree : (ConfigV & MAP_OCTREE_STORE_NORMALS); public: - MapOctree(const double voxel_res); + MapOctree(double voxel_res); MapOctree(const MapOctree&) = delete; ~MapOctree() = default; @@ -188,12 +191,19 @@ class MapOctree : } // TODO: invalidate other pcl::octree::OctreePointCloud point insertion methods + void clear(); + /* Note that this also clears the map */ + void reconfigure(double voxel_res); + + const PointCloudT& points() const; + PointT& pointAt(pcl::index_t i); + size_t addPoint( const PointT& pt, uint64_t stamp = 0, bool compute_normal = true); void addPoints( - const pcl::PointCloud& pts, + const PointCloudT& pts, const pcl::Indices* indices = nullptr); void deletePoint(const pcl::index_t pt_idx, bool trim_nodes = false); void deletePoints(const pcl::Indices& indices, bool trim_nodes = false); @@ -207,14 +217,14 @@ class MapOctree : void computePointNormal(size_t idx); - LeafContainer_T* getOctreePoint( + LeafContainerT* getOctreePoint( const PointT& pt, pcl::octree::OctreeKey& key); - LeafContainer_T* getOrCreateOctreePoint( + LeafContainerT* getOrCreateOctreePoint( const PointT& pt, pcl::octree::OctreeKey& key); - typename Super_T::PointCloudPtr cloud_buff; + PointCloudPtr cloud_buff; std::vector hole_indices; }; diff --git a/include/modules/path_plan_map.hpp b/include/modules/path_plan_map.hpp new file mode 100644 index 0000000..89aa037 --- /dev/null +++ b/include/modules/path_plan_map.hpp @@ -0,0 +1,227 @@ +/******************************************************************************* +* Copyright (C) 2024-2026 Cardinal Space Mining Club * +* * +* ;xxxxxxx: * +* ;$$$$$$$$$ ...::.. * +* $$$$$$$$$$x .:::::::::::.. * +* x$$$$$$$$$$$$$$::::::::::::::::. * +* :$$$$$&X; .xX:::::::::::::.::... * +* .$$Xx++$$$$+ :::. :;: .::::::. .... : * +* :$$$$$$$$$ ;: ;xXXXXXXXx .::. .::::. .:. * +* :$$$$$$$$: ; ;xXXXXXXXXXXXXx: ..:::::: .::. * +* ;$$$$$$$$ :: :;XXXXXXXXXXXXXXXXXX+ .::::. .::: * +* X$$$$$X : +XXXXXXXXXXXXXXXXXXXXXXXX; .:: .::::. * +* .$$$$ :xXXXXXXXXXXXXXXXXXXXXXXXXXXX. .:::::. * +* X$$X XXXXXXXXXXXXXXXXXXXXXXXXXXXXx: .::::. * +* $$$:.XXXXXXXXXXXXXXXXXXXXXXXXXXX ;; ..:. * +* $$& :XXXXXXXXXXXXXXXXXXXXXXXX; +XX; X$$; * +* $$$: XXXXXXXXXXXXXXXXXXXXXX; :XXXXX; X$$; * +* X$$X XXXXXXXXXXXXXXXXXXX; .+XXXXXXX; $$$ * +* $$$$ ;XXXXXXXXXXXXXXX+ +XXXXXXXXx+ X$$$+ * +* x$$$$$X ;XXXXXXXXXXX+ :xXXXXXXXX+ .;$$$$$$ * +* +$$$$$$$$ ;XXXXXXx;;+XXXXXXXXX+ : +$$$$$$$$ * +* +$$$$$$$$: xXXXXXXXXXXXXXX+ ; X$$$$$$$$ * +* :$$$$$$$$$. +XXXXXXXXX; ;: x$$$$$$$$$ * +* ;x$$$$XX$$$$+ .;+X+ :;: :$$$$$xX$$$X * +* ;;;;;;;;;;X$$$$$$$+ :X$$$$$$&. * +* ;;;;;;;:;;;;;x$$$$$$$$$$$$$$$$x. * +* :;;;;;;;;;;;;. :$$$$$$$$$$X * +* .;;;;;;;;:;; +$$$$$$$$$ * +* .;;;;;;. X$$$$$$$: * +* * +* Unless required by applicable law or agreed to in writing, software * +* distributed under the License is distributed on an "AS IS" BASIS, * +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * +* See the License for the specific language governing permissions and * +* limitations under the License. * +* * +*******************************************************************************/ + +#pragma once + +#include + +#include +#include + +#include +#include + +#include +#include + +#include "ue_octree.hpp" +#include "map_octree.hpp" + + +namespace csm +{ +namespace perception +{ + +template +class PathPlanMap +{ + static_assert(util::traits::supports_traversibility::value); + + using PointT = Point_T; + using PointCloudT = pcl::PointCloud; + + using UEOctreeT = UEOctree; + using MapOctreeT = MapOctree; + + using Vec3f = Eigen::Vector3f; + using Arr3f = Eigen::Array3f; + +public: + PathPlanMap(float voxel_size = 0.1f); + PathPlanMap(const PathPlanMap&) = delete; + ~PathPlanMap() = default; + +public: + void clear(); + void reconfigure( + float obstacle_merge_window, + float frontier_offset, + float voxel_size = -1.f); + + void append( + const PointCloudT& pts, + const Vec3f& bound_min, + const Vec3f& bound_max); + +protected: + UEOctreeT ue_octree; + MapOctreeT map_octree; + + float obstacle_merge_window{0.5f}; + float frontier_offset{0.25f}; +}; + + + +// --- Implementation ---------------------------------------------------------- + +template +PathPlanMap

::PathPlanMap(float vox_sz) : + ue_octree{vox_sz}, + map_octree{vox_sz} +{ +} + +template +void PathPlanMap

::clear() +{ + this->ue_octree.clear(); + this->map_octree.clear(); +} + +template +void PathPlanMap

::reconfigure( + float obstacle_merge_window, + float frontier_offset, + float voxel_size) +{ + this->obstacle_merge_window = obstacle_merge_window; + this->frontier_offset = frontier_offset; + + if (voxel_size > 0.f) + { + this->ue_octree.reconfigure(voxel_size); + this->map_octree.reconfigure(voxel_size); + } +} + +template +void PathPlanMap

::append( + const PointCloudT& pts, + const Vec3f& bound_min, + const Vec3f& bound_max) +{ + using namespace csm::perception::traversibility; + + pcl::Indices tmp_indices; + typename PointCloudT::VectorType merge_pts; + + const auto& map_pts_vec = this->map_octree.getInputCloud()->points; + + // 1. If the update region is large enough to have space that doesn't + // intersect the merge window, then remove it first. + if ((bound_max.array() - bound_min.array()) > + Arr3f::Constant(this->obstacle_merge_window * 2.f).all()) + { + Vec3f inner_min = + bound_min + Vec3f::Constant(this->obstacle_merge_window); + Vec3f inner_max = + bound_max - Vec3f::Constant(this->obstacle_merge_window); + + this->map_octree.boxSearch(inner_min, inner_max, tmp_indices); + this->map_octree.deletePoints(tmp_indices, false); + this->tmp_indices.clear(); + } + + // 2. Copy out merge-window points, then remove from map + this->map_octree.boxSearch(bound_min, bound_max, tmp_indices); + merge_pts.reserve(tmp_indices.size()); + util::copySelection(map_pts_vec, tmp_indices, merge_pts); + this->map_octree.deletePoints(tmp_indices); + tmp_indices.clear(); + + // 3. Insert new points + this->map_octree.addPoints(pts); + + // 4. Update trav scores for old-new intersecting voxels (in window) + for (const auto& pt : merge_pts) + { + if ((isWeighted(pt) || isObstacle(pt)) && + this->map_octree.voxelSearch(pt, tmp_indices)) + { + PointT& new_pt = this->map_octree.pointAt(tmp_indices[0]); + if (!isFrontier(new_pt) && weight(pt) > weight(new_pt)) + { + weight(new_pt) = weight(pt); + } + } + tmp_indices.clear(); + } + + // 5. Update u-e octree, insert new frontier points + this->ue_octree.addExploredSpace(bounds_min, bounds_max); + + const float res = this->ue_octree.resolution(); + + const Arr3f length3 = (bounds_max - bounds_min).array(); + const Arr3f innerlen3 = (length3 / res).floor() * res; + const Arr3f margin3 = (length3 - innerlen3) * 0.5f; + const Vec3f start3 = bounds_min + margin3.matrix(); + + PointT a{}, b{}; + weight(a) = weight(b) = FRONTIER_MARKER_VAL; + +#define EXPLORE_PARALLEL_SIDES(U, V, W) \ + a.U = bounds_min.U() - this->frontier_offset; \ + b.U = bounds_max.U() + this->frontier_offset; \ + for (a.V = b.V = start3.V(); a.V < bounds_max.V(); a.V = (b.V += res)) \ + { \ + for (a.W = b.W = start3.W(); a.W < bounds_max.W(); a.W = (b.W += res)) \ + { \ + if (!this->ue_octree.isExplored(a)) \ + { \ + this->map_octree.addPoint(a); \ + } \ + if (!this->ue_octree.isExplored(b)) \ + { \ + this->map_octree.addPoint(b); \ + } \ + } \ + } + + EXPLORE_PARALLEL_SIDES(x, y, z) + EXPLORE_PARALLEL_SIDES(y, x, z) + EXPLORE_PARALLEL_SIDES(z, x, y) + +#undef EXPLORE_PARALLEL_SIDES +} + +}; // namespace perception +}; // namespace csm diff --git a/include/modules/selection_octree.hpp b/include/modules/selection_octree.hpp index d17e214..e9fe92f 100644 --- a/include/modules/selection_octree.hpp +++ b/include/modules/selection_octree.hpp @@ -61,20 +61,20 @@ class SelectionOctree : { static_assert(pcl::traits::has_xyz::value); - using Super_T = pcl::octree::OctreePointCloudSearch< + using SuperT = pcl::octree::OctreePointCloudSearch< PointT, pcl::octree::OctreeContainerPointIndices>; - using LeafContainer_T = typename Super_T::OctreeT::Base::LeafContainer; + using LeafContainerT = typename SuperT::OctreeT::Base::LeafContainer; - using typename Super_T::IndicesPtr; - using typename Super_T::IndicesConstPtr; + using typename SuperT::IndicesPtr; + using typename SuperT::IndicesConstPtr; - using typename Super_T::PointCloud; - using typename Super_T::PointCloudPtr; - using typename Super_T::PointCloudConstPtr; + using typename SuperT::PointCloud; + using typename SuperT::PointCloudPtr; + using typename SuperT::PointCloudConstPtr; public: - inline SelectionOctree(const double voxel_res) : Super_T(voxel_res) {} + inline SelectionOctree(const double voxel_res) : SuperT(voxel_res) {} void initPoints( const PointCloudConstPtr& cloud, @@ -94,13 +94,13 @@ void SelectionOctree::initPoints( { if (this->input_) { - Super_T:: + SuperT:: deleteTree(); // remove old tree // indices get reset when they are copied } - Super_T::setInputCloud(cloud, indices); - Super_T::addPointsFromInputCloud(); + SuperT::setInputCloud(cloud, indices); + SuperT::addPointsFromInputCloud(); } template diff --git a/include/modules/ue_octree.hpp b/include/modules/ue_octree.hpp index c972a34..b6aee3f 100644 --- a/include/modules/ue_octree.hpp +++ b/include/modules/ue_octree.hpp @@ -55,6 +55,7 @@ namespace perception template class UEOctree { +public: using FloatT = Float_T; using IndexT = std::make_unsigned::type; @@ -73,10 +74,14 @@ class UEOctree ~UEOctree() = default; public: - void addExploredSpace(const Vec3f& min, const Vec3f& max); void clear(); + void reconfigure(FloatT max_res); + + void addExploredSpace(const Vec3f& min, const Vec3f& max); bool isExplored(const Vec3f& pt); + inline FloatT resolution() const { return vox_res; } + protected: struct Node { @@ -135,6 +140,21 @@ UEOctree::UEOctree(FloatT res) : vox_res{res} { } +template +void UEOctree::clear() +{ + this->root.clear(); + this->root_height = 0; + this->vox_span = 0; +} + +template +void UEOctree::reconfigure(FloatT res) +{ + this->clear(); + this->vox_res = res; +} + template void UEOctree::addExploredSpace(const Vec3f& min, const Vec3f& max) { @@ -154,14 +174,6 @@ void UEOctree::addExploredSpace(const Vec3f& min, const Vec3f& max) } } -template -void UEOctree::clear() -{ - this->root.clear(); - this->root_height = 0; - this->vox_span = 0; -} - template bool UEOctree::isExplored(const Vec3f& pt) { diff --git a/src/core/threads/mapping_worker.cpp b/src/core/threads/mapping_worker.cpp index e49347d..4228bbe 100644 --- a/src/core/threads/mapping_worker.cpp +++ b/src/core/threads/mapping_worker.cpp @@ -245,7 +245,7 @@ void MappingWorker::mapping_callback(MappingResources& buff) x.lidar_to_base = buff.lidar_to_base; x.base_to_odom = buff.base_to_odom; util::copySelection( - *this->sparse_map.getPoints(), + this->sparse_map.getPoints(), export_points, x.points); x.stamp = util::toFloatSeconds(buff.raw_scan->header.stamp); @@ -257,7 +257,7 @@ void MappingWorker::mapping_callback(MappingResources& buff) { thread_local pcl::PointCloud trav_points; util::copySelection( - *this->sparse_map.getPoints(), + this->sparse_map.getPoints(), export_points, trav_points); @@ -289,7 +289,7 @@ void MappingWorker::mapping_callback(MappingResources& buff) try { pcl::PointCloud output_buff; - const auto& map_pts = *this->sparse_map.getPoints(); + const auto& map_pts = this->sparse_map.getPoints(); const auto& map_norms = this->sparse_map.getMap().pointNormals(); output_buff.points.resize(map_pts.size()); From 19fcee34b543ae656d82be3dbce62f815a0b941f Mon Sep 17 00:00:00 2001 From: S1ink Date: Sun, 4 Jan 2026 21:35:42 -0600 Subject: [PATCH 2/8] back in stupid town --- include/modules/impl/path_planner_impl.hpp | 504 +++++++++++---------- include/modules/map_octree.hpp | 8 +- include/modules/path_plan_map.hpp | 77 ++-- include/modules/path_planner.hpp | 44 +- include/modules/ue_octree.hpp | 89 +++- src/core/modules/map_octree.cpp | 3 + src/core/threads/path_planning_worker.cpp | 80 ++-- src/core/threads/path_planning_worker.hpp | 5 +- src/core/threads/shared_resources.hpp | 2 - 9 files changed, 493 insertions(+), 319 deletions(-) diff --git a/include/modules/impl/path_planner_impl.hpp b/include/modules/impl/path_planner_impl.hpp index 82f7c7d..c7c5777 100644 --- a/include/modules/impl/path_planner_impl.hpp +++ b/include/modules/impl/path_planner_impl.hpp @@ -69,7 +69,6 @@ namespace perception template PathPlanner

::Node::Node(const P& point, float h, Node* p) : trav_point(point), - cost(weight(point)), g(std::numeric_limits::infinity()), h(h), parent(p) @@ -100,243 +99,286 @@ void PathPlanner

::setParameters( } template -bool PathPlanner

::solvePath( +bool PathPlanner

::solveBoundedPath( + std::vector& path, const Vec3f& start, const Vec3f& goal, - const Vec3f& local_bound_min, - const Vec3f& local_bound_max, - const PointCloudT& trav_points, - std::vector& path) + const Vec3f& bound_min, + const Vec3f& bound_max, + const PointCloudT& points, + const WeightT max_weight) { -#if PATH_PLANNING_PEDANTIC - if (start.x() < local_bound_min.x() || start.y() < local_bound_min.y() || - start.x() > local_bound_max.x() || start.y() > local_bound_max.y()) - { - throw std::out_of_range("Start point is out of bounds"); - } - if (trav_points.points.empty()) - { - throw std::invalid_argument("Input point cloud are empty"); - } -#endif + return false; +} + +template +bool PathPlanner

::solveFrontierPath( + std::vector& path, + const Vec3f& start, + const Vec3f& goal, + const PointCloudT& points, + const WeightT max_weight) +{ + // pcl::Indices tmp_indices; + // std::vector tmp_dists; + + // this->pt_selection.clear(); + // this->pt_selection.reserve(points.size()); + // for (size_t i = 0; i < points.size(); i++) + // { + // const auto& pt = points[i]; + // if (isWeighted(pt) && weight(pt) <= max_weight || isFrontier(pt)) + // { + // this->pt_selection.push_back(i); + // } + // } + + // const auto points_ptr = util::wrapUnmanaged(points); + // const auto selection_ptr = util::wrapUnmanaged(this->pt_selection); + // this->kdtree.setInputCloud(points_ptr, selection_ptr); - // temp buffers reused by radiusSearch - pcl::Indices tmp_indices; - std::vector tmp_dists; - - auto shared_trav_points = util::wrapUnmanaged(trav_points); - this->kdtree.setInputCloud(shared_trav_points); - - PointT goal_pt; - goal_pt.getVector3fMap() = goal; - // handle goal point isn't reachable given goal thresh - if ((goal.array() >= local_bound_min.array()).all() && - (goal.array() < local_bound_max.array()).all()) - { - if (!this->kdtree.radiusSearch( - goal_pt, - this->goal_threshold, - tmp_indices, - tmp_dists, - 1)) - { - if (this->kdtree.nearestKSearch(goal_pt, 1, tmp_indices, tmp_dists)) - { - goal_pt = trav_points.points[tmp_indices[0]]; - if (std::sqrt(tmp_dists[0]) > this->search_radius) - { - DEBUG_COUT( - "Warning - path planning goal node snapped to available" - " node with distance greater than search radius!"); - } - } - else - { - return false; - } - } - } - - // 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(isNominal(trav_points.points[i])) - { - goal_pt = trav_points.points[i]; - all_invalid = false; - break; - } - } - if(all_invalid) - { - DEBUG_COUT("Error - goal is unreachable!"); - return false; - } - - this->nodes.clear(); - this->nodes.reserve(trav_points.points.size()); - - // Heuristic = lambda_d * Euclidean distance - for (size_t i = 0; i < trav_points.points.size(); ++i) - { - const auto& point = trav_points.points[i]; - float h = lambda_dist * - (goal_pt.getVector3fMap() - point.getVector3fMap()).norm(); - this->nodes.emplace_back(point, h); - } - - // find start node - int start_idx; - if (this->kdtree.nearestKSearch( - PointT{start.x(), start.y(), start.z()}, - 1, - tmp_indices, - tmp_dists)) - { - start_idx = tmp_indices[0]; - this->nodes[start_idx].g = 0.f; - } - else - { - DEBUG_COUT("Error - could not initialize starting location!"); - return false; - } - - // create open heap over f = g + h - util::DAryHeap open(static_cast(this->nodes.size())); - open.reserve_heap(this->nodes.size()); - open.push(start_idx, this->nodes[start_idx].f()); - - // closed set - std::vector closed(this->nodes.size(), false); - - // keep track of closest visited node to goal - int closest_idx = start_idx; - // h proportional to distance to goal - float closest_dist = this->nodes[start_idx].h; - bool found_boundary = false; - - const Box3f outside_boundary{ - local_bound_min + Vec3f::Constant(boundary_radius), - local_bound_max - Vec3f::Constant(boundary_radius)}; - - auto create_path = [&](Node& path_end) - { - path.clear(); - for (Node* n = &path_end; n != nullptr; n = n->parent) - { - path.push_back(n->position()); - } - std::reverse(path.begin(), path.end()); - }; - - while (!open.empty()) - { - const int idx = open.pop(); - Node& current = this->nodes[idx]; - - if (closed[idx]) - { - continue; // skip if already closed - } - closed[idx] = true; - - // Goal check (by geometry) - if ((current.position() - goal_pt.getVector3fMap()).norm() < - goal_threshold) - { - create_path(current); - return true; - } - - // Lazily compute neighbors on first expansion - if (current.neighbors.empty()) - { - current.neighbors.clear(); - tmp_dists.clear(); - this->kdtree.radiusSearch( - current.trav_point, - search_radius, - current.neighbors, - tmp_dists, - max_neighbors); - } - - // Relax neighbors - for (const int nb_idx : current.neighbors) - { - if (closed[nb_idx]) - { - continue; - } - else if (this->nodes[nb_idx].cost > 1.f) - { - closed[nb_idx] = true; - continue; - } - Node& nb = this->nodes[nb_idx]; - - // geometric edge length - const float geom = (nb.position() - current.position()).norm(); - - const float edge = lambda_dist * geom + lambda_penalty * nb.cost; - - const float tentative_g = current.g + edge; - if (tentative_g < nb.g) - { - nb.g = tentative_g; - nb.parent = ¤t; - - bool in_boundary = !outside_boundary.contains(nb.position()); - - if (!found_boundary && in_boundary) - { - closest_dist = nb.h; - closest_idx = nb_idx; - found_boundary = true; - } - else if (nb.h < closest_dist) - { - if (!found_boundary || in_boundary) - { - closest_dist = nb.h; - closest_idx = nb_idx; - } - } - - const float new_f = nb.f(); - if (!open.contains(nb_idx)) - { - open.push(nb_idx, new_f); - } - else - { - open.decrease_key(nb_idx, new_f); // strictly better - } - } - } - } - - // No path to goal: pick closest visited node to the goal - if (closest_idx >= 0) - { - create_path(this->nodes[closest_idx]); - - if (!found_boundary) - { - DEBUG_COUT( - "No boundary nodes reached, returning closest node to goal"); - } - return found_boundary; - } - - // Nothing reachable at all - DEBUG_COUT("No reachable nodes; start may be isolated"); return false; } +// template +// bool PathPlanner

::solvePath( +// const Vec3f& start, +// const Vec3f& goal, +// const Vec3f& local_bound_min, +// const Vec3f& local_bound_max, +// const PointCloudT& trav_points, +// std::vector& path) +// { +// #if PATH_PLANNING_PEDANTIC +// if (start.x() < local_bound_min.x() || start.y() < local_bound_min.y() || +// start.x() > local_bound_max.x() || start.y() > local_bound_max.y()) +// { +// throw std::out_of_range("Start point is out of bounds"); +// } +// if (trav_points.points.empty()) +// { +// throw std::invalid_argument("Input point cloud are empty"); +// } +// #endif + +// // temp buffers reused by radiusSearch +// pcl::Indices tmp_indices; +// std::vector tmp_dists; + +// auto shared_trav_points = util::wrapUnmanaged(trav_points); +// this->kdtree.setInputCloud(shared_trav_points); + +// PointT goal_pt; +// goal_pt.getVector3fMap() = goal; +// // handle goal point isn't reachable given goal thresh +// if ((goal.array() >= local_bound_min.array()).all() && +// (goal.array() < local_bound_max.array()).all()) +// { +// if (!this->kdtree.radiusSearch( +// goal_pt, +// this->goal_threshold, +// tmp_indices, +// tmp_dists, +// 1)) +// { +// if (this->kdtree.nearestKSearch(goal_pt, 1, tmp_indices, tmp_dists)) +// { +// goal_pt = trav_points.points[tmp_indices[0]]; +// if (std::sqrt(tmp_dists[0]) > this->search_radius) +// { +// DEBUG_COUT( +// "Warning - path planning goal node snapped to available" +// " node with distance greater than search radius!"); +// } +// } +// else +// { +// return false; +// } +// } +// } + +// // 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 (isNominal(trav_points.points[i])) +// { +// goal_pt = trav_points.points[i]; +// all_invalid = false; +// break; +// } +// } +// if (all_invalid) +// { +// DEBUG_COUT("Error - goal is unreachable!"); +// return false; +// } + +// this->nodes.clear(); +// this->nodes.reserve(trav_points.points.size()); + +// // Heuristic = lambda_d * Euclidean distance +// for (size_t i = 0; i < trav_points.points.size(); ++i) +// { +// const auto& point = trav_points.points[i]; +// float h = lambda_dist * +// (goal_pt.getVector3fMap() - point.getVector3fMap()).norm(); +// this->nodes.emplace_back(point, h); +// } + +// // find start node +// int start_idx; +// if (this->kdtree.nearestKSearch( +// PointT{start.x(), start.y(), start.z()}, +// 1, +// tmp_indices, +// tmp_dists)) +// { +// start_idx = tmp_indices[0]; +// this->nodes[start_idx].g = 0.f; +// } +// else +// { +// DEBUG_COUT("Error - could not initialize starting location!"); +// return false; +// } + +// // create open heap over f = g + h +// util::DAryHeap open(static_cast(this->nodes.size())); +// open.reserve_heap(this->nodes.size()); +// open.push(start_idx, this->nodes[start_idx].f()); + +// // closed set +// std::vector closed(this->nodes.size(), false); + +// // keep track of closest visited node to goal +// int closest_idx = start_idx; +// // h proportional to distance to goal +// float closest_dist = this->nodes[start_idx].h; +// bool found_boundary = false; + +// const Box3f outside_boundary{ +// local_bound_min + Vec3f::Constant(boundary_radius), +// local_bound_max - Vec3f::Constant(boundary_radius)}; + +// auto create_path = [&](Node& path_end) +// { +// path.clear(); +// for (Node* n = &path_end; n != nullptr; n = n->parent) +// { +// path.push_back(n->position()); +// } +// std::reverse(path.begin(), path.end()); +// }; + +// while (!open.empty()) +// { +// const int idx = open.pop(); +// Node& current = this->nodes[idx]; + +// if (closed[idx]) +// { +// continue; // skip if already closed +// } +// closed[idx] = true; + +// // Goal check (by geometry) +// if ((current.position() - goal_pt.getVector3fMap()).norm() < +// goal_threshold) +// { +// create_path(current); +// return true; +// } + +// // Lazily compute neighbors on first expansion +// if (current.neighbors.empty()) +// { +// current.neighbors.clear(); +// tmp_dists.clear(); +// this->kdtree.radiusSearch( +// current.trav_point, +// search_radius, +// current.neighbors, +// tmp_dists, +// max_neighbors); +// } + +// // Relax neighbors +// for (const int nb_idx : current.neighbors) +// { +// if (closed[nb_idx]) +// { +// continue; +// } +// else if (this->nodes[nb_idx].cost() > 1.f) +// { +// closed[nb_idx] = true; +// continue; +// } +// Node& nb = this->nodes[nb_idx]; + +// // geometric edge length +// const float geom = (nb.position() - current.position()).norm(); + +// const float edge = lambda_dist * geom + lambda_penalty * nb.cost(); + +// const float tentative_g = current.g + edge; +// if (tentative_g < nb.g) +// { +// nb.g = tentative_g; +// nb.parent = ¤t; + +// bool in_boundary = !outside_boundary.contains(nb.position()); + +// if (!found_boundary && in_boundary) +// { +// closest_dist = nb.h; +// closest_idx = nb_idx; +// found_boundary = true; +// } +// else if (nb.h < closest_dist) +// { +// if (!found_boundary || in_boundary) +// { +// closest_dist = nb.h; +// closest_idx = nb_idx; +// } +// } + +// const float new_f = nb.f(); +// if (!open.contains(nb_idx)) +// { +// open.push(nb_idx, new_f); +// } +// else +// { +// open.decrease_key(nb_idx, new_f); // strictly better +// } +// } +// } +// } + +// // No path to goal: pick closest visited node to the goal +// if (closest_idx >= 0) +// { +// create_path(this->nodes[closest_idx]); + +// if (!found_boundary) +// { +// DEBUG_COUT( +// "No boundary nodes reached, returning closest node to goal"); +// } +// return found_boundary; +// } + +// // Nothing reachable at all +// DEBUG_COUT("No reachable nodes; start may be isolated"); +// return false; +// } + }; // namespace perception }; // namespace csm diff --git a/include/modules/map_octree.hpp b/include/modules/map_octree.hpp index 148bbe5..a81efa4 100644 --- a/include/modules/map_octree.hpp +++ b/include/modules/map_octree.hpp @@ -99,6 +99,10 @@ enum class MapOctreeBase { +private: + class Empty1 {}; + class Empty2 {}; + private: class StampStorageBase { @@ -141,11 +145,11 @@ class MapOctree : public std::conditional< (ConfigV & MAP_OCTREE_STORE_STAMPS), MapOctreeBase::StampStorageBase, - MapOctreeBase>::type, + MapOctreeBase::Empty1>::type, public std::conditional< (ConfigV & MAP_OCTREE_STORE_NORMALS), MapOctreeBase::NormalStorageBase, - MapOctreeBase>::type + MapOctreeBase::Empty2>::type { static_assert(pcl::traits::has_xyz::value); diff --git a/include/modules/path_plan_map.hpp b/include/modules/path_plan_map.hpp index 89aa037..8690fac 100644 --- a/include/modules/path_plan_map.hpp +++ b/include/modules/path_plan_map.hpp @@ -90,6 +90,10 @@ class PathPlanMap const Vec3f& bound_min, const Vec3f& bound_max); + void crop(const Vec3f& min, const Vec3f& max); + + const PointCloudT& getPoints() const; + protected: UEOctreeT ue_octree; MapOctreeT map_octree; @@ -140,6 +144,8 @@ void PathPlanMap

::append( { using namespace csm::perception::traversibility; + const float res = this->ue_octree.resolution(); + pcl::Indices tmp_indices; typename PointCloudT::VectorType merge_pts; @@ -147,8 +153,9 @@ void PathPlanMap

::append( // 1. If the update region is large enough to have space that doesn't // intersect the merge window, then remove it first. - if ((bound_max.array() - bound_min.array()) > - Arr3f::Constant(this->obstacle_merge_window * 2.f).all()) + if (((bound_max.array() - bound_min.array()) > + Arr3f::Constant(this->obstacle_merge_window * 2.f)) + .all()) { Vec3f inner_min = bound_min + Vec3f::Constant(this->obstacle_merge_window); @@ -157,16 +164,21 @@ void PathPlanMap

::append( this->map_octree.boxSearch(inner_min, inner_max, tmp_indices); this->map_octree.deletePoints(tmp_indices, false); - this->tmp_indices.clear(); + tmp_indices.clear(); } // 2. Copy out merge-window points, then remove from map - this->map_octree.boxSearch(bound_min, bound_max, tmp_indices); + this->map_octree.boxSearch( + bound_min + Vec3f::Constant(res), + bound_max - Vec3f::Constant(res), + tmp_indices); merge_pts.reserve(tmp_indices.size()); util::copySelection(map_pts_vec, tmp_indices, merge_pts); this->map_octree.deletePoints(tmp_indices); tmp_indices.clear(); + // 2.5. Remove frontier nodes in extended box region + // 3. Insert new points this->map_octree.addPoints(pts); @@ -186,41 +198,54 @@ void PathPlanMap

::append( } // 5. Update u-e octree, insert new frontier points - this->ue_octree.addExploredSpace(bounds_min, bounds_max); - - const float res = this->ue_octree.resolution(); + this->ue_octree.addExploredSpace(bound_min, bound_max); - const Arr3f length3 = (bounds_max - bounds_min).array(); + const Arr3f length3 = (bound_max - bound_min).array(); const Arr3f innerlen3 = (length3 / res).floor() * res; const Arr3f margin3 = (length3 - innerlen3) * 0.5f; - const Vec3f start3 = bounds_min + margin3.matrix(); + const Vec3f start3 = bound_min + margin3.matrix(); PointT a{}, b{}; weight(a) = weight(b) = FRONTIER_MARKER_VAL; -#define EXPLORE_PARALLEL_SIDES(U, V, W) \ - a.U = bounds_min.U() - this->frontier_offset; \ - b.U = bounds_max.U() + this->frontier_offset; \ - for (a.V = b.V = start3.V(); a.V < bounds_max.V(); a.V = (b.V += res)) \ - { \ - for (a.W = b.W = start3.W(); a.W < bounds_max.W(); a.W = (b.W += res)) \ - { \ - if (!this->ue_octree.isExplored(a)) \ - { \ - this->map_octree.addPoint(a); \ - } \ - if (!this->ue_octree.isExplored(b)) \ - { \ - this->map_octree.addPoint(b); \ - } \ - } \ +#define EXPLORE_PARALLEL_SIDES(U, V, W) \ + a.U = bound_min.U() - this->frontier_offset; \ + b.U = bound_max.U() + this->frontier_offset; \ + for (a.V = b.V = start3.V(); a.V < bound_max.V(); a.V = (b.V += res)) \ + { \ + for (a.W = b.W = start3.W(); a.W < bound_max.W(); a.W = (b.W += res)) \ + { \ + if (!this->ue_octree.isExplored(a.getVector3fMap())) \ + { \ + this->map_octree.addPoint(a); \ + } \ + if (!this->ue_octree.isExplored(b.getVector3fMap())) \ + { \ + this->map_octree.addPoint(b); \ + } \ + } \ } EXPLORE_PARALLEL_SIDES(x, y, z) EXPLORE_PARALLEL_SIDES(y, x, z) - EXPLORE_PARALLEL_SIDES(z, x, y) + // EXPLORE_PARALLEL_SIDES(z, x, y) #undef EXPLORE_PARALLEL_SIDES + + this->map_octree.optimizeStorage(); +} + +template +void PathPlanMap

::crop(const Vec3f& min, const Vec3f& max) +{ + this->ue_octree.crop(min, max); + this->map_octree.crop(min, max); +} + +template +const typename PathPlanMap

::PointCloudT& PathPlanMap

::getPoints() const +{ + return this->map_octree.points(); } }; // namespace perception diff --git a/include/modules/path_planner.hpp b/include/modules/path_planner.hpp index d6ff09f..caeb2c1 100644 --- a/include/modules/path_planner.hpp +++ b/include/modules/path_planner.hpp @@ -73,6 +73,8 @@ class PathPlanner using PointT = Point_T; using PointCloudT = pcl::PointCloud; + using WeightT = traversibility::weight_t; + using Vec3f = Eigen::Vector3f; using Box3f = Eigen::AlignedBox3f; @@ -80,19 +82,24 @@ class PathPlanner struct Node { const PointT& trav_point; - float cost; // traversal cost of this node only - float g; // cost from start to this node - float h; // heuristic cost to goal + float g; // cost from start to this node + float h; // heuristic cost to goal Node* parent = nullptr; pcl::Indices neighbors; - Node( - const PointT& point, - float h = 0.0f, - Node* p = nullptr); - - inline float f() const { return g + h; } // total cost - inline auto position() const { return trav_point.getVector3fMap(); } + Node(const PointT& point, float h = 0.0f, Node* p = nullptr); + + // total cost + inline float f() const { return this->g + this->h; } + // cost of this node + inline WeightT cost() const + { + return traversibility::weight(this->trav_point); + } + inline auto position() const + { + return this->trav_point.getVector3fMap(); + } }; public: @@ -107,24 +114,35 @@ class PathPlanner float lambda_penalty, size_t max_neighbors = 10); - bool solvePath( + bool solveBoundedPath( + std::vector& path, const Vec3f& start, const Vec3f& goal, const Vec3f& local_bound_min, const Vec3f& local_bound_max, const PointCloudT& trav_points, - std::vector& path); + const WeightT max_weight = + traversibility::NOMINAL_MAX_WEIGHT); + + bool solveFrontierPath( + std::vector& path, + const Vec3f& start, + const Vec3f& goal, + const PointCloudT& trav_points, + const WeightT max_weight = + traversibility::NOMINAL_MAX_WEIGHT); private: pcl::search::KdTree kdtree; std::vector nodes; // all nodes in the search space + pcl::Indices pt_selection; // Dist. from search space edge for boundary nodes float boundary_radius = 0.15f; // threshold for considering goal reached float goal_threshold = 0.1f; // radius for neighbor search - float search_radius = 1.0f; + float search_radius = 0.5f; // weights for cost model: edge_cost = lambda_d * dist + lambda_p * penalty float lambda_dist = 1.f; float lambda_penalty = 1.f; diff --git a/include/modules/ue_octree.hpp b/include/modules/ue_octree.hpp index b6aee3f..9f6a586 100644 --- a/include/modules/ue_octree.hpp +++ b/include/modules/ue_octree.hpp @@ -78,7 +78,8 @@ class UEOctree void reconfigure(FloatT max_res); void addExploredSpace(const Vec3f& min, const Vec3f& max); - bool isExplored(const Vec3f& pt); + void crop(const Vec3f& min, const Vec3f& max); + bool isExplored(const Vec3f& pt) const; inline FloatT resolution() const { return vox_res; } @@ -121,6 +122,10 @@ class UEOctree Node& node, const NodeDescriptor& descriptor, const Box3f& zone); + void recursiveCrop( + Node& node, + const NodeDescriptor& descriptor, + const Box3f& zone); protected: Vec3f origin{Vec3f::Zero()}; @@ -175,7 +180,13 @@ void UEOctree::addExploredSpace(const Vec3f& min, const Vec3f& max) } template -bool UEOctree::isExplored(const Vec3f& pt) +void UEOctree::crop(const Vec3f& min, const Vec3f& max) +{ + this->recursiveCrop(this->root, this->getRootDescriptor(), Box3f{min, max}); +} + +template +bool UEOctree::isExplored(const Vec3f& pt) const { const Arr3f aligned_pt = ((pt - this->origin) / this->vox_res).array(); if ((aligned_pt >= Arr3f::Zero()).all() && @@ -188,7 +199,7 @@ bool UEOctree::isExplored(const Vec3f& pt) const Arr3i vox_idx = aligned_pt.floor().template cast(); - Node* node = &(this->root); + const Node* node = &(this->root); // iterates through [root_height - 1 ... 0], while active node still has children for (size_t height = this->root_height; height-- > 0 && !node->isNull();) @@ -267,7 +278,7 @@ UEOctree::Vec3f UEOctree::getDescriptorSpan3f( template UEOctree::NodeDescriptor UEOctree::getRootDescriptor() const { - return NodeDescriptor{0, 0, 0, 0x1 << this->root_height}; + return NodeDescriptor{0, 0, 0, (I)0x1 << this->root_height}; } template @@ -277,9 +288,9 @@ UEOctree::NodeDescriptor UEOctree::getChildDescriptor( { // assert(i < 8); return NodeDescriptor{ - n[0] + (n[3] / 2) * (i >> 2 & 0x1), - n[1] + (n[3] / 2) * (i >> 1 & 0x1), - n[2] + (n[3] / 2) * (i >> 0 & 0x1), + n[0] + (n[3] / 2) * static_cast(i >> 2 & 0x1), + n[1] + (n[3] / 2) * static_cast(i >> 1 & 0x1), + n[2] + (n[3] / 2) * static_cast(i >> 0 & 0x1), n[3] / 2}; } @@ -321,11 +332,14 @@ bool UEOctree::adjustBounds(const Vec3f& min, const Vec3f& max) return false; } - uint8_t x_bit = min.x() < this->origin.x() ? 0x1 : 0x0; - uint8_t y_bit = min.y() < this->origin.y() ? 0x1 : 0x0; - uint8_t z_bit = min.z() < this->origin.z() ? 0x1 : 0x0; - this->origin -= Vec3f{x_bit, y_bit, z_bit} * this->vox_res * - (0x1 << this->root_height); + size_t x_bit = min.x() < this->origin.x() ? 0x1 : 0x0; + size_t y_bit = min.y() < this->origin.y() ? 0x1 : 0x0; + size_t z_bit = min.z() < this->origin.z() ? 0x1 : 0x0; + const Vec3f coeffs{ + static_cast(x_bit), + static_cast(y_bit), + static_cast(z_bit)}; + this->origin -= coeffs * this->vox_res * (0x1 << this->root_height); this->vox_span *= 2; this->root_height++; if (this->root.anyExplored()) @@ -405,5 +419,56 @@ void UEOctree::recursiveExplore( } } +template +void UEOctree::recursiveCrop( + Node& node, + const NodeDescriptor& descriptor, + const Box3f& zone) +{ + Box3f span = this->getDescriptorBox(descriptor); + if (!zone.contains(span)) + { + if (zone.intersects(span)) + { + // not contained but intersected + if (descriptor[3] > 1) + { + // not leaf --> expand children and recurse + const bool was_explored = node.explored; + node.init(); + node.explored = false; + + bool all_explored = true; + for (size_t i = 0; i < 8; i++) + { + Node& child = node[i]; + // need to propegate down in the case that the parent was split up, + // but leave untouched otherwise + child.explored |= was_explored; + this->recursiveCrop( + child, + this->getChildDescriptor(descriptor, i), + zone); + + all_explored &= child.fullyExplored(); + } + + if (all_explored) + { + node.explored = true; + node.clear(); + } + } + // leaf --> leave alone + } + else + { + // not contained and not intersected --> delete + node.explored = false; + node.clear(); + } + } +} + }; // namespace perception }; // namespace csm diff --git a/src/core/modules/map_octree.cpp b/src/core/modules/map_octree.cpp index 7375185..15860f0 100644 --- a/src/core/modules/map_octree.cpp +++ b/src/core/modules/map_octree.cpp @@ -13,3 +13,6 @@ using namespace csm::perception; MAP_OCTREE_INSTANTIATE_CLASS_TEMPLATE(MappingPointType, MAP_OCTREE_STORE_NORMALS) MAP_OCTREE_INSTANTIATE_PCL_DEPENDENCIES(MappingPointType) + +MAP_OCTREE_INSTANTIATE_CLASS_TEMPLATE(TraversibilityPointType) +MAP_OCTREE_INSTANTIATE_PCL_DEPENDENCIES(TraversibilityPointType) diff --git a/src/core/threads/path_planning_worker.cpp b/src/core/threads/path_planning_worker.cpp index 85eca1c..759560e 100644 --- a/src/core/threads/path_planning_worker.cpp +++ b/src/core/threads/path_planning_worker.cpp @@ -138,25 +138,26 @@ void PathPlanningWorker::path_planning_thread_worker() do { - if (this->srv_enable_state.load()) + const PathPlanningResources& buff = + this->path_planning_resources.waitNewestResource(); + + if (!this->threads_running.load()) { - auto& buff = this->path_planning_resources.waitNewestResource(); - if (!this->threads_running.load()) - { - break; - } - - buff.target = this->pplan_target_notifier.aquireNewestOutput(); - - PROFILING_SYNC(); - PROFILING_NOTIFY_ALWAYS(path_planning); - this->path_planning_callback(buff); - PROFILING_NOTIFY_ALWAYS(path_planning); + break; } - else + + PROFILING_SYNC(); + PROFILING_NOTIFY_ALWAYS(path_planning); + this->update_map_callback(buff); + + if (this->srv_enable_state.load() && this->threads_running.load()) { - this->pplan_target_notifier.waitNewestResource(); + // this->path_planning_callback( + // buff, + // this->pplan_target_notifier.aquireNewestOutput()); } + + PROFILING_NOTIFY_ALWAYS(path_planning); } // while (this->threads_running.load()); @@ -166,28 +167,43 @@ void PathPlanningWorker::path_planning_thread_worker() } -void PathPlanningWorker::path_planning_callback(PathPlanningResources& buff) +void PathPlanningWorker::update_map_callback(const PathPlanningResources& buff) { - if (buff.target.header.frame_id != this->odom_frame) + this->pplan_map.append(buff.points, buff.bounds_min, buff.bounds_max); + + // crop if not actively planning + + PointCloudMsg msg; + pcl::toROSMsg(this->pplan_map.getPoints(), msg); + msg.header.frame_id = this->odom_frame; + msg.header.stamp = util::toTimeMsg(buff.stamp); + this->pub_map.publish("pplan_map", msg); +} + +void PathPlanningWorker::path_planning_callback( + const PathPlanningResources& buff, + PoseStampedMsg& target) +{ + if (target.header.frame_id != this->odom_frame) { try { auto tf = this->tf_buffer.lookupTransform( this->odom_frame, - buff.target.header.frame_id, + target.header.frame_id, // util::toTf2TimePoint(buff.stamp)); tf2::timeFromSec(0)); // TODO: ensure tf stamp is not wildly out of date - tf2::doTransform(buff.target, buff.target, tf); + tf2::doTransform(target, target, tf); } catch (const std::exception& e) { RCLCPP_INFO( this->node.get_logger(), "[PATH PLANNING CALLBACK]: Failed to transform target pose from '%s' to '%s'\n\twhat(): %s", - buff.target.header.frame_id.c_str(), + target.header.frame_id.c_str(), this->odom_frame.c_str(), e.what()); return; @@ -195,20 +211,20 @@ void PathPlanningWorker::path_planning_callback(PathPlanningResources& buff) } Vec3f odom_target; - odom_target << buff.target.pose.position; + odom_target << target.pose.position; std::vector path; - if (!this->path_planner.solvePath( - buff.base_to_odom.pose.vec, - odom_target, - buff.bounds_min, - buff.bounds_max, - buff.points, - path)) - { - return; - } + // if (!this->path_planner.solvePath( + // buff.base_to_odom.pose.vec, + // odom_target, + // buff.bounds_min, + // buff.bounds_max, + // buff.points, + // path)) + // { + // return; + // } PathMsg path_msg; path_msg.header.frame_id = this->odom_frame; @@ -223,7 +239,7 @@ void PathPlanningWorker::path_planning_callback(PathPlanningResources& buff) } this->pub_map.publish("planned_path", path_msg); - this->pub_map.publish("pplan_target", buff.target); + this->pub_map.publish("pplan_target", target); } }; // namespace perception diff --git a/src/core/threads/path_planning_worker.hpp b/src/core/threads/path_planning_worker.hpp index 29c6bd5..0768235 100644 --- a/src/core/threads/path_planning_worker.hpp +++ b/src/core/threads/path_planning_worker.hpp @@ -54,6 +54,7 @@ #include #include +#include #include #include @@ -96,7 +97,8 @@ class PathPlanningWorker protected: void path_planning_thread_worker(); - void path_planning_callback(PathPlanningResources& buff); + void update_map_callback(const PathPlanningResources& buff); + void path_planning_callback(const PathPlanningResources& buff, PoseStampedMsg& target); protected: RclNode& node; @@ -108,6 +110,7 @@ class PathPlanningWorker std::atomic threads_running{false}; std::atomic srv_enable_state{false}; + PathPlanMap pplan_map; PathPlanner path_planner; util::ResourcePipeline pplan_target_notifier; util::ResourcePipeline path_planning_resources; diff --git a/src/core/threads/shared_resources.hpp b/src/core/threads/shared_resources.hpp index 369f85b..597aed0 100644 --- a/src/core/threads/shared_resources.hpp +++ b/src/core/threads/shared_resources.hpp @@ -48,7 +48,6 @@ #include #include -#include #include #include @@ -88,7 +87,6 @@ struct PathPlanningResources Eigen::Vector3f bounds_min; Eigen::Vector3f bounds_max; util::geom::PoseTf3f base_to_odom; - geometry_msgs::msg::PoseStamped target; pcl::PointCloud points; }; From 1adb0781ba50f08228dd75ed4113816d24063033 Mon Sep 17 00:00:00 2001 From: S1ink Date: Wed, 7 Jan 2026 21:01:37 -0600 Subject: [PATCH 3/8] map accumulation [mostly] successful --- include/modules/impl/path_planner_impl.hpp | 222 +++++++++++++++++++-- include/modules/path_plan_map.hpp | 49 +++-- include/modules/path_planner.hpp | 17 +- include/modules/ue_octree.hpp | 172 +++++++++++++++- include/traversibility_def.hpp | 2 +- src/core/threads/path_planning_worker.cpp | 25 +-- 6 files changed, 414 insertions(+), 73 deletions(-) diff --git a/include/modules/impl/path_planner_impl.hpp b/include/modules/impl/path_planner_impl.hpp index c7c5777..3f6bbbb 100644 --- a/include/modules/impl/path_planner_impl.hpp +++ b/include/modules/impl/path_planner_impl.hpp @@ -68,7 +68,7 @@ namespace perception template PathPlanner

::Node::Node(const P& point, float h, Node* p) : - trav_point(point), + point(point), g(std::numeric_limits::infinity()), h(h), parent(p) @@ -99,7 +99,7 @@ void PathPlanner

::setParameters( } template -bool PathPlanner

::solveBoundedPath( +bool PathPlanner

::solvePath( std::vector& path, const Vec3f& start, const Vec3f& goal, @@ -112,30 +112,208 @@ bool PathPlanner

::solveBoundedPath( } template -bool PathPlanner

::solveFrontierPath( +bool PathPlanner

::solvePath( std::vector& path, const Vec3f& start, const Vec3f& goal, - const PointCloudT& points, + const PathPlanMapT& map, const WeightT max_weight) { - // pcl::Indices tmp_indices; - // std::vector tmp_dists; - - // this->pt_selection.clear(); - // this->pt_selection.reserve(points.size()); - // for (size_t i = 0; i < points.size(); i++) - // { - // const auto& pt = points[i]; - // if (isWeighted(pt) && weight(pt) <= max_weight || isFrontier(pt)) - // { - // this->pt_selection.push_back(i); - // } - // } - - // const auto points_ptr = util::wrapUnmanaged(points); - // const auto selection_ptr = util::wrapUnmanaged(this->pt_selection); - // this->kdtree.setInputCloud(points_ptr, selection_ptr); + const typename PathPlanMapT::UEOctreeT& ue_space = map.getUESpace(); + + pcl::Indices tmp_indices; + std::vector tmp_dists; + + // 1. Filter out non-traversible points and build KDTree + this->points.points.clear(); + this->points.points.reserve(map.getPoints().size()); + for (const PointT& pt : map.getPoints()) + { + if (isWeighted(pt) && weight(pt) <= max_weight /*|| isFrontier(pt)*/) + { + this->points.points.emplace_back(pt); + } + } + this->points.width = this->points.points.size(); + this->points.height = 1; + this->points.is_dense = true; + + const auto points_ptr = util::wrapUnmanaged(this->points); + this->kdtree.setInputCloud(points_ptr); + + // 2. Snap goal pt to pointset if inside explored region but outside of + // discovery range. Since we removed all obstacle points, this changes + // the goal to be the nearest non-obstacle point in the map as long as it + // is in the explored region (in the case which the goal was + // non-traversible). + PointT goal_pt{goal.x(), goal.y(), goal.z()}; + if (ue_space.isExplored(goal) && !this->kdtree.radiusSearch( + goal_pt, + this->goal_threshold, + tmp_indices, + tmp_dists, + 1)) + { + if (this->kdtree.nearestKSearch(goal_pt, 1, tmp_indices, tmp_dists)) + { + goal_pt = points[tmp_indices[0]]; + if (std::sqrt(tmp_dists[0]) > this->search_radius) + { + DEBUG_COUT( + "Warning - path planning goal node snapped to available" + " node with distance greater than search radius!"); + } + } + else + { + // no nearest point --> no points at all! + return false; + } + } + + // 3. Construct nodes + this->nodes.clear(); + this->nodes.reserve(this->points.size()); + for (const auto& pt : this->points) + { + // compute h as proportional to straight-line goal distance + this->nodes.emplace_back( + pt, + this->lambda_dist * (goal - pt.getVector3fMap()).norm()); + } + + // 4. Find start node + int start_idx = -1; + if (this->kdtree.nearestKSearch( + PointT{start.x(), start.y(), start.z()}, + 1, + tmp_indices, + tmp_dists)) + { + start_idx = tmp_indices[0]; + this->nodes[start_idx].g = 0.f; + } + else + { + DEBUG_COUT("Error - could not snap start node to available points!"); + return false; + } + + // 5. Setup + util::DAryHeap open(static_cast(this->nodes.size())); + open.reserve_heap(this->nodes.size()); + open.push(start_idx, this->nodes[start_idx].f()); + + std::vector closed(this->nodes.size(), false); + + int closest_idx = start_idx; + float closest_dist = this->nodes[start_idx].h; + bool found_frontier = false; + + auto create_path = [&](Node& path_end) + { + path.clear(); + for (Node* n = &path_end; n != nullptr; n = n->parent) + { + path.push_back(n->position()); + } + std::reverse(path.begin(), path.end()); + }; + + // 6. A* body + while (!open.empty()) + { + const int idx = open.pop(); + Node& current = this->nodes[idx]; + + if (closed[idx]) + { + continue; + } + else + { + closed[idx] = true; + } + + if ((current.position() - goal_pt.getVector3fMap()).norm() < + this->goal_threshold) + { + create_path(current); + return true; + } + + if (current.neighbors.empty()) + { + tmp_dists.clear(); + this->kdtree.radiusSearch( + current.point, + this->search_radius, + current.neighbors, + tmp_dists, + this->max_neighbors); + } + + for (const pcl::index_t nb_idx : current.neighbors) + { + Node& nb = this->nodes[nb_idx]; + + if (closed[nb_idx]) + { + continue; + } + // this is prefiltered + // else if (nb.cost() > max_weight) + // { + // closed[nb_idx] = true; + // continue; + // } + + const float geom = (nb.position() - current.position()).norm(); + const float edge = + this->lambda_dist * geom + this->lambda_penalty * nb.cost(); + const float tentative_g = current.g + edge; + + if (tentative_g < nb.g) + { + nb.g = tentative_g; + nb.parent = ¤t; + + const bool is_frontier = + ue_space.distToUnexplored( + nb.position(), + this->boundary_radius) <= this->boundary_radius; + if(!found_frontier && is_frontier) + { + closest_dist = nb.h; + closest_idx = nb_idx; + found_frontier = true; + } + else + { + if(nb.h < closest_dist && (!found_frontier || is_frontier)) + { + closest_dist = nb.h; + closest_idx = nb_idx; + } + } + + if(!open.contains(nb_idx)) + { + open.push(nb_idx, nb.f()); + } + else + { + open.decrease_key(nb_idx, nb.f()); + } + } + } + } + + if(closest_idx >= 0) + { + create_path(this->nodes[closest_idx]); + return found_frontier; + } return false; } @@ -299,7 +477,7 @@ bool PathPlanner

::solveFrontierPath( // current.neighbors.clear(); // tmp_dists.clear(); // this->kdtree.radiusSearch( -// current.trav_point, +// current.point, // search_radius, // current.neighbors, // tmp_dists, diff --git a/include/modules/path_plan_map.hpp b/include/modules/path_plan_map.hpp index 8690fac..9d1f225 100644 --- a/include/modules/path_plan_map.hpp +++ b/include/modules/path_plan_map.hpp @@ -64,6 +64,7 @@ class PathPlanMap { static_assert(util::traits::supports_traversibility::value); +public: using PointT = Point_T; using PointCloudT = pcl::PointCloud; @@ -93,6 +94,7 @@ class PathPlanMap void crop(const Vec3f& min, const Vec3f& max); const PointCloudT& getPoints() const; + const UEOctreeT& getUESpace() const; protected: UEOctreeT ue_octree; @@ -108,7 +110,7 @@ class PathPlanMap template PathPlanMap

::PathPlanMap(float vox_sz) : - ue_octree{vox_sz}, + ue_octree{vox_sz * 0.5f}, map_octree{vox_sz} { } @@ -144,9 +146,10 @@ void PathPlanMap

::append( { using namespace csm::perception::traversibility; - const float res = this->ue_octree.resolution(); + const float res = static_cast(this->map_octree.getResolution()); pcl::Indices tmp_indices; + std::vector tmp_dists; typename PointCloudT::VectorType merge_pts; const auto& map_pts_vec = this->map_octree.getInputCloud()->points; @@ -174,11 +177,9 @@ void PathPlanMap

::append( tmp_indices); merge_pts.reserve(tmp_indices.size()); util::copySelection(map_pts_vec, tmp_indices, merge_pts); - this->map_octree.deletePoints(tmp_indices); + this->map_octree.deletePoints(tmp_indices, false); tmp_indices.clear(); - // 2.5. Remove frontier nodes in extended box region - // 3. Insert new points this->map_octree.addPoints(pts); @@ -186,12 +187,15 @@ void PathPlanMap

::append( for (const auto& pt : merge_pts) { if ((isWeighted(pt) || isObstacle(pt)) && - this->map_octree.voxelSearch(pt, tmp_indices)) + this->map_octree.radiusSearch(pt, res * 0.5f, tmp_indices, tmp_dists)) { - PointT& new_pt = this->map_octree.pointAt(tmp_indices[0]); - if (!isFrontier(new_pt) && weight(pt) > weight(new_pt)) + for(const pcl::index_t i : tmp_indices) { - weight(new_pt) = weight(pt); + PointT& new_pt = this->map_octree.pointAt(i); + if (weight(pt) > weight(new_pt)) + { + weight(new_pt) = weight(pt); + } } } tmp_indices.clear(); @@ -200,14 +204,15 @@ void PathPlanMap

::append( // 5. Update u-e octree, insert new frontier points this->ue_octree.addExploredSpace(bound_min, bound_max); - const Arr3f length3 = (bound_max - bound_min).array(); - const Arr3f innerlen3 = (length3 / res).floor() * res; - const Arr3f margin3 = (length3 - innerlen3) * 0.5f; - const Vec3f start3 = bound_min + margin3.matrix(); + // const Arr3f length3 = (bound_max - bound_min).array(); + // const Arr3f innerlen3 = (length3 / res).floor() * res; + // const Arr3f margin3 = (length3 - innerlen3) * 0.5f; + // const Vec3f start3 = bound_min + margin3.matrix(); - PointT a{}, b{}; - weight(a) = weight(b) = FRONTIER_MARKER_VAL; + // PointT a{}, b{}; + // weight(a) = weight(b) = FRONTIER_MARKER_VAL; + /* #define EXPLORE_PARALLEL_SIDES(U, V, W) \ a.U = bound_min.U() - this->frontier_offset; \ b.U = bound_max.U() + this->frontier_offset; \ @@ -225,13 +230,15 @@ void PathPlanMap

::append( } \ } \ } + */ - EXPLORE_PARALLEL_SIDES(x, y, z) - EXPLORE_PARALLEL_SIDES(y, x, z) + // EXPLORE_PARALLEL_SIDES(x, y, z) + // EXPLORE_PARALLEL_SIDES(y, x, z) // EXPLORE_PARALLEL_SIDES(z, x, y) -#undef EXPLORE_PARALLEL_SIDES +// #undef EXPLORE_PARALLEL_SIDES + // 6. Optimize point layout this->map_octree.optimizeStorage(); } @@ -248,5 +255,11 @@ const typename PathPlanMap

::PointCloudT& PathPlanMap

::getPoints() const return this->map_octree.points(); } +template +const typename PathPlanMap

::UEOctreeT& PathPlanMap

::getUESpace() const +{ + return this->ue_octree; +} + }; // namespace perception }; // namespace csm diff --git a/include/modules/path_planner.hpp b/include/modules/path_planner.hpp index caeb2c1..d10329c 100644 --- a/include/modules/path_planner.hpp +++ b/include/modules/path_planner.hpp @@ -49,6 +49,8 @@ #include +#include "path_plan_map.hpp" + #ifndef PATH_PLANNING_PEDANTIC #define PATH_PLANNING_PEDANTIC 0 @@ -72,6 +74,7 @@ class PathPlanner public: using PointT = Point_T; using PointCloudT = pcl::PointCloud; + using PathPlanMapT = PathPlanMap; using WeightT = traversibility::weight_t; @@ -81,7 +84,7 @@ class PathPlanner private: struct Node { - const PointT& trav_point; + const PointT& point; float g; // cost from start to this node float h; // heuristic cost to goal Node* parent = nullptr; @@ -94,11 +97,11 @@ class PathPlanner // cost of this node inline WeightT cost() const { - return traversibility::weight(this->trav_point); + return traversibility::weight(this->point); } inline auto position() const { - return this->trav_point.getVector3fMap(); + return this->point.getVector3fMap(); } }; @@ -114,7 +117,7 @@ class PathPlanner float lambda_penalty, size_t max_neighbors = 10); - bool solveBoundedPath( + bool solvePath( std::vector& path, const Vec3f& start, const Vec3f& goal, @@ -124,18 +127,18 @@ class PathPlanner const WeightT max_weight = traversibility::NOMINAL_MAX_WEIGHT); - bool solveFrontierPath( + bool solvePath( std::vector& path, const Vec3f& start, const Vec3f& goal, - const PointCloudT& trav_points, + const PathPlanMapT& map, const WeightT max_weight = traversibility::NOMINAL_MAX_WEIGHT); private: + PointCloudT points; pcl::search::KdTree kdtree; std::vector nodes; // all nodes in the search space - pcl::Indices pt_selection; // Dist. from search space edge for boundary nodes float boundary_radius = 0.15f; diff --git a/include/modules/ue_octree.hpp b/include/modules/ue_octree.hpp index 9f6a586..46c52cd 100644 --- a/include/modules/ue_octree.hpp +++ b/include/modules/ue_octree.hpp @@ -79,7 +79,9 @@ class UEOctree void addExploredSpace(const Vec3f& min, const Vec3f& max); void crop(const Vec3f& min, const Vec3f& max); + bool isExplored(const Vec3f& pt) const; + FloatT distToUnexplored(const Vec3f& pt, FloatT max_dist) const; inline FloatT resolution() const { return vox_res; } @@ -104,9 +106,14 @@ class UEOctree { return !this->isNull() || this->fullyExplored(); } + inline bool isUnexplored() const + { + return !this->explored && this->isNull(); + } }; protected: + static bool isLeaf(const NodeDescriptor& n); static Vec3i getDescriptorKey(const NodeDescriptor& n); static Vec3f getDescriptorKeyF(const NodeDescriptor& n); static Vec3i getDescriptorSpan3(const NodeDescriptor& n); @@ -116,6 +123,9 @@ class UEOctree NodeDescriptor getChildDescriptor(const NodeDescriptor& n, size_t i) const; Box3f getDescriptorBox(const NodeDescriptor& n) const; + static FloatT distSquaredToBox(const Vec3f& p, const Box3f& b); + static FloatT distToBoxInv(const Vec3f& p, const Box3f& b); + protected: bool adjustBounds(const Vec3f& min, const Vec3f& max); void recursiveExplore( @@ -219,6 +229,98 @@ bool UEOctree::isExplored(const Vec3f& pt) const return false; } +template +typename UEOctree::FloatT UEOctree::distToUnexplored( + const Vec3f& pt, + FloatT max_dist) const +{ + const FloatT max_dist2 = max_dist * max_dist; + + const NodeDescriptor root_desc = this->getRootDescriptor(); + const Box3f root_box = this->getDescriptorBox(root_desc); + + if (!root_box.contains(pt)) + { + return 0; + } + if (this->root.fullyExplored()) + { + const FloatT d = distToBoxInv(pt, root_box); + return d <= max_dist ? d : std::numeric_limits::infinity(); + } + + struct QNode + { + const Node* node; + NodeDescriptor desc; + FloatT dist2; + + bool operator>(const QNode& other) const + { + return this->dist2 > other.dist2; + } + }; + + std::priority_queue, std::greater> queue; + queue.push({&this->root, root_desc, 0}); + + while (!queue.empty()) + { + const QNode cur = queue.top(); + queue.pop(); + + if (cur.dist2 > max_dist2) + { + continue; + } + + const Node* node = cur.node; + const NodeDescriptor& desc = cur.desc; + + if (node->isUnexplored()) + { + return std::sqrt(cur.dist2); + } + + if (isLeaf(desc)) + { + if (!node->explored) + { + return std::sqrt(cur.dist2); + } + continue; + } + + if (!node->isNull()) + { + for (size_t i = 0; i < 8; ++i) + { + const Node& child = (*node)[i]; + if (child.fullyExplored()) + { + continue; + } + + const NodeDescriptor child_desc = + this->getChildDescriptor(desc, i); + + const FloatT d2 = + distSquaredToBox(pt, this->getDescriptorBox(child_desc)); + if (d2 <= max_dist2) + { + queue.push({&child, child_desc, d2}); + } + } + } + else + { + return std::sqrt(cur.dist2); + } + } + + return std::numeric_limits::infinity(); +} + template void UEOctree::Node::init() @@ -239,13 +341,14 @@ void UEOctree::Node::clear() } template -UEOctree::Node& UEOctree::Node::operator[](size_t i) +typename UEOctree::Node& UEOctree::Node::operator[](size_t i) { // assert(i < 8 && this->children); return this->children[i]; } template -const UEOctree::Node& UEOctree::Node::operator[](size_t i) const +const typename UEOctree::Node& UEOctree::Node::operator[]( + size_t i) const { // assert(i < 8 && this->children); return this->children[i]; @@ -253,36 +356,44 @@ const UEOctree::Node& UEOctree::Node::operator[](size_t i) const template -UEOctree::Vec3i UEOctree::getDescriptorKey(const NodeDescriptor& n) +bool UEOctree::isLeaf(const NodeDescriptor& n) +{ + return n[3] <= 1; +} +template +typename UEOctree::Vec3i UEOctree::getDescriptorKey( + const NodeDescriptor& n) { return n.template head<3>(); } template -UEOctree::Vec3f UEOctree::getDescriptorKeyF(const NodeDescriptor& n) +typename UEOctree::Vec3f UEOctree::getDescriptorKeyF( + const NodeDescriptor& n) { return n.template head<3>().template cast(); } template -UEOctree::Vec3i UEOctree::getDescriptorSpan3( +typename UEOctree::Vec3i UEOctree::getDescriptorSpan3( const NodeDescriptor& n) { return Vec3i::Constant(n[3]); } template -UEOctree::Vec3f UEOctree::getDescriptorSpan3f( +typename UEOctree::Vec3f UEOctree::getDescriptorSpan3f( const NodeDescriptor& n) { return Vec3f::Constant(static_cast(n[3])); } template -UEOctree::NodeDescriptor UEOctree::getRootDescriptor() const +typename UEOctree::NodeDescriptor UEOctree::getRootDescriptor() + const { return NodeDescriptor{0, 0, 0, (I)0x1 << this->root_height}; } template -UEOctree::NodeDescriptor UEOctree::getChildDescriptor( +typename UEOctree::NodeDescriptor UEOctree::getChildDescriptor( const NodeDescriptor& n, size_t i) const { @@ -295,7 +406,7 @@ UEOctree::NodeDescriptor UEOctree::getChildDescriptor( } template -UEOctree::Box3f UEOctree::getDescriptorBox( +typename UEOctree::Box3f UEOctree::getDescriptorBox( const NodeDescriptor& n) const { const Vec3f min_corner = @@ -305,6 +416,45 @@ UEOctree::Box3f UEOctree::getDescriptorBox( min_corner + getDescriptorSpan3f(n) * this->vox_res}; } +template +typename UEOctree::FloatT UEOctree::distSquaredToBox( + const Vec3f& p, + const Box3f& b) +{ + FloatT d2 = 0; + for (size_t i = 0; i < 3; i++) + { + if (p[i] < b.min()[i]) + { + FloatT d = b.min()[i] - p[i]; + d2 += d * d; + } + else if (p[i] > b.max()[i]) + { + FloatT d = p[i] - b.max()[i]; + d2 += d * d; + } + } + return d2; +} + +template +typename UEOctree::FloatT UEOctree::distToBoxInv( + const Vec3f& p, + const Box3f& b) +{ + if (!b.contains(p)) + { + return 0; + } + + FloatT dx = std::min(p.x() - b.min().x(), b.max().x() - p.x()); + FloatT dy = std::min(p.y() - b.min().y(), b.max().y() - p.y()); + FloatT dz = std::min(p.z() - b.min().z(), b.max().z() - p.z()); + + return std::min({dx, dy, dz}); +} + template bool UEOctree::adjustBounds(const Vec3f& min, const Vec3f& max) @@ -398,7 +548,7 @@ void UEOctree::recursiveExplore( } else if (zone.intersects(child_span)) { - if (child_desc[3] > 1) + if (!isLeaf(child_desc)) { this->recursiveExplore(child, child_desc, zone); } @@ -431,7 +581,7 @@ void UEOctree::recursiveCrop( if (zone.intersects(span)) { // not contained but intersected - if (descriptor[3] > 1) + if (!isLeaf(descriptor)) { // not leaf --> expand children and recurse const bool was_explored = node.explored; diff --git a/include/traversibility_def.hpp b/include/traversibility_def.hpp index 33a3caa..a2c8915 100644 --- a/include/traversibility_def.hpp +++ b/include/traversibility_def.hpp @@ -328,7 +328,7 @@ inline constexpr bool isUnknown(const T& val) return std::isnan(constexprWeight(val)); } /* Test if a traversibility weight should be read as a marker - ie. it's weight - * value is not meaningful. + * value is not meaningful as a weight. * Weight evaluators are automatically applied to "unbox" the weight value. */ template inline constexpr bool isMarker(const T& val) diff --git a/src/core/threads/path_planning_worker.cpp b/src/core/threads/path_planning_worker.cpp index 759560e..d227391 100644 --- a/src/core/threads/path_planning_worker.cpp +++ b/src/core/threads/path_planning_worker.cpp @@ -152,9 +152,9 @@ void PathPlanningWorker::path_planning_thread_worker() if (this->srv_enable_state.load() && this->threads_running.load()) { - // this->path_planning_callback( - // buff, - // this->pplan_target_notifier.aquireNewestOutput()); + this->path_planning_callback( + buff, + this->pplan_target_notifier.aquireNewestOutput()); } PROFILING_NOTIFY_ALWAYS(path_planning); @@ -214,17 +214,14 @@ void PathPlanningWorker::path_planning_callback( odom_target << target.pose.position; std::vector path; - - // if (!this->path_planner.solvePath( - // buff.base_to_odom.pose.vec, - // odom_target, - // buff.bounds_min, - // buff.bounds_max, - // buff.points, - // path)) - // { - // return; - // } + if (!this->path_planner.solvePath( + path, + buff.base_to_odom.pose.vec, + odom_target, + this->pplan_map)) + { + return; + } PathMsg path_msg; path_msg.header.frame_id = this->odom_frame; From 49c150416a2d5bac22f7205dbedcccb4df39e06f Mon Sep 17 00:00:00 2001 From: S1ink Date: Tue, 13 Jan 2026 14:15:48 -0600 Subject: [PATCH 4/8] add new pplan configs, implement crop and retry, add lance2 configs --- config/perception.json | 64 +++++++++++++++++----- include/modules/impl/path_planner_impl.hpp | 2 + include/modules/path_plan_map.hpp | 6 +- src/core/perception_core.cpp | 40 ++++++++++++-- src/core/threads/mapping_worker.cpp | 20 +++---- src/core/threads/mapping_worker.hpp | 16 +++--- src/core/threads/path_planning_worker.cpp | 52 ++++++++++++++---- src/core/threads/path_planning_worker.hpp | 12 +++- 8 files changed, 155 insertions(+), 57 deletions(-) diff --git a/config/perception.json b/config/perception.json index c1403f5..c4b7593 100644 --- a/config/perception.json +++ b/config/perception.json @@ -489,7 +489,7 @@ }, "robot_tf": { - "pragma:default": "lance", + "pragma:default": "lance2", "lance": { "robot_description": @@ -499,8 +499,6 @@ "links": [ "base_link", "frame_link", - "left_track_link", - "right_track_link", "collection_link", "lidar_link", "lidar_link_inv_rotated" @@ -512,18 +510,6 @@ "child": "frame_link", "origin": { "xyz": [0, 0, 0.13103567] } }, - "left_track_joint": { - "type": "fixed", - "parent": "frame_link", - "child": "left_track_link", - "origin": { "xyz": [0.20318559, 0.289888, 0.00242022] } - }, - "right_track_joint": { - "type": "fixed", - "parent": "frame_link", - "child": "right_track_link", - "origin": { "xyz": [0.20318559, -0.289888, 0.00242022] } - }, "dump_joint": { "type": "revolute", "parent": "frame_link", @@ -561,6 +547,54 @@ } } } + }, + "lance2": + { + "robot_description": + { + "name": "lance2", + "structure": + { + "links": [ + "base_link", + "hopper_link", + "lidar_link" + ], + "joints": + { + "hopper_joint": + { + "type": "revolute", + "parent": "base_link", + "child": "hopper_link", + "origin": + { + "xyz": [-0.4572, 0, 0.3488], + "rpy": [0, 0.175, 0] + }, + "axis": [0, 1, 0], + "limit": + { + "upper": 0.175, + "lower": -0.175, + "effort": 100, + "velocity": 100 + } + }, + "lidar_joint": + { + "type": "fixed", + "parent": "base_link", + "child": "lidar_link", + "origin": + { + "xyz": [0.5156, 0.0, 0.5691], + "rpy": [3.14159, 0.349066, 0.0] + } + } + } + } + } } }, "bag_play": diff --git a/include/modules/impl/path_planner_impl.hpp b/include/modules/impl/path_planner_impl.hpp index 3f6bbbb..03a5b04 100644 --- a/include/modules/impl/path_planner_impl.hpp +++ b/include/modules/impl/path_planner_impl.hpp @@ -119,6 +119,8 @@ bool PathPlanner

::solvePath( const PathPlanMapT& map, const WeightT max_weight) { + // TODO: add passthrough mode + const typename PathPlanMapT::UEOctreeT& ue_space = map.getUESpace(); pcl::Indices tmp_indices; diff --git a/include/modules/path_plan_map.hpp b/include/modules/path_plan_map.hpp index 9d1f225..21e605e 100644 --- a/include/modules/path_plan_map.hpp +++ b/include/modules/path_plan_map.hpp @@ -83,7 +83,6 @@ class PathPlanMap void clear(); void reconfigure( float obstacle_merge_window, - float frontier_offset, float voxel_size = -1.f); void append( @@ -101,7 +100,6 @@ class PathPlanMap MapOctreeT map_octree; float obstacle_merge_window{0.5f}; - float frontier_offset{0.25f}; }; @@ -125,11 +123,9 @@ void PathPlanMap

::clear() template void PathPlanMap

::reconfigure( float obstacle_merge_window, - float frontier_offset, float voxel_size) { this->obstacle_merge_window = obstacle_merge_window; - this->frontier_offset = frontier_offset; if (voxel_size > 0.f) { @@ -144,6 +140,8 @@ void PathPlanMap

::append( const Vec3f& bound_min, const Vec3f& bound_max) { + // TODO: frame-to-frame "bounds alignment" for more condistent map + using namespace csm::perception::traversibility; const float res = static_cast(this->map_octree.getResolution()); diff --git a/src/core/perception_core.cpp b/src/core/perception_core.cpp index 8ba7178..532830b 100644 --- a/src/core/perception_core.cpp +++ b/src/core/perception_core.cpp @@ -138,10 +138,10 @@ struct PerceptionConfig double kfc_add_max_range; double kfc_voxel_size; - double map_crop_horizontal_range; - double map_crop_vertical_range; - double map_export_horizontal_range; - double map_export_vertical_range; + float map_crop_horizontal_range; + float map_crop_vertical_range; + float map_export_horizontal_range; + float map_export_vertical_range; // traversibility float trav_norm_estimation_radius; @@ -161,6 +161,9 @@ struct PerceptionConfig float pplan_search_radius; float pplan_lambda_dist; float pplan_lambda_penalty; + float pplan_map_obstacle_merge_window; + float pplan_map_passive_crop_horizontal_range; + float pplan_map_passive_crop_vertical_range; int pplan_max_neighbors; public: @@ -340,7 +343,13 @@ std::ostream& operator<<(std::ostream& os, const PerceptionConfig& config) << align("Search Radius") << config.pplan_search_radius << " meters\n" << align("Lambda Distance") << config.pplan_lambda_dist << " meters\n" << align("Lambda Penalty") << config.pplan_lambda_penalty << "\n" - << align("Max Num Neighbors") << config.pplan_max_neighbors << "\n"; + << align("Max Num Neighbors") << config.pplan_max_neighbors << "\n" + << align("Map Merge Window") << config.pplan_map_obstacle_merge_window + << " meters\n" + << align("Map Hrz. Crop Range") + << config.pplan_map_passive_crop_horizontal_range << " meters\n" + << align("Map Vrt. Crop Range") + << config.pplan_map_passive_crop_vertical_range << " meters\n"; #endif os << " +\n"; @@ -723,6 +732,21 @@ void PerceptionNode::getParams(PerceptionConfig& config) "pplan.max_neighbors", config.pplan_max_neighbors, 10); + util::declare_param( + this, + "pplan.map_obstacle_merge_window", + config.pplan_map_obstacle_merge_window, + 0.5f); + util::declare_param( + this, + "pplan.map_passive_crop_horizontal_range", + config.pplan_map_passive_crop_horizontal_range, + 10.f); + util::declare_param( + this, + "pplan.map_passive_crop_vertical_range", + config.pplan_map_passive_crop_vertical_range, + 5.f); this->path_planning_worker.path_planner.setParameters( config.pplan_boundary_radius, @@ -745,7 +769,11 @@ void PerceptionNode::getParams(PerceptionConfig& config) config.map_export_horizontal_range, config.map_export_vertical_range); this->traversibility_worker.configure(config.odom_frame); - this->path_planning_worker.configure(config.odom_frame); + this->path_planning_worker.configure( + config.odom_frame, + config.pplan_map_obstacle_merge_window, + config.pplan_map_passive_crop_horizontal_range, + config.pplan_map_passive_crop_vertical_range); this->mining_eval_worker.configure(config.odom_frame); } diff --git a/src/core/threads/mapping_worker.cpp b/src/core/threads/mapping_worker.cpp index 4228bbe..d5c442d 100644 --- a/src/core/threads/mapping_worker.cpp +++ b/src/core/threads/mapping_worker.cpp @@ -77,10 +77,10 @@ MappingWorker::~MappingWorker() { this->stopThreads(); } void MappingWorker::configure( const std::string& odom_frame, - double map_crop_horizontal_range, - double map_crop_vertical_range, - double map_export_horizontal_range, - double map_export_vertical_range) + float map_crop_horizontal_range, + float map_crop_vertical_range, + float map_export_horizontal_range, + float map_export_vertical_range) { this->odom_frame = odom_frame; this->map_crop_horizontal_range = map_crop_horizontal_range; @@ -188,9 +188,9 @@ void MappingWorker::mapping_callback(MappingResources& buff) this->map_crop_vertical_range > 0.) { const Vec3f crop_range{ - static_cast(this->map_crop_horizontal_range), - static_cast(this->map_crop_horizontal_range), - static_cast(this->map_crop_vertical_range)}; + this->map_crop_horizontal_range, + this->map_crop_horizontal_range, + this->map_crop_vertical_range}; this->sparse_map.setBounds( buff.base_to_odom.pose.vec - crop_range, buff.base_to_odom.pose.vec + crop_range); @@ -224,9 +224,9 @@ void MappingWorker::mapping_callback(MappingResources& buff) pcl::Indices export_points; const Vec3f search_range{ - static_cast(this->map_export_horizontal_range), - static_cast(this->map_export_horizontal_range), - static_cast(this->map_export_vertical_range)}; + this->map_export_horizontal_range, + this->map_export_horizontal_range, + this->map_export_vertical_range}; const Vec3f search_min{buff.base_to_odom.pose.vec - search_range}; const Vec3f search_max{buff.base_to_odom.pose.vec + search_range}; diff --git a/src/core/threads/mapping_worker.hpp b/src/core/threads/mapping_worker.hpp index cd7a6d1..2185d18 100644 --- a/src/core/threads/mapping_worker.hpp +++ b/src/core/threads/mapping_worker.hpp @@ -79,10 +79,10 @@ class MappingWorker public: void configure( const std::string& odom_frame, - double map_crop_horizontal_range, - double map_crop_vertical_range, - double map_export_horizontal_range, - double map_export_vertical_range); + float map_crop_horizontal_range, + float map_crop_vertical_range, + float map_export_horizontal_range, + float map_export_vertical_range); util::ResourcePipeline& getInput(); void connectOutput( @@ -101,10 +101,10 @@ class MappingWorker util::GenericPubMap pub_map; std::string odom_frame; - double map_crop_horizontal_range{0.}; - double map_crop_vertical_range{0.}; - double map_export_horizontal_range{0.}; - double map_export_vertical_range{0.}; + float map_crop_horizontal_range{0.f}; + float map_crop_vertical_range{0.f}; + float map_export_horizontal_range{0.f}; + float map_export_vertical_range{0.f}; std::atomic threads_running{false}; diff --git a/src/core/threads/path_planning_worker.cpp b/src/core/threads/path_planning_worker.cpp index d227391..161dd60 100644 --- a/src/core/threads/path_planning_worker.cpp +++ b/src/core/threads/path_planning_worker.cpp @@ -55,6 +55,7 @@ #include #include +#include using namespace util::geom::cvt::ops; @@ -81,9 +82,17 @@ PathPlanningWorker::PathPlanningWorker( PathPlanningWorker::~PathPlanningWorker() { this->stopThreads(); } -void PathPlanningWorker::configure(const std::string& odom_frame) +void PathPlanningWorker::configure( + const std::string& odom_frame, + float map_obstacle_merge_window, + float passive_crop_horizontal_range, + float passive_crop_vertical_range) { this->odom_frame = odom_frame; + this->passive_crop_horizontal_range = passive_crop_horizontal_range; + this->passive_crop_vertical_range = passive_crop_vertical_range; + + this->pplan_map.reconfigure(map_obstacle_merge_window); } void PathPlanningWorker::accept( @@ -148,11 +157,11 @@ void PathPlanningWorker::path_planning_thread_worker() PROFILING_SYNC(); PROFILING_NOTIFY_ALWAYS(path_planning); - this->update_map_callback(buff); + this->updateMap(buff); if (this->srv_enable_state.load() && this->threads_running.load()) { - this->path_planning_callback( + this->planPath( buff, this->pplan_target_notifier.aquireNewestOutput()); } @@ -167,11 +176,22 @@ void PathPlanningWorker::path_planning_thread_worker() } -void PathPlanningWorker::update_map_callback(const PathPlanningResources& buff) +void PathPlanningWorker::updateMap(const PathPlanningResources& buff) { this->pplan_map.append(buff.points, buff.bounds_min, buff.bounds_max); - // crop if not actively planning + if (!this->srv_enable_state.load() && + (this->passive_crop_horizontal_range * + this->passive_crop_vertical_range) > 0.f) + { + const Vec3f crop_range{ + this->passive_crop_horizontal_range, + this->passive_crop_horizontal_range, + this->passive_crop_vertical_range}; + this->pplan_map.crop( + buff.base_to_odom.pose.vec - crop_range, + buff.base_to_odom.pose.vec + crop_range); + } PointCloudMsg msg; pcl::toROSMsg(this->pplan_map.getPoints(), msg); @@ -180,10 +200,12 @@ void PathPlanningWorker::update_map_callback(const PathPlanningResources& buff) this->pub_map.publish("pplan_map", msg); } -void PathPlanningWorker::path_planning_callback( +void PathPlanningWorker::planPath( const PathPlanningResources& buff, PoseStampedMsg& target) { + using namespace csm::perception::traversibility; + if (target.header.frame_id != this->odom_frame) { try @@ -214,12 +236,20 @@ void PathPlanningWorker::path_planning_callback( odom_target << target.pose.position; std::vector path; - if (!this->path_planner.solvePath( - path, - buff.base_to_odom.pose.vec, - odom_target, - this->pplan_map)) + for (auto max_weight = NOMINAL_MAX_WEIGHT; + isWeighted(max_weight) && !this->path_planner.solvePath( + path, + buff.base_to_odom.pose.vec, + odom_target, + this->pplan_map, + max_weight); + max_weight *= 2) + { + path.clear(); + } + if(path.empty()) { + // Fail return; } diff --git a/src/core/threads/path_planning_worker.hpp b/src/core/threads/path_planning_worker.hpp index 0768235..fce97ee 100644 --- a/src/core/threads/path_planning_worker.hpp +++ b/src/core/threads/path_planning_worker.hpp @@ -84,7 +84,11 @@ class PathPlanningWorker ~PathPlanningWorker(); public: - void configure(const std::string& odom_frame); + void configure( + const std::string& odom_frame, + float map_obstacle_merge_window, + float passive_crop_horizontal_range, + float passive_crop_vertical_range); void accept( const UpdatePathPlanSrv::Request::SharedPtr& req, @@ -97,8 +101,8 @@ class PathPlanningWorker protected: void path_planning_thread_worker(); - void update_map_callback(const PathPlanningResources& buff); - void path_planning_callback(const PathPlanningResources& buff, PoseStampedMsg& target); + void updateMap(const PathPlanningResources& buff); + void planPath(const PathPlanningResources& buff, PoseStampedMsg& target); protected: RclNode& node; @@ -106,6 +110,8 @@ class PathPlanningWorker util::GenericPubMap pub_map; std::string odom_frame; + float passive_crop_horizontal_range{0.f}; + float passive_crop_vertical_range{0.f}; std::atomic threads_running{false}; std::atomic srv_enable_state{false}; From a9e54a8be88b611ba2257b559253df9d950316dd Mon Sep 17 00:00:00 2001 From: S1ink Date: Wed, 14 Jan 2026 20:10:39 -0600 Subject: [PATCH 5/8] performance tuning --- include/modules/impl/kfc_map_impl.hpp | 2 +- include/modules/impl/map_octree_impl.hpp | 43 +++++++++--- include/modules/impl/path_planner_impl.hpp | 55 ++++++++++++--- include/modules/map_octree.hpp | 2 + include/modules/path_plan_map.hpp | 78 ++++++++-------------- include/modules/ue_octree.hpp | 49 +++++++++----- src/core/threads/mapping_worker.cpp | 3 + src/core/threads/path_planning_worker.cpp | 11 +++ 8 files changed, 155 insertions(+), 88 deletions(-) diff --git a/include/modules/impl/kfc_map_impl.hpp b/include/modules/impl/kfc_map_impl.hpp index 0eba754..87a2e5d 100644 --- a/include/modules/impl/kfc_map_impl.hpp +++ b/include/modules/impl/kfc_map_impl.hpp @@ -280,7 +280,7 @@ typename KFCMap::UpdateResult KFCMap::updateMap( itr != buff.submap_remove_indices.end(); itr++) { - this->map_octree.deletePoint(*itr); + this->map_octree.deletePoint(*itr, true); } // add source points this->map_octree.addPoints(pts, &buff.points_to_add); diff --git a/include/modules/impl/map_octree_impl.hpp b/include/modules/impl/map_octree_impl.hpp index 8aae79d..f3fc660 100644 --- a/include/modules/impl/map_octree_impl.hpp +++ b/include/modules/impl/map_octree_impl.hpp @@ -164,7 +164,8 @@ const typename MapOctree::PointCloudT& MapOctree::points() } template -typename MapOctree::PointT& MapOctree::pointAt(pcl::index_t pt_idx) +typename MapOctree::PointT& MapOctree::pointAt( + pcl::index_t pt_idx) { const size_t pt_idx_ = static_cast(pt_idx); assert(pt_idx_ < this->cloud_buff->size()); @@ -172,6 +173,12 @@ typename MapOctree::PointT& MapOctree::pointAt(pcl::index_t pt return (*this->cloud_buff)[pt_idx_]; } +template +size_t MapOctree::octreeSize() const +{ + return this->leaf_count_ + this->branch_count_; +} + template size_t MapOctree::addPoint( const PointT& pt, @@ -412,11 +419,17 @@ void MapOctree::crop( template void MapOctree::optimizeStorage() { + // std::cout << "MapOctree::optimizeStorage() => start pts : " + // << this->cloud_buff->points.size() + // << ", holes : " << this->hole_indices.size() << std::endl; + // std::cout << "exhibit a" << std::endl; - if (this->hole_indices.size() >= this->cloud_buff->size()) + auto& points = this->cloud_buff->points; + + if (this->hole_indices.size() >= points.size()) { - this->cloud_buff->clear(); + points.clear(); if constexpr (HAS_POINT_STAMPS) { this->pt_stamps.clear(); @@ -431,9 +444,8 @@ void MapOctree::optimizeStorage() // std::cout << "exhibit b" << std::endl; - const size_t target_len = - this->cloud_buff->size() - this->hole_indices.size(); - int64_t end_idx = static_cast(this->cloud_buff->size()) - 1; + const size_t target_len = points.size() - this->hole_indices.size(); + int64_t end_idx = static_cast(points.size()) - 1; // std::cout << "exhibit c" << std::endl; @@ -444,9 +456,8 @@ void MapOctree::optimizeStorage() LeafContainerT* pt_idx = nullptr; while (end_idx >= 0 && - (!pcl::isFinite((*this->cloud_buff)[end_idx]) || - !(pt_idx = - this->getOctreePoint((*this->cloud_buff)[end_idx], key)))) + (!pcl::isFinite(points[end_idx]) || + !(pt_idx = this->getOctreePoint(points[end_idx], key)))) { end_idx--; } @@ -467,7 +478,7 @@ void MapOctree::optimizeStorage() if (pt_idx->getSize() > 0 && pt_idx->getPointIndex() == end_idx) { { - (*this->cloud_buff)[idx] = (*this->cloud_buff)[end_idx]; + points[idx] = points[end_idx]; if constexpr (HAS_POINT_STAMPS) { this->pt_stamps[idx] = this->pt_stamps[end_idx]; @@ -491,7 +502,13 @@ void MapOctree::optimizeStorage() // std::cout << "exhibit h" << std::endl; - this->cloud_buff->resize(end_idx + 1); + points.resize(end_idx + 1); + this->cloud_buff->width = points.size(); + // if(points.size() <= points.capacity() / 2) + // { + // points.shrink_to_fit(); + // } + if constexpr (HAS_POINT_STAMPS) { this->pt_stamps.resize(end_idx + 1); @@ -503,6 +520,10 @@ void MapOctree::optimizeStorage() this->hole_indices.clear(); // std::cout << "exhibit i" << std::endl; + + // std::cout << "MapOctree::optimizeStorage() => end pts : " + // << this->cloud_buff->points.size() + // << ", holes : " << this->hole_indices.size() << std::endl; } diff --git a/include/modules/impl/path_planner_impl.hpp b/include/modules/impl/path_planner_impl.hpp index 03a5b04..09e5019 100644 --- a/include/modules/impl/path_planner_impl.hpp +++ b/include/modules/impl/path_planner_impl.hpp @@ -119,17 +119,54 @@ bool PathPlanner

::solvePath( const PathPlanMapT& map, const WeightT max_weight) { - // TODO: add passthrough mode - const typename PathPlanMapT::UEOctreeT& ue_space = map.getUESpace(); + const PointCloudT& map_points = map.getPoints(); pcl::Indices tmp_indices; std::vector tmp_dists; + // if (!path.empty()) + // { + // const auto points_ptr = util::wrapUnmanaged(map.getPoints()); + // this->kdtree.setInputCloud(points_ptr); + + // size_t start_idx = 0; + // for (; start_idx + 1 < path.size(); start_idx++) + // { + // const auto& prev = path[start_idx]; + // const auto& curr = path[start_idx + 1]; + + // Vec3f diff = curr - prev; + // float proj = (diff.dot(start - prev)) / diff.squaredNorm(); + + // if (proj < 1.f) + // { + // break; + // } + // } + // for (; start_idx < path.size(); start_idx++) + // { + // const auto& pt = path[start_idx]; + // this->kdtree.radiusSearch( + // PointT{pt.x(), pt.y(), pt.z()}, + // this->search_radius, + // tmp_indices, + // tmp_dists); + // for(const pcl::index_t i : tmp_indices) + // { + // const auto& map_pt = map_points[i]; + // if(isWeighted(map_pt) && weight(map_pt) > max_weight) + // { + + // } + // } + // } + // } + // 1. Filter out non-traversible points and build KDTree this->points.points.clear(); - this->points.points.reserve(map.getPoints().size()); - for (const PointT& pt : map.getPoints()) + this->points.points.reserve(map_points.size()); + for (const PointT& pt : map_points) { if (isWeighted(pt) && weight(pt) <= max_weight /*|| isFrontier(pt)*/) { @@ -158,7 +195,7 @@ bool PathPlanner

::solvePath( { if (this->kdtree.nearestKSearch(goal_pt, 1, tmp_indices, tmp_dists)) { - goal_pt = points[tmp_indices[0]]; + goal_pt = this->points[tmp_indices[0]]; if (std::sqrt(tmp_dists[0]) > this->search_radius) { DEBUG_COUT( @@ -284,7 +321,7 @@ bool PathPlanner

::solvePath( ue_space.distToUnexplored( nb.position(), this->boundary_radius) <= this->boundary_radius; - if(!found_frontier && is_frontier) + if (!found_frontier && is_frontier) { closest_dist = nb.h; closest_idx = nb_idx; @@ -292,14 +329,14 @@ bool PathPlanner

::solvePath( } else { - if(nb.h < closest_dist && (!found_frontier || is_frontier)) + if (nb.h < closest_dist && (!found_frontier || is_frontier)) { closest_dist = nb.h; closest_idx = nb_idx; } } - if(!open.contains(nb_idx)) + if (!open.contains(nb_idx)) { open.push(nb_idx, nb.f()); } @@ -311,7 +348,7 @@ bool PathPlanner

::solvePath( } } - if(closest_idx >= 0) + if (closest_idx >= 0) { create_path(this->nodes[closest_idx]); return found_frontier; diff --git a/include/modules/map_octree.hpp b/include/modules/map_octree.hpp index a81efa4..99b54f3 100644 --- a/include/modules/map_octree.hpp +++ b/include/modules/map_octree.hpp @@ -202,6 +202,8 @@ class MapOctree : const PointCloudT& points() const; PointT& pointAt(pcl::index_t i); + size_t octreeSize() const; + size_t addPoint( const PointT& pt, uint64_t stamp = 0, diff --git a/include/modules/path_plan_map.hpp b/include/modules/path_plan_map.hpp index 21e605e..8b7bf29 100644 --- a/include/modules/path_plan_map.hpp +++ b/include/modules/path_plan_map.hpp @@ -94,6 +94,7 @@ class PathPlanMap const PointCloudT& getPoints() const; const UEOctreeT& getUESpace() const; + const MapOctreeT& getMap() const; protected: UEOctreeT ue_octree; @@ -108,7 +109,7 @@ class PathPlanMap template PathPlanMap

::PathPlanMap(float vox_sz) : - ue_octree{vox_sz * 0.5f}, + ue_octree{vox_sz}, map_octree{vox_sz} { } @@ -150,7 +151,7 @@ void PathPlanMap

::append( std::vector tmp_dists; typename PointCloudT::VectorType merge_pts; - const auto& map_pts_vec = this->map_octree.getInputCloud()->points; + const auto& map_pts_vec = this->map_octree.points().points; // 1. If the update region is large enough to have space that doesn't // intersect the merge window, then remove it first. @@ -181,61 +182,28 @@ void PathPlanMap

::append( // 3. Insert new points this->map_octree.addPoints(pts); + // TODO: fix weird artifacts when doing this: // 4. Update trav scores for old-new intersecting voxels (in window) - for (const auto& pt : merge_pts) - { - if ((isWeighted(pt) || isObstacle(pt)) && - this->map_octree.radiusSearch(pt, res * 0.5f, tmp_indices, tmp_dists)) - { - for(const pcl::index_t i : tmp_indices) - { - PointT& new_pt = this->map_octree.pointAt(i); - if (weight(pt) > weight(new_pt)) - { - weight(new_pt) = weight(pt); - } - } - } - tmp_indices.clear(); - } + // for (const auto& pt : merge_pts) + // { + // if ((isWeighted(pt) || isObstacle(pt)) && + // this->map_octree.radiusSearch(pt, res * 0.5f, tmp_indices, tmp_dists)) + // { + // for(const pcl::index_t i : tmp_indices) + // { + // PointT& new_pt = this->map_octree.pointAt(i); + // if (weight(pt) > weight(new_pt)) + // { + // weight(new_pt) = weight(pt); + // } + // } + // } + // tmp_indices.clear(); + // } // 5. Update u-e octree, insert new frontier points this->ue_octree.addExploredSpace(bound_min, bound_max); - // const Arr3f length3 = (bound_max - bound_min).array(); - // const Arr3f innerlen3 = (length3 / res).floor() * res; - // const Arr3f margin3 = (length3 - innerlen3) * 0.5f; - // const Vec3f start3 = bound_min + margin3.matrix(); - - // PointT a{}, b{}; - // weight(a) = weight(b) = FRONTIER_MARKER_VAL; - - /* -#define EXPLORE_PARALLEL_SIDES(U, V, W) \ - a.U = bound_min.U() - this->frontier_offset; \ - b.U = bound_max.U() + this->frontier_offset; \ - for (a.V = b.V = start3.V(); a.V < bound_max.V(); a.V = (b.V += res)) \ - { \ - for (a.W = b.W = start3.W(); a.W < bound_max.W(); a.W = (b.W += res)) \ - { \ - if (!this->ue_octree.isExplored(a.getVector3fMap())) \ - { \ - this->map_octree.addPoint(a); \ - } \ - if (!this->ue_octree.isExplored(b.getVector3fMap())) \ - { \ - this->map_octree.addPoint(b); \ - } \ - } \ - } - */ - - // EXPLORE_PARALLEL_SIDES(x, y, z) - // EXPLORE_PARALLEL_SIDES(y, x, z) - // EXPLORE_PARALLEL_SIDES(z, x, y) - -// #undef EXPLORE_PARALLEL_SIDES - // 6. Optimize point layout this->map_octree.optimizeStorage(); } @@ -259,5 +227,11 @@ const typename PathPlanMap

::UEOctreeT& PathPlanMap

::getUESpace() const return this->ue_octree; } +template +const typename PathPlanMap

::MapOctreeT& PathPlanMap

::getMap() const +{ + return this->map_octree; +} + }; // namespace perception }; // namespace csm diff --git a/include/modules/ue_octree.hpp b/include/modules/ue_octree.hpp index 46c52cd..8168e2e 100644 --- a/include/modules/ue_octree.hpp +++ b/include/modules/ue_octree.hpp @@ -40,7 +40,9 @@ #pragma once #include +#include #include +#include #include #include @@ -71,7 +73,7 @@ class UEOctree public: UEOctree(FloatT max_res); - ~UEOctree() = default; + ~UEOctree(); public: void clear(); @@ -83,7 +85,9 @@ class UEOctree bool isExplored(const Vec3f& pt) const; FloatT distToUnexplored(const Vec3f& pt, FloatT max_dist) const; - inline FloatT resolution() const { return vox_res; } + inline FloatT resolution() const { return this->vox_res; } + inline size_t treeDepth() const { return this->root_height; } + inline size_t allocEstimate() const { return this->alloc_estimate; } protected: struct Node @@ -92,10 +96,10 @@ class UEOctree bool explored{false}; Node() = default; - inline ~Node() { this->clear(); } + inline ~Node() = default; - void init(); - void clear(); + void init(UEOctree*); + void clear(UEOctree*); Node& operator[](size_t i); const Node& operator[](size_t i) const; @@ -144,6 +148,8 @@ class UEOctree Node root; size_t root_height{0}; + + size_t alloc_estimate{0}; }; @@ -155,10 +161,16 @@ UEOctree::UEOctree(FloatT res) : vox_res{res} { } +template +UEOctree::~UEOctree() +{ + this->root.clear(this); +} + template void UEOctree::clear() { - this->root.clear(); + this->root.clear(this); this->root_height = 0; this->vox_span = 0; } @@ -323,20 +335,27 @@ typename UEOctree::FloatT UEOctree::distToUnexplored( template -void UEOctree::Node::init() +void UEOctree::Node::init(UEOctree* inst) { if (this->isNull()) { this->children = new Node[8]; + inst->alloc_estimate += sizeof(Node) * 8; } } template -void UEOctree::Node::clear() +void UEOctree::Node::clear(UEOctree* inst) { if (this->children) { + for(size_t i = 0; i < 8; i++) + { + this->children[i].clear(inst); + } + delete[] this->children; this->children = nullptr; + inst->alloc_estimate -= sizeof(Node) * 8; } } @@ -533,7 +552,7 @@ void UEOctree::recursiveExplore( const NodeDescriptor& descriptor, const Box3f& zone) { - node.init(); + node.init(this); bool all_explored = true; for (size_t i = 0; i < 8; i++) { @@ -544,7 +563,7 @@ void UEOctree::recursiveExplore( if (zone.contains(child_span)) { child.explored = true; - child.clear(); + child.clear(this); } else if (zone.intersects(child_span)) { @@ -555,7 +574,7 @@ void UEOctree::recursiveExplore( else { child.explored = true; - child.clear(); + child.clear(this); } } @@ -565,7 +584,7 @@ void UEOctree::recursiveExplore( if (all_explored) { node.explored = true; - node.clear(); + node.clear(this); } } @@ -585,7 +604,7 @@ void UEOctree::recursiveCrop( { // not leaf --> expand children and recurse const bool was_explored = node.explored; - node.init(); + node.init(this); node.explored = false; bool all_explored = true; @@ -606,7 +625,7 @@ void UEOctree::recursiveCrop( if (all_explored) { node.explored = true; - node.clear(); + node.clear(this); } } // leaf --> leave alone @@ -615,7 +634,7 @@ void UEOctree::recursiveCrop( { // not contained and not intersected --> delete node.explored = false; - node.clear(); + node.clear(this); } } } diff --git a/src/core/threads/mapping_worker.cpp b/src/core/threads/mapping_worker.cpp index d5c442d..9f7eb13 100644 --- a/src/core/threads/mapping_worker.cpp +++ b/src/core/threads/mapping_worker.cpp @@ -332,6 +332,9 @@ void MappingWorker::mapping_callback(MappingResources& buff) this->pub_map.publish( "metrics/mapping/points_deleted", static_cast(results.points_deleted)); + this->pub_map.publish( + "metrics/mapping/octree_size", + this->sparse_map.getMap().octreeSize()); PROFILING_NOTIFY(mapping_debpub); } diff --git a/src/core/threads/path_planning_worker.cpp b/src/core/threads/path_planning_worker.cpp index 161dd60..f782adb 100644 --- a/src/core/threads/path_planning_worker.cpp +++ b/src/core/threads/path_planning_worker.cpp @@ -167,6 +167,7 @@ void PathPlanningWorker::path_planning_thread_worker() } PROFILING_NOTIFY_ALWAYS(path_planning); + PROFILING_FLUSH_LOCAL(); } // while (this->threads_running.load()); @@ -198,6 +199,16 @@ void PathPlanningWorker::updateMap(const PathPlanningResources& buff) msg.header.frame_id = this->odom_frame; msg.header.stamp = util::toTimeMsg(buff.stamp); this->pub_map.publish("pplan_map", msg); + + this->pub_map.publish( + "metrics/pplan/map_octree_size", + this->pplan_map.getMap().octreeSize()); + this->pub_map.publish( + "metrics/pplan/ue_octree_depth", + this->pplan_map.getUESpace().treeDepth()); + this->pub_map.publish( + "metrics/pplan/ue_octree_alloc", + this->pplan_map.getUESpace().allocEstimate()); } void PathPlanningWorker::planPath( From c8c4e5469b07951a0bcf2c24da0a473a3aef43a2 Mon Sep 17 00:00:00 2001 From: S1ink Date: Thu, 15 Jan 2026 01:13:35 -0600 Subject: [PATCH 6/8] pplan segment straightness weighting --- config/perception.json | 10 +++++--- include/modules/impl/path_planner_impl.hpp | 29 +++++++++++++-------- include/modules/path_planner.hpp | 16 ++++++++---- src/core/perception_core.cpp | 30 ++++++++++++++-------- 4 files changed, 55 insertions(+), 30 deletions(-) diff --git a/config/perception.json b/config/perception.json index c4b7593..88352f1 100644 --- a/config/perception.json +++ b/config/perception.json @@ -173,9 +173,13 @@ "boundary_radius": 0.15, "goal_threshold": 0.1, "search_radius": 0.5, - "lambda_distance": 1.0, - "lambda_penalty": 1.0, - "max_neighbors": 11 + "distance_coeff": 1.0, + "straightness_coeff": 2.0, + "traversibility_coeff": 1.0, + "max_neighbors": 11, + "map_obstacle_merge_window": 0.5, + "map_passive_crop_horizontal_range": 10.0, + "map_passive_crop_vertical_range": 5.0 } }, "sim": diff --git a/include/modules/impl/path_planner_impl.hpp b/include/modules/impl/path_planner_impl.hpp index 09e5019..a938c33 100644 --- a/include/modules/impl/path_planner_impl.hpp +++ b/include/modules/impl/path_planner_impl.hpp @@ -86,15 +86,17 @@ void PathPlanner

::setParameters( float boundary_radius, float goal_threshold, float search_radius, - float lambda_dist, - float lambda_penalty, + float distance_coeff, + float straightness_coeff, + float traversibility_coeff, size_t max_neighbors) { this->boundary_radius = boundary_radius; this->goal_threshold = goal_threshold; this->search_radius = search_radius; - this->lambda_dist = lambda_dist; - this->lambda_penalty = lambda_penalty; + this->distance_coeff = distance_coeff; + this->straightness_coeff = straightness_coeff; + this->traversibility_coeff = traversibility_coeff; this->max_neighbors = max_neighbors; } @@ -218,7 +220,7 @@ bool PathPlanner

::solvePath( // compute h as proportional to straight-line goal distance this->nodes.emplace_back( pt, - this->lambda_dist * (goal - pt.getVector3fMap()).norm()); + this->distance_coeff * (goal - pt.getVector3fMap()).norm()); } // 4. Find start node @@ -231,6 +233,7 @@ bool PathPlanner

::solvePath( { start_idx = tmp_indices[0]; this->nodes[start_idx].g = 0.f; + this->nodes[start_idx].dir.setZero(); } else { @@ -307,13 +310,17 @@ bool PathPlanner

::solvePath( // continue; // } - const float geom = (nb.position() - current.position()).norm(); - const float edge = - this->lambda_dist * geom + this->lambda_penalty * nb.cost(); - const float tentative_g = current.g + edge; + const Vec3f diff = (nb.position() - current.position()); + const float dist = diff.norm(); + const float inv_dot = (1.f - diff.dot(current.dir) / dist); + const float edge_cost = (this->distance_coeff * dist) + + (this->straightness_coeff * inv_dot) + + (this->traversibility_coeff * nb.cost()); + const float tentative_g = current.g + edge_cost; if (tentative_g < nb.g) { + nb.dir = (diff / dist); nb.g = tentative_g; nb.parent = ¤t; @@ -441,7 +448,7 @@ bool PathPlanner

::solvePath( // for (size_t i = 0; i < trav_points.points.size(); ++i) // { // const auto& point = trav_points.points[i]; -// float h = lambda_dist * +// float h = distance_coeff * // (goal_pt.getVector3fMap() - point.getVector3fMap()).norm(); // this->nodes.emplace_back(point, h); // } @@ -540,7 +547,7 @@ bool PathPlanner

::solvePath( // // geometric edge length // const float geom = (nb.position() - current.position()).norm(); -// const float edge = lambda_dist * geom + lambda_penalty * nb.cost(); +// const float edge = distance_coeff * geom + traversibility_coeff * nb.cost(); // const float tentative_g = current.g + edge; // if (tentative_g < nb.g) diff --git a/include/modules/path_planner.hpp b/include/modules/path_planner.hpp index d10329c..6b7c5bc 100644 --- a/include/modules/path_planner.hpp +++ b/include/modules/path_planner.hpp @@ -85,6 +85,7 @@ class PathPlanner struct Node { const PointT& point; + Vec3f dir; // previous direction vec float g; // cost from start to this node float h; // heuristic cost to goal Node* parent = nullptr; @@ -113,8 +114,9 @@ class PathPlanner float boundary_radius, float goal_threshold, float search_radius, - float lambda_dist, - float lambda_penalty, + float distance_coeff, + float straightness_coeff, + float traversibility_coeff, size_t max_neighbors = 10); bool solvePath( @@ -146,9 +148,13 @@ class PathPlanner float goal_threshold = 0.1f; // radius for neighbor search float search_radius = 0.5f; - // weights for cost model: edge_cost = lambda_d * dist + lambda_p * penalty - float lambda_dist = 1.f; - float lambda_penalty = 1.f; + // cost model : + // distance_coeff * (curr.pos - prev.pos).norm() + + // straightness_coeff * (1 - prev.dir.dot(curr.pos - prev.pos).normalized()) + + // traversibility_coeff * trav_weight + float distance_coeff = 1.f; + float straightness_coeff = 1.f; + float traversibility_coeff = 1.f; // maximum number of neighbors to consider size_t max_neighbors = 10; }; diff --git a/src/core/perception_core.cpp b/src/core/perception_core.cpp index 532830b..4e46c6d 100644 --- a/src/core/perception_core.cpp +++ b/src/core/perception_core.cpp @@ -159,8 +159,9 @@ struct PerceptionConfig float pplan_boundary_radius; float pplan_goal_thresh; float pplan_search_radius; - float pplan_lambda_dist; - float pplan_lambda_penalty; + float pplan_dist_coeff; + float pplan_dir_coeff; + float pplan_trav_coeff; float pplan_map_obstacle_merge_window; float pplan_map_passive_crop_horizontal_range; float pplan_map_passive_crop_vertical_range; @@ -341,9 +342,10 @@ std::ostream& operator<<(std::ostream& os, const PerceptionConfig& config) << " meters\n" << align("Goal Threshold") << config.pplan_goal_thresh << " meters\n" << align("Search Radius") << config.pplan_search_radius << " meters\n" - << align("Lambda Distance") << config.pplan_lambda_dist << " meters\n" - << align("Lambda Penalty") << config.pplan_lambda_penalty << "\n" - << align("Max Num Neighbors") << config.pplan_max_neighbors << "\n" + << align("Distance Coeff") << config.pplan_dist_coeff << "\n" + << align("Straightness Coeff") << config.pplan_dir_coeff << "\n" + << align("Traversibility Coeff") << config.pplan_trav_coeff << "\n" + << align("Max Num Neighbors") << config.pplan_max_neighbors << " points\n" << align("Map Merge Window") << config.pplan_map_obstacle_merge_window << " meters\n" << align("Map Hrz. Crop Range") @@ -719,13 +721,18 @@ void PerceptionNode::getParams(PerceptionConfig& config) 1.f); util::declare_param( this, - "pplan.lambda_distance", - config.pplan_lambda_dist, + "pplan.distance_coeff", + config.pplan_dist_coeff, 1.f); util::declare_param( this, - "pplan.lambda_penalty", - config.pplan_lambda_penalty, + "pplan.straightness_coeff", + config.pplan_dir_coeff, + 1.f); + util::declare_param( + this, + "pplan.traversibility_coeff", + config.pplan_trav_coeff, 1.f); util::declare_param( this, @@ -752,8 +759,9 @@ void PerceptionNode::getParams(PerceptionConfig& config) config.pplan_boundary_radius, config.pplan_goal_thresh, config.pplan_search_radius, - config.pplan_lambda_dist, - config.pplan_lambda_penalty, + config.pplan_dist_coeff, + config.pplan_dir_coeff, + config.pplan_trav_coeff, config.pplan_max_neighbors); #endif From 07c6291644f930e05ce629538e09752cb0690989 Mon Sep 17 00:00:00 2001 From: S1ink Date: Thu, 15 Jan 2026 21:43:08 -0600 Subject: [PATCH 7/8] my final commit* --- include/modules/impl/path_planner_impl.hpp | 409 ++++++++++++++------- include/modules/path_planner.hpp | 76 ++-- src/core/threads/path_planning_worker.cpp | 23 +- src/core/threads/path_planning_worker.hpp | 7 +- 4 files changed, 338 insertions(+), 177 deletions(-) diff --git a/include/modules/impl/path_planner_impl.hpp b/include/modules/impl/path_planner_impl.hpp index a938c33..b2a7c92 100644 --- a/include/modules/impl/path_planner_impl.hpp +++ b/include/modules/impl/path_planner_impl.hpp @@ -68,10 +68,26 @@ namespace perception template PathPlanner

::Node::Node(const P& point, float h, Node* p) : - point(point), - g(std::numeric_limits::infinity()), - h(h), - parent(p) + point{point}, + dir{}, + g{std::numeric_limits::infinity()}, + h{h}, + parent{p} +{ +} + +template +PathPlanner

::Node::Node( + const PointT& point, + const Vec3f& dir, + float g, + float h, + Node* p) : + point{point}, + dir{dir}, + g{g}, + h{h}, + parent{p} { } @@ -100,18 +116,18 @@ void PathPlanner

::setParameters( this->max_neighbors = max_neighbors; } -template -bool PathPlanner

::solvePath( - std::vector& path, - const Vec3f& start, - const Vec3f& goal, - const Vec3f& bound_min, - const Vec3f& bound_max, - const PointCloudT& points, - const WeightT max_weight) -{ - return false; -} +// template +// bool PathPlanner

::solvePath( +// std::vector& path, +// const Vec3f& start, +// const Vec3f& goal, +// const Vec3f& bound_min, +// const Vec3f& bound_max, +// const PointCloudT& points, +// const WeightT max_weight) +// { +// return false; +// } template bool PathPlanner

::solvePath( @@ -121,49 +137,17 @@ bool PathPlanner

::solvePath( const PathPlanMapT& map, const WeightT max_weight) { + // 0. Init state const typename PathPlanMapT::UEOctreeT& ue_space = map.getUESpace(); const PointCloudT& map_points = map.getPoints(); pcl::Indices tmp_indices; std::vector tmp_dists; + std::vector path_prefix; + PointT extra_pt_buff; + int start_idx = -1; - // if (!path.empty()) - // { - // const auto points_ptr = util::wrapUnmanaged(map.getPoints()); - // this->kdtree.setInputCloud(points_ptr); - - // size_t start_idx = 0; - // for (; start_idx + 1 < path.size(); start_idx++) - // { - // const auto& prev = path[start_idx]; - // const auto& curr = path[start_idx + 1]; - - // Vec3f diff = curr - prev; - // float proj = (diff.dot(start - prev)) / diff.squaredNorm(); - - // if (proj < 1.f) - // { - // break; - // } - // } - // for (; start_idx < path.size(); start_idx++) - // { - // const auto& pt = path[start_idx]; - // this->kdtree.radiusSearch( - // PointT{pt.x(), pt.y(), pt.z()}, - // this->search_radius, - // tmp_indices, - // tmp_dists); - // for(const pcl::index_t i : tmp_indices) - // { - // const auto& map_pt = map_points[i]; - // if(isWeighted(map_pt) && weight(map_pt) > max_weight) - // { - - // } - // } - // } - // } + this->nodes.clear(); // 1. Filter out non-traversible points and build KDTree this->points.points.clear(); @@ -188,12 +172,13 @@ bool PathPlanner

::solvePath( // is in the explored region (in the case which the goal was // non-traversible). PointT goal_pt{goal.x(), goal.y(), goal.z()}; - if (ue_space.isExplored(goal) && !this->kdtree.radiusSearch( - goal_pt, - this->goal_threshold, - tmp_indices, - tmp_dists, - 1)) + const bool goal_is_explored = ue_space.isExplored(goal); + if (goal_is_explored && !this->kdtree.radiusSearch( + goal_pt, + this->goal_threshold, + tmp_indices, + tmp_dists, + 1)) { if (this->kdtree.nearestKSearch(goal_pt, 1, tmp_indices, tmp_dists)) { @@ -212,57 +197,208 @@ bool PathPlanner

::solvePath( } } - // 3. Construct nodes - this->nodes.clear(); - this->nodes.reserve(this->points.size()); + // 3. Attempt to reuse previous path if provided + if (!path.empty()) + { + // 3-A. Ignore initial segments that are no longer relevant + size_t start_i = 0; + for (; start_i + 1 < path.size(); start_i++) + { + const auto& prev = path[start_i]; + const auto& curr = path[start_i + 1]; + + Vec3f diff = curr - prev; + float proj = (diff.dot(start - prev)) / diff.squaredNorm(); + + if (proj < 1.f) + { + break; + } + } + + // 3-B. Verify remaining segments + pcl::Indices prev_nearest; + float path_len = 0.f; + float min_goal_dist = std::numeric_limits::infinity(); + size_t i = start_i; + size_t prev_i = start_i; + size_t checkpt_i = start_i; + size_t goal_trim_i = start_i; + while (i < path.size()) + { + const auto& pt = path[i]; + + const float d_to_goal = (goal_pt.getVector3fMap() - pt).norm(); + if (d_to_goal < min_goal_dist) + { + goal_trim_i = i; + min_goal_dist = d_to_goal; + } + + tmp_indices.clear(); + if (!this->kdtree.nearestKSearch( + PointT{pt.x(), pt.y(), pt.z()}, + this->verification_degree, + tmp_indices, + tmp_dists)) + { + // no nearest pts - bad keypoint + goto BREAK_L; + } + + for (size_t j = 0; j < tmp_indices.size(); j++) + { + // test if the current keypoint is still valid + if (std::sqrt(tmp_dists[j]) > this->search_radius) + { + // bad keypoint + goto BREAK_L; + } + + // test if the prev-to-curr segment is still valid + // if prev is empty (init), this doesn't run (as required) + const auto& pt_a = this->points[tmp_indices[j]]; + for (const pcl::index_t k : prev_nearest) + { + const auto& pt_b = this->points[k]; + const float d = + (pt_a.getVector3fMap() - pt_b.getVector3fMap()).norm(); + if (d > this->search_radius) + { + // bad segment + goto BREAK_L; + } + } + } + + prev_nearest.swap(tmp_indices); + path_len += (pt - path[prev_i]).norm(); + if (checkpt_i == start_i && path_len >= this->verification_range) + { + checkpt_i = i; + } + prev_i = i; + i++; + continue; + + BREAK_L: + break; + } + + // 3-C. Analyze results + if (i >= path.size()) + { + // Verified entire path: extend if needed, otherwise exit + if ((goal_is_explored && min_goal_dist > this->goal_threshold) || + (!goal_is_explored && + ue_space.distToUnexplored(path.back(), this->boundary_radius) > + this->boundary_radius)) + { + // extend from path end + path_prefix.insert( + path_prefix.begin(), + path.begin() + start_i, + path.end() - 1); + + extra_pt_buff.getVector3fMap() = path.back(); + const Vec3f dir = + path.size() > 1 + ? (path.back() - path[path.size() - 2]).normalized() + : Vec3f::Zero(); + this->nodes.emplace_back( + extra_pt_buff, + dir, + 0.f, + this->distance_coeff * + (goal_pt.getVector3fMap() - path.back()).norm()); + start_idx = 0; + } + else + { + // trim unneeded keypoints + if (min_goal_dist <= this->goal_threshold) + { + path.erase(path.begin() + goal_trim_i + 1, path.end()); + } + path.erase(path.begin(), path.begin() + start_i); + return true; + } + } + else + { + // Error occurred somewhere: replan from checkpoint or from scratch + if (checkpt_i != start_i) + { + // replan from checkpt + path_prefix.insert( + path_prefix.begin(), + path.begin() + start_i, + path.begin() + checkpt_i); + + extra_pt_buff.getVector3fMap() = path[checkpt_i]; + const Vec3f dir = + checkpt_i > 0 + ? (path[checkpt_i] - path[checkpt_i - 1]).normalized() + : Vec3f::Zero(); + this->nodes.emplace_back( + extra_pt_buff, + dir, + 0.f, + this->distance_coeff * + (goal_pt.getVector3fMap() - path[checkpt_i]).norm()); + start_idx = 0; + } + else + { + // discard path and replan (continue as normal) + } + } + } + + // 4. Construct nodes + const size_t num_prefix_nodes = this->nodes.size(); + this->nodes.reserve(num_prefix_nodes + this->points.size()); for (const auto& pt : this->points) { // compute h as proportional to straight-line goal distance this->nodes.emplace_back( pt, - this->distance_coeff * (goal - pt.getVector3fMap()).norm()); + this->distance_coeff * + (goal_pt.getVector3fMap() - pt.getVector3fMap()).norm()); } - // 4. Find start node - int start_idx = -1; - if (this->kdtree.nearestKSearch( - PointT{start.x(), start.y(), start.z()}, - 1, - tmp_indices, - tmp_dists)) - { - start_idx = tmp_indices[0]; - this->nodes[start_idx].g = 0.f; - this->nodes[start_idx].dir.setZero(); - } - else + // 5. Init start node if not already done (snap to pointset) + if (start_idx < 0) { - DEBUG_COUT("Error - could not snap start node to available points!"); - return false; + if (this->kdtree.nearestKSearch( + PointT{start.x(), start.y(), start.z()}, + 1, + tmp_indices, + tmp_dists)) + { + start_idx = static_cast(num_prefix_nodes) + tmp_indices[0]; + this->nodes[start_idx].g = 0.f; + this->nodes[start_idx].dir.setZero(); + } + else + { + DEBUG_COUT( + "Error - could not snap start node to available points!"); + return false; + } } - // 5. Setup + // 6. Setup util::DAryHeap open(static_cast(this->nodes.size())); open.reserve_heap(this->nodes.size()); open.push(start_idx, this->nodes[start_idx].f()); std::vector closed(this->nodes.size(), false); - int closest_idx = start_idx; - float closest_dist = this->nodes[start_idx].h; - bool found_frontier = false; + int closest_idx = -1; + float closest_dist = std::numeric_limits::infinity(); - auto create_path = [&](Node& path_end) - { - path.clear(); - for (Node* n = &path_end; n != nullptr; n = n->parent) - { - path.push_back(n->position()); - } - std::reverse(path.begin(), path.end()); - }; - - // 6. A* body + // 7. A* body while (!open.empty()) { const int idx = open.pop(); @@ -277,11 +413,12 @@ bool PathPlanner

::solvePath( closed[idx] = true; } - if ((current.position() - goal_pt.getVector3fMap()).norm() < - this->goal_threshold) + // if ((current.position() - goal_pt.getVector3fMap()).norm() < + // this->goal_threshold) + if (current.h < this->goal_threshold * this->distance_coeff) { - create_path(current); - return true; + closest_idx = idx; + break; } if (current.neighbors.empty()) @@ -295,20 +432,16 @@ bool PathPlanner

::solvePath( this->max_neighbors); } - for (const pcl::index_t nb_idx : current.neighbors) + for (const pcl::index_t nb_idx_ : current.neighbors) { + const size_t nb_idx = + num_prefix_nodes + static_cast(nb_idx_); Node& nb = this->nodes[nb_idx]; if (closed[nb_idx]) { continue; } - // this is prefiltered - // else if (nb.cost() > max_weight) - // { - // closed[nb_idx] = true; - // continue; - // } const Vec3f diff = (nb.position() - current.position()); const float dist = diff.norm(); @@ -324,25 +457,6 @@ bool PathPlanner

::solvePath( nb.g = tentative_g; nb.parent = ¤t; - const bool is_frontier = - ue_space.distToUnexplored( - nb.position(), - this->boundary_radius) <= this->boundary_radius; - if (!found_frontier && is_frontier) - { - closest_dist = nb.h; - closest_idx = nb_idx; - found_frontier = true; - } - else - { - if (nb.h < closest_dist && (!found_frontier || is_frontier)) - { - closest_dist = nb.h; - closest_idx = nb_idx; - } - } - if (!open.contains(nb_idx)) { open.push(nb_idx, nb.f()); @@ -351,14 +465,47 @@ bool PathPlanner

::solvePath( { open.decrease_key(nb_idx, nb.f()); } + + const bool is_frontier = + ue_space.distToUnexplored( + nb.position(), + this->boundary_radius) <= this->boundary_radius; + + // We already snapped the goal to the nearest point in the set + // as long as it was in explored range, thus if the goal is + // reachable, it will trigger the exit condition above + // (proximity). This implies that we should only track the + // closest frontier node - either the goal was outside explored + // range or there is an obstacle blocking it; either way we need + // to keep exploring (ie. traverse to a frontier point). If we + // can't reach any frontier nodes [and goal is not reached], + // then the function should return false, meaning there is no + // meaningful path to follow. Thus, the return state is only + // correct if we limit closest_idx to being a frontier node here! + if (is_frontier && nb.h < closest_dist) + { + closest_idx = nb_idx; + closest_dist = nb.h; + } } } } + // 8. Export if (closest_idx >= 0) { - create_path(this->nodes[closest_idx]); - return found_frontier; + path.clear(); + for (Node* n = &this->nodes[closest_idx]; n != nullptr; n = n->parent) + { + path.push_back(n->position()); + } + if (!path_prefix.empty()) + { + path.insert(path.end(), path_prefix.rbegin(), path_prefix.rend()); + } + std::reverse(path.begin(), path.end()); + + return true; } return false; @@ -454,15 +601,15 @@ bool PathPlanner

::solvePath( // } // // find start node -// int start_idx; +// int start_i; // if (this->kdtree.nearestKSearch( // PointT{start.x(), start.y(), start.z()}, // 1, // tmp_indices, // tmp_dists)) // { -// start_idx = tmp_indices[0]; -// this->nodes[start_idx].g = 0.f; +// start_i = tmp_indices[0]; +// this->nodes[start_i].g = 0.f; // } // else // { @@ -473,15 +620,15 @@ bool PathPlanner

::solvePath( // // create open heap over f = g + h // util::DAryHeap open(static_cast(this->nodes.size())); // open.reserve_heap(this->nodes.size()); -// open.push(start_idx, this->nodes[start_idx].f()); +// open.push(start_i, this->nodes[start_i].f()); // // closed set // std::vector closed(this->nodes.size(), false); // // keep track of closest visited node to goal -// int closest_idx = start_idx; +// int closest_idx = start_i; // // h proportional to distance to goal -// float closest_dist = this->nodes[start_idx].h; +// float closest_dist = this->nodes[start_i].h; // bool found_boundary = false; // const Box3f outside_boundary{ diff --git a/include/modules/path_planner.hpp b/include/modules/path_planner.hpp index 6b7c5bc..e9b8f54 100644 --- a/include/modules/path_planner.hpp +++ b/include/modules/path_planner.hpp @@ -81,31 +81,6 @@ class PathPlanner using Vec3f = Eigen::Vector3f; using Box3f = Eigen::AlignedBox3f; -private: - struct Node - { - const PointT& point; - Vec3f dir; // previous direction vec - float g; // cost from start to this node - float h; // heuristic cost to goal - Node* parent = nullptr; - pcl::Indices neighbors; - - Node(const PointT& point, float h = 0.0f, Node* p = nullptr); - - // total cost - inline float f() const { return this->g + this->h; } - // cost of this node - inline WeightT cost() const - { - return traversibility::weight(this->point); - } - inline auto position() const - { - return this->point.getVector3fMap(); - } - }; - public: PathPlanner(); ~PathPlanner() = default; @@ -119,23 +94,50 @@ class PathPlanner float traversibility_coeff, size_t max_neighbors = 10); - bool solvePath( - std::vector& path, - const Vec3f& start, - const Vec3f& goal, - const Vec3f& local_bound_min, - const Vec3f& local_bound_max, - const PointCloudT& trav_points, - const WeightT max_weight = - traversibility::NOMINAL_MAX_WEIGHT); + // bool solvePath( + // std::vector& path, + // const Vec3f& start, + // const Vec3f& goal, + // const Vec3f& local_bound_min, + // const Vec3f& local_bound_max, + // const PointCloudT& trav_points, + // const WeightT max_weight = + // traversibility::NOMINAL_MAX_WEIGHT); bool solvePath( std::vector& path, const Vec3f& start, const Vec3f& goal, const PathPlanMapT& map, - const WeightT max_weight = - traversibility::NOMINAL_MAX_WEIGHT); + const WeightT max_weight = traversibility::NOMINAL_MAX_WEIGHT); + +private: + struct Node + { + const PointT& point; + Vec3f dir; // previous direction vec + float g; // cost from start to this node + float h; // heuristic cost to goal + Node* parent = nullptr; + pcl::Indices neighbors; + + Node(const PointT& point, float h = 0.f, Node* p = nullptr); + Node( + const PointT& point, + const Vec3f& dir, + float g = 0.f, + float h = 0.f, + Node* p = nullptr); + + // total cost + inline float f() const { return this->g + this->h; } + // cost of this node + inline WeightT cost() const + { + return traversibility::weight(this->point); + } + inline auto position() const { return this->point.getVector3fMap(); } + }; private: PointCloudT points; @@ -148,6 +150,8 @@ class PathPlanner float goal_threshold = 0.1f; // radius for neighbor search float search_radius = 0.5f; + float verification_range = 1.5f; + int verification_degree = 2; // cost model : // distance_coeff * (curr.pos - prev.pos).norm() + // straightness_coeff * (1 - prev.dir.dot(curr.pos - prev.pos).normalized()) + diff --git a/src/core/threads/path_planning_worker.cpp b/src/core/threads/path_planning_worker.cpp index f782adb..04a7396 100644 --- a/src/core/threads/path_planning_worker.cpp +++ b/src/core/threads/path_planning_worker.cpp @@ -60,8 +60,6 @@ using namespace util::geom::cvt::ops; -using Vec3f = Eigen::Vector3f; - using PathMsg = nav_msgs::msg::Path; using PointCloudMsg = sensor_msgs::msg::PointCloud2; @@ -105,8 +103,9 @@ void PathPlanningWorker::accept( } else { - this->srv_enable_state = true; this->pplan_target_notifier.updateAndNotify(req->target); + this->need_clear_buffered = true; + this->srv_enable_state = true; } resp->running = this->srv_enable_state; @@ -246,19 +245,25 @@ void PathPlanningWorker::planPath( Vec3f odom_target; odom_target << target.pose.position; - std::vector path; + if (this->need_clear_buffered) + { + this->path.clear(); + this->need_clear_buffered = false; + } + + std::vector prev_path = this->path; for (auto max_weight = NOMINAL_MAX_WEIGHT; isWeighted(max_weight) && !this->path_planner.solvePath( - path, + this->path, buff.base_to_odom.pose.vec, odom_target, this->pplan_map, max_weight); max_weight *= 2) { - path.clear(); + this->path = prev_path; } - if(path.empty()) + if (this->path.empty()) { // Fail return; @@ -268,8 +273,8 @@ void PathPlanningWorker::planPath( path_msg.header.frame_id = this->odom_frame; path_msg.header.stamp = util::toTimeMsg(buff.stamp); - path_msg.poses.reserve(path.size()); - for (const Vec3f& kp : path) + path_msg.poses.reserve(this->path.size()); + for (const Vec3f& kp : this->path) { PoseStampedMsg& pose = path_msg.poses.emplace_back(); pose.pose.position << kp; diff --git a/src/core/threads/path_planning_worker.hpp b/src/core/threads/path_planning_worker.hpp index fce97ee..8182c59 100644 --- a/src/core/threads/path_planning_worker.hpp +++ b/src/core/threads/path_planning_worker.hpp @@ -79,6 +79,8 @@ class PathPlanningWorker using UpdatePathPlanSrv = cardinal_perception::srv::UpdatePathPlanningMode; + using Vec3f = Eigen::Vector3f; + public: PathPlanningWorker(RclNode& node, const Tf2Buffer& tf_buffer); ~PathPlanningWorker(); @@ -115,12 +117,15 @@ class PathPlanningWorker std::atomic threads_running{false}; std::atomic srv_enable_state{false}; + std::atomic need_clear_buffered{false}; + + std::thread path_planning_thread; + std::vector path; PathPlanMap pplan_map; PathPlanner path_planner; util::ResourcePipeline pplan_target_notifier; util::ResourcePipeline path_planning_resources; - std::thread path_planning_thread; }; }; // namespace perception From 0db45eb19c92bcb643461e62eec70edff21011f2 Mon Sep 17 00:00:00 2001 From: S1ink Date: Thu, 15 Jan 2026 22:00:56 -0600 Subject: [PATCH 8/8] forgot to expose params womp --- config/perception.json | 2 ++ include/modules/impl/path_planner_impl.hpp | 6 +++++- include/modules/path_planner.hpp | 6 ++++-- src/core/perception_core.cpp | 16 ++++++++++++++++ 4 files changed, 27 insertions(+), 3 deletions(-) diff --git a/config/perception.json b/config/perception.json index 88352f1..80967ec 100644 --- a/config/perception.json +++ b/config/perception.json @@ -176,6 +176,8 @@ "distance_coeff": 1.0, "straightness_coeff": 2.0, "traversibility_coeff": 1.0, + "verification_range": 1.5, + "verification_degree": 2, "max_neighbors": 11, "map_obstacle_merge_window": 0.5, "map_passive_crop_horizontal_range": 10.0, diff --git a/include/modules/impl/path_planner_impl.hpp b/include/modules/impl/path_planner_impl.hpp index b2a7c92..af20a59 100644 --- a/include/modules/impl/path_planner_impl.hpp +++ b/include/modules/impl/path_planner_impl.hpp @@ -105,6 +105,8 @@ void PathPlanner

::setParameters( float distance_coeff, float straightness_coeff, float traversibility_coeff, + float verification_range, + size_t verification_degree, size_t max_neighbors) { this->boundary_radius = boundary_radius; @@ -113,6 +115,8 @@ void PathPlanner

::setParameters( this->distance_coeff = distance_coeff; this->straightness_coeff = straightness_coeff; this->traversibility_coeff = traversibility_coeff; + this->verification_range = verification_range; + this->verification_degree = verification_degree; this->max_neighbors = max_neighbors; } @@ -238,7 +242,7 @@ bool PathPlanner

::solvePath( tmp_indices.clear(); if (!this->kdtree.nearestKSearch( PointT{pt.x(), pt.y(), pt.z()}, - this->verification_degree, + static_cast(this->verification_degree), tmp_indices, tmp_dists)) { diff --git a/include/modules/path_planner.hpp b/include/modules/path_planner.hpp index e9b8f54..9cd5ef0 100644 --- a/include/modules/path_planner.hpp +++ b/include/modules/path_planner.hpp @@ -92,6 +92,8 @@ class PathPlanner float distance_coeff, float straightness_coeff, float traversibility_coeff, + float verification_range, + size_t verification_degree, size_t max_neighbors = 10); // bool solvePath( @@ -150,8 +152,6 @@ class PathPlanner float goal_threshold = 0.1f; // radius for neighbor search float search_radius = 0.5f; - float verification_range = 1.5f; - int verification_degree = 2; // cost model : // distance_coeff * (curr.pos - prev.pos).norm() + // straightness_coeff * (1 - prev.dir.dot(curr.pos - prev.pos).normalized()) + @@ -159,6 +159,8 @@ class PathPlanner float distance_coeff = 1.f; float straightness_coeff = 1.f; float traversibility_coeff = 1.f; + float verification_range = 1.5f; + size_t verification_degree = 2; // maximum number of neighbors to consider size_t max_neighbors = 10; }; diff --git a/src/core/perception_core.cpp b/src/core/perception_core.cpp index 4e46c6d..e74c167 100644 --- a/src/core/perception_core.cpp +++ b/src/core/perception_core.cpp @@ -162,9 +162,11 @@ struct PerceptionConfig float pplan_dist_coeff; float pplan_dir_coeff; float pplan_trav_coeff; + float pplan_verification_range; float pplan_map_obstacle_merge_window; float pplan_map_passive_crop_horizontal_range; float pplan_map_passive_crop_vertical_range; + int pplan_verification_degree; int pplan_max_neighbors; public: @@ -345,6 +347,8 @@ std::ostream& operator<<(std::ostream& os, const PerceptionConfig& config) << align("Distance Coeff") << config.pplan_dist_coeff << "\n" << align("Straightness Coeff") << config.pplan_dir_coeff << "\n" << align("Traversibility Coeff") << config.pplan_trav_coeff << "\n" + << align("Verification Range") << config.pplan_verification_range << " meters\n" + << align("Verification Degree") << config.pplan_verification_degree << " points\n" << align("Max Num Neighbors") << config.pplan_max_neighbors << " points\n" << align("Map Merge Window") << config.pplan_map_obstacle_merge_window << " meters\n" @@ -734,6 +738,16 @@ void PerceptionNode::getParams(PerceptionConfig& config) "pplan.traversibility_coeff", config.pplan_trav_coeff, 1.f); + util::declare_param( + this, + "pplan.verification_range", + config.pplan_verification_range, + 1.5f); + util::declare_param( + this, + "pplan.verification_degree", + config.pplan_verification_degree, + 2); util::declare_param( this, "pplan.max_neighbors", @@ -762,6 +776,8 @@ void PerceptionNode::getParams(PerceptionConfig& config) config.pplan_dist_coeff, config.pplan_dir_coeff, config.pplan_trav_coeff, + config.pplan_verification_range, + config.pplan_verification_degree, config.pplan_max_neighbors); #endif