node_spatial_tree.hpp

// Copyright (c) 2025 Open Navigation LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// 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.
 
#ifndef NAV2_ROUTE__NODE_SPATIAL_TREE_HPP_
#define NAV2_ROUTE__NODE_SPATIAL_TREE_HPP_
 
#include <string>
#include <memory>
#include <vector>
#include <nanoflann.hpp>
 
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_route/types.hpp"
#include "nav2_route/utils.hpp"
 
namespace nav2_route
{
 
// Search in XY dimension for nearest neighbors
const size_t DIMENSION = 2;
 
struct GraphAdaptor
{
  explicit GraphAdaptor(const Graph & obj_)
  : obj(obj_) {}
 
  inline size_t kdtree_get_point_count() const {return obj.size();}
 
  inline double kdtree_get_pt(const size_t idx, const size_t dim) const
  {
    if (dim == 0) {
      return obj[idx].coords.x;
    }
    return obj[idx].coords.y;
  }
 
  template<class BBOX> bool kdtree_get_bbox(BBOX & /*bb*/) const {return false;}
 
  const Graph & obj;
};
 
typedef nanoflann::KDTreeSingleIndexAdaptor<
    nanoflann::L2_Simple_Adaptor<double, GraphAdaptor>, GraphAdaptor, DIMENSION> kd_tree_t;
 
class NodeSpatialTree
{
public:
  NodeSpatialTree() = default;
 
  ~NodeSpatialTree();
 
  void computeTree(Graph & graph);
 
  bool findNearestGraphNodesToPose(
    const geometry_msgs::msg::PoseStamped & pose_in,
    std::vector<unsigned int> & node_ids);
 
  void setNumOfNearestNodes(int num_of_nearest_nodes);
 
protected:
  kd_tree_t * kdtree_;
  GraphAdaptor * adaptor_;
  Graph * graph_;
  int num_of_nearest_nodes_{3};
};
 
}  // namespace nav2_route
 
#endif  // NAV2_ROUTE__NODE_SPATIAL_TREE_HPP_