diff --git a/config/perception.json b/config/perception.json index c1403f5..80967ec 100644 --- a/config/perception.json +++ b/config/perception.json @@ -173,9 +173,15 @@ "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, + "verification_range": 1.5, + "verification_degree": 2, + "max_neighbors": 11, + "map_obstacle_merge_window": 0.5, + "map_passive_crop_horizontal_range": 10.0, + "map_passive_crop_vertical_range": 5.0 } }, "sim": @@ -489,7 +495,7 @@ }, "robot_tf": { - "pragma:default": "lance", + "pragma:default": "lance2", "lance": { "robot_description": @@ -499,8 +505,6 @@ "links": [ "base_link", "frame_link", - "left_track_link", - "right_track_link", "collection_link", "lidar_link", "lidar_link_inv_rotated" @@ -512,18 +516,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 +553,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/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..87a2e5d 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()); @@ -284,7 +280,7 @@ typename KFCMap::UpdateResult 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 8170d3d..f3fc660 100644 --- a/include/modules/impl/map_octree_impl.hpp +++ b/include/modules/impl/map_octree_impl.hpp @@ -123,18 +123,64 @@ 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::octreeSize() const +{ + return this->leaf_count_ + this->branch_count_; +} + +template +size_t MapOctree::addPoint( const PointT& pt, uint64_t stamp, bool compute_normal) @@ -194,7 +240,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 +272,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 +329,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 +361,8 @@ void MapOctree::deletePoint( } } -template -void MapOctree::deletePoints( +template +void MapOctree::deletePoints( const pcl::Indices& indices, bool trim_nodes) { @@ -329,8 +373,8 @@ void MapOctree::deletePoints( } -template -void MapOctree::crop( +template +void MapOctree::crop( const Vec3f& min, const Vec3f& max, bool trim_nodes) @@ -372,14 +416,20 @@ void MapOctree::crop( } } -template -void MapOctree::optimizeStorage() +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(); @@ -394,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; @@ -405,11 +454,10 @@ 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 = - this->getOctreePoint((*this->cloud_buff)[end_idx], key)))) + (!pcl::isFinite(points[end_idx]) || + !(pt_idx = this->getOctreePoint(points[end_idx], key)))) { end_idx--; } @@ -430,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]; @@ -454,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); @@ -466,12 +520,16 @@ 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; } -template -bool MapOctree::mergePointFields( +template +bool MapOctree::mergePointFields( PointT& map_point, const PointT& new_point) { @@ -509,8 +567,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 +603,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 +624,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/impl/path_planner_impl.hpp b/include/modules/impl/path_planner_impl.hpp index 82f7c7d..af20a59 100644 --- a/include/modules/impl/path_planner_impl.hpp +++ b/include/modules/impl/path_planner_impl.hpp @@ -68,11 +68,26 @@ 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) + 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} { } @@ -87,151 +102,307 @@ 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, + float verification_range, + size_t verification_degree, 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->verification_range = verification_range; + this->verification_degree = verification_degree; 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& local_bound_min, - const Vec3f& local_bound_max, - const PointCloudT& trav_points, - std::vector& path) + const PathPlanMapT& map, + 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()) + // 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; + + this->nodes.clear(); + + // 1. Filter out non-traversible points and build KDTree + this->points.points.clear(); + this->points.points.reserve(map_points.size()); + for (const PointT& pt : map_points) { - throw std::out_of_range("Start point is out of bounds"); + if (isWeighted(pt) && weight(pt) <= max_weight /*|| isFrontier(pt)*/) + { + this->points.points.emplace_back(pt); + } } - if (trav_points.points.empty()) + 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()}; + 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)) { - throw std::invalid_argument("Input point cloud are empty"); + if (this->kdtree.nearestKSearch(goal_pt, 1, tmp_indices, tmp_dists)) + { + goal_pt = this->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; + } } -#endif - // temp buffers reused by radiusSearch - pcl::Indices tmp_indices; - std::vector tmp_dists; + // 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]; - auto shared_trav_points = util::wrapUnmanaged(trav_points); - this->kdtree.setInputCloud(shared_trav_points); + Vec3f diff = curr - prev; + float proj = (diff.dot(start - prev)) / diff.squaredNorm(); - 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 (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()) { - if (this->kdtree.nearestKSearch(goal_pt, 1, tmp_indices, tmp_dists)) + 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()}, + static_cast(this->verification_degree), + tmp_indices, + tmp_dists)) { - goal_pt = trav_points.points[tmp_indices[0]]; - if (std::sqrt(tmp_dists[0]) > this->search_radius) + // 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) { - DEBUG_COUT( - "Warning - path planning goal node snapped to available" - " node with distance greater than 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; + } } } - else + + prev_nearest.swap(tmp_indices); + path_len += (pt - path[prev_i]).norm(); + if (checkpt_i == start_i && path_len >= this->verification_range) { - return false; + checkpt_i = i; } + prev_i = i; + i++; + continue; + + BREAK_L: + break; } - } - // 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])) + // 3-C. Analyze results + if (i >= path.size()) { - goal_pt = trav_points.points[i]; - all_invalid = false; - break; + // 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) + } } - } - 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) + // 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) { - const auto& point = trav_points.points[i]; - float h = lambda_dist * - (goal_pt.getVector3fMap() - point.getVector3fMap()).norm(); - this->nodes.emplace_back(point, h); + // compute h as proportional to straight-line goal distance + this->nodes.emplace_back( + pt, + this->distance_coeff * + (goal_pt.getVector3fMap() - pt.getVector3fMap()).norm()); } - // 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 + // 5. Init start node if not already done (snap to pointset) + if (start_idx < 0) { - DEBUG_COUT("Error - could not initialize starting location!"); - 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; + } } - // create open heap over f = g + h + // 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()); - // 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()); - }; + int closest_idx = -1; + float closest_dist = std::numeric_limits::infinity(); + // 7. A* body while (!open.empty()) { const int idx = open.pop(); @@ -239,104 +410,350 @@ bool PathPlanner

