Class NodeSpatialTree

Class Documentation

class NodeSpatialTree

An object to find kNNs of the graph to determining the start and end nodes to utilize for planning in a free-space-style request of start and goal poses. Since the graph does not change over time, we can precompute this quadtree and reuse it over many requests.

Public Functions

NodeSpatialTree() = default

Constructor.

~NodeSpatialTree()

Destructor.

void computeTree(Graph &graph)

Compute the kd-tree based on the graph node information.

Parameters:

graph – The graph of nodes for the route

bool findNearestGraphNodesToPose(const geometry_msgs::msg::PoseStamped &pose_in, std::vector<unsigned int> &node_ids)

Find the closest node to a given pose.

Parameters:

  • pose_in – Pose to find node near

  • node_id – The return ID of the node

Returns:

if successfully found

void setNumOfNearestNodes(int num_of_nearest_nodes)

Set the number of nodes to search in local area for.

Parameters:

num – Numbers of nearest nodes to return

Protected Attributes

kd_tree_t *kdtree_
GraphAdaptor *adaptor_
Graph *graph_
int num_of_nearest_nodes_ = {3}