::solvePath( if (closed[idx]) { - continue; // skip if already closed + continue; + } + else + { + closed[idx] = true; } - closed[idx] = true; - // Goal check (by geometry) - if ((current.position() - goal_pt.getVector3fMap()).norm() < - 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; } - // 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.point, + this->search_radius, current.neighbors, tmp_dists, - max_neighbors); + this->max_neighbors); } - // Relax neighbors - for (const int 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; } - 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 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; - const float edge = lambda_dist * geom + lambda_penalty * nb.cost; - - const float tentative_g = current.g + edge; if (tentative_g < nb.g) { + nb.dir = (diff / dist); nb.g = tentative_g; nb.parent = ¤t; - bool in_boundary = !outside_boundary.contains(nb.position()); - - if (!found_boundary && in_boundary) + if (!open.contains(nb_idx)) { - closest_dist = nb.h; - closest_idx = nb_idx; - found_boundary = true; + open.push(nb_idx, nb.f()); } - else if (nb.h < closest_dist) + else { - if (!found_boundary || in_boundary) - { - closest_dist = nb.h; - closest_idx = nb_idx; - } + open.decrease_key(nb_idx, nb.f()); } - const float new_f = nb.f(); - if (!open.contains(nb_idx)) + 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) { - open.push(nb_idx, new_f); - } - else - { - open.decrease_key(nb_idx, new_f); // strictly better + closest_idx = nb_idx; + closest_dist = nb.h; } } } } - // No path to goal: pick closest visited node to the goal + // 8. Export if (closest_idx >= 0) { - create_path(this->nodes[closest_idx]); - - if (!found_boundary) + path.clear(); + for (Node* n = &this->nodes[closest_idx]; n != nullptr; n = n->parent) { - DEBUG_COUT( - "No boundary nodes reached, returning closest node to goal"); + path.push_back(n->position()); + } + if (!path_prefix.empty()) + { + path.insert(path.end(), path_prefix.rbegin(), path_prefix.rend()); } - return found_boundary; + std::reverse(path.begin(), path.end()); + + return true; } - // 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 = distance_coeff * +// (goal_pt.getVector3fMap() - point.getVector3fMap()).norm(); +// this->nodes.emplace_back(point, h); +// } + +// // find start node +// int start_i; +// if (this->kdtree.nearestKSearch( +// PointT{start.x(), start.y(), start.z()}, +// 1, +// tmp_indices, +// tmp_dists)) +// { +// start_i = tmp_indices[0]; +// this->nodes[start_i].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_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_i; +// // h proportional to distance to goal +// float closest_dist = this->nodes[start_i].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.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 = distance_coeff * geom + traversibility_coeff * 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/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..99b54f3 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 { @@ -133,35 +137,38 @@ 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, - 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); + static_assert(pcl::traits::has_xyz::value); + + using PointT = Point_T; + using ChildT = Child_T; - using Super_T = pcl::octree::OctreePointCloudSearch; - using LeafContainer_T = typename Super_T::OctreeT::Base::LeafContainer; - using Derived_T = typename std::conditional< + 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 +182,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 +195,21 @@ 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 octreeSize() const; + 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 +223,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..8b7bf29 --- /dev/null +++ b/include/modules/path_plan_map.hpp @@ -0,0 +1,237 @@ +/******************************************************************************* +* 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); + +public: + 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 voxel_size = -1.f); + + void append( + const PointCloudT& pts, + const Vec3f& bound_min, + const Vec3f& bound_max); + + void crop(const Vec3f& min, const Vec3f& max); + + const PointCloudT& getPoints() const; + const UEOctreeT& getUESpace() const; + const MapOctreeT& getMap() const; + +protected: + UEOctreeT ue_octree; + MapOctreeT map_octree; + + float obstacle_merge_window{0.5f}; +}; + + + +// --- 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 voxel_size) +{ + this->obstacle_merge_window = obstacle_merge_window; + + 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) +{ + // TODO: frame-to-frame "bounds alignment" for more condistent map + + using namespace csm::perception::traversibility; + + 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.points().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); + tmp_indices.clear(); + } + + // 2. Copy out merge-window points, then remove from map + 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, false); + tmp_indices.clear(); + + // 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(); + // } + + // 5. Update u-e octree, insert new frontier points + this->ue_octree.addExploredSpace(bound_min, bound_max); + + // 6. Optimize point layout + 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(); +} + +template +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/path_planner.hpp b/include/modules/path_planner.hpp index d6ff09f..9cd5ef0 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,29 +74,13 @@ class PathPlanner public: using PointT = Point_T; using PointCloudT = pcl::PointCloud; + using PathPlanMapT = PathPlanMap; + + using WeightT = traversibility::weight_t; using Vec3f = Eigen::Vector3f; using Box3f = Eigen::AlignedBox3f; -private: - 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 - 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(); } - }; - public: PathPlanner(); ~PathPlanner() = default; @@ -103,19 +89,60 @@ 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, + float verification_range, + size_t verification_degree, 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, - std::vector& path); + const PathPlanMapT& map, + 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; pcl::search::KdTree kdtree; std::vector nodes; // all nodes in the search space @@ -124,10 +151,16 @@ class PathPlanner // threshold for considering goal reached float goal_threshold = 0.1f; // radius for neighbor search - float search_radius = 1.0f; - // weights for cost model: edge_cost = lambda_d * dist + lambda_p * penalty - float lambda_dist = 1.f; - float lambda_penalty = 1.f; + float search_radius = 0.5f; + // 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; + float verification_range = 1.5f; + size_t verification_degree = 2; // maximum number of neighbors to consider size_t max_neighbors = 10; }; 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..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 @@ -55,6 +57,7 @@ namespace perception template class UEOctree { +public: using FloatT = Float_T; using IndexT = std::make_unsigned::type; @@ -70,12 +73,21 @@ class UEOctree public: UEOctree(FloatT max_res); - ~UEOctree() = default; + ~UEOctree(); public: - void addExploredSpace(const Vec3f& min, const Vec3f& max); void clear(); - bool isExplored(const Vec3f& pt); + void reconfigure(FloatT max_res); + + 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 this->vox_res; } + inline size_t treeDepth() const { return this->root_height; } + inline size_t allocEstimate() const { return this->alloc_estimate; } protected: struct Node @@ -84,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; @@ -98,9 +110,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); @@ -110,12 +127,19 @@ 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( Node& node, const NodeDescriptor& descriptor, const Box3f& zone); + void recursiveCrop( + Node& node, + const NodeDescriptor& descriptor, + const Box3f& zone); protected: Vec3f origin{Vec3f::Zero()}; @@ -124,6 +148,8 @@ class UEOctree Node root; size_t root_height{0}; + + size_t alloc_estimate{0}; }; @@ -135,6 +161,27 @@ UEOctree::UEOctree(FloatT res) : vox_res{res} { } +template +UEOctree::~UEOctree() +{ + this->root.clear(this); +} + +template +void UEOctree::clear() +{ + this->root.clear(this); + 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) { @@ -155,15 +202,13 @@ void UEOctree::addExploredSpace(const Vec3f& min, const Vec3f& max) } template -void UEOctree::clear() +void UEOctree::crop(const Vec3f& min, const Vec3f& max) { - this->root.clear(); - this->root_height = 0; - this->vox_span = 0; + this->recursiveCrop(this->root, this->getRootDescriptor(), Box3f{min, max}); } template -bool UEOctree::isExplored(const Vec3f& pt) +bool UEOctree::isExplored(const Vec3f& pt) const { const Arr3f aligned_pt = ((pt - this->origin) / this->vox_res).array(); if ((aligned_pt >= Arr3f::Zero()).all() && @@ -176,7 +221,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();) @@ -196,33 +241,133 @@ bool UEOctree::isExplored(const Vec3f& pt) 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() +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; } } 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]; @@ -230,49 +375,57 @@ 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, 0x1 << this->root_height}; + 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 { // 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}; } template -UEOctree::Box3f UEOctree::getDescriptorBox( +typename UEOctree::Box3f UEOctree::getDescriptorBox( const NodeDescriptor& n) const { const Vec3f min_corner = @@ -282,6 +435,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) @@ -309,11 +501,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()) @@ -357,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++) { @@ -368,18 +563,18 @@ void UEOctree::recursiveExplore( if (zone.contains(child_span)) { child.explored = true; - child.clear(); + child.clear(this); } else if (zone.intersects(child_span)) { - if (child_desc[3] > 1) + if (!isLeaf(child_desc)) { this->recursiveExplore(child, child_desc, zone); } else { child.explored = true; - child.clear(); + child.clear(this); } } @@ -389,7 +584,58 @@ void UEOctree::recursiveExplore( if (all_explored) { node.explored = true; - node.clear(); + node.clear(this); + } +} + +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 (!isLeaf(descriptor)) + { + // not leaf --> expand children and recurse + const bool was_explored = node.explored; + node.init(this); + 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(this); + } + } + // leaf --> leave alone + } + else + { + // not contained and not intersected --> delete + node.explored = false; + node.clear(this); + } } } 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/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/perception_core.cpp b/src/core/perception_core.cpp index 8ba7178..e74c167 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; @@ -159,8 +159,14 @@ 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_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: @@ -338,9 +344,18 @@ 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("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" + << 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"; @@ -710,26 +725,59 @@ 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, + "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", 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, 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_verification_range, + config.pplan_verification_degree, config.pplan_max_neighbors); #endif @@ -745,7 +793,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 e49347d..9f7eb13 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}; @@ -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()); @@ -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/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 85eca1c..04a7396 100644 --- a/src/core/threads/path_planning_worker.cpp +++ b/src/core/threads/path_planning_worker.cpp @@ -55,12 +55,11 @@ #include #include +#include using namespace util::geom::cvt::ops; -using Vec3f = Eigen::Vector3f; - using PathMsg = nav_msgs::msg::Path; using PointCloudMsg = sensor_msgs::msg::PointCloud2; @@ -81,9 +80,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( @@ -96,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; @@ -138,25 +146,27 @@ 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->updateMap(buff); + + if (this->srv_enable_state.load() && this->threads_running.load()) { - this->pplan_target_notifier.waitNewestResource(); + this->planPath( + buff, + this->pplan_target_notifier.aquireNewestOutput()); } + + PROFILING_NOTIFY_ALWAYS(path_planning); + PROFILING_FLUSH_LOCAL(); } // while (this->threads_running.load()); @@ -166,28 +176,66 @@ void PathPlanningWorker::path_planning_thread_worker() } -void PathPlanningWorker::path_planning_callback(PathPlanningResources& buff) +void PathPlanningWorker::updateMap(const PathPlanningResources& buff) +{ + this->pplan_map.append(buff.points, buff.bounds_min, buff.bounds_max); + + 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); + 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( + const PathPlanningResources& buff, + PoseStampedMsg& target) { - if (buff.target.header.frame_id != this->odom_frame) + using namespace csm::perception::traversibility; + + 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,18 +243,29 @@ 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->need_clear_buffered) + { + this->path.clear(); + this->need_clear_buffered = false; + } - if (!this->path_planner.solvePath( - buff.base_to_odom.pose.vec, - odom_target, - buff.bounds_min, - buff.bounds_max, - buff.points, - path)) + std::vector prev_path = this->path; + for (auto max_weight = NOMINAL_MAX_WEIGHT; + isWeighted(max_weight) && !this->path_planner.solvePath( + this->path, + buff.base_to_odom.pose.vec, + odom_target, + this->pplan_map, + max_weight); + max_weight *= 2) + { + this->path = prev_path; + } + if (this->path.empty()) { + // Fail return; } @@ -214,8 +273,8 @@ void PathPlanningWorker::path_planning_callback(PathPlanningResources& buff) 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; @@ -223,7 +282,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..8182c59 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 @@ -78,12 +79,18 @@ class PathPlanningWorker using UpdatePathPlanSrv = cardinal_perception::srv::UpdatePathPlanningMode; + using Vec3f = Eigen::Vector3f; + public: PathPlanningWorker(RclNode& node, const Tf2Buffer& tf_buffer); ~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, @@ -96,7 +103,8 @@ class PathPlanningWorker protected: void path_planning_thread_worker(); - void path_planning_callback(PathPlanningResources& buff); + void updateMap(const PathPlanningResources& buff); + void planPath(const PathPlanningResources& buff, PoseStampedMsg& target); protected: RclNode& node; @@ -104,14 +112,20 @@ 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}; + 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 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; };