Skip to content

Fix closest node handling in TopologicalNavServer for improved navigation accuracy #229

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
marc-hanheide merged 1 commit into from
Jan 12, 2026
Merged

Fix closest node handling in TopologicalNavServer for improved navigation accuracy #229

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 17 additions & 6 deletions topological_navigation/topological_navigation/scripts/navigation2.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -845,13 +845,17 @@ def navigate(self, target):

self.max_dist_to_closest_edge = self.get_parameter_or("max_dist_to_closest_edge", Parameter('double', Parameter.Type.DOUBLE, 1.0)).value

# if we are nowhere near an edge or not at a node, then do a node plan
if self.closest_edges.distances and (self.closest_edges.distances[0] > self.max_dist_to_closest_edge or self.current_node != "none"):
self.nav_from_closest_edge = False
o_node = self.rsearch.get_node_from_tmap2(self.closest_node)
self.get_logger().info("Planning from the closest NODE: {}".format(self.closest_node))
else:
self.nav_from_closest_edge = True
o_node, the_edge = self.orig_node_from_closest_edge(g_node)
# This creates essentially a fake "previous node" to address the edge case when navigating over a single edge and the closest node is on the edge but the current node isnt. (otherwise it will mark as complete without navigation).
if o_node == target:
o_node, the_edge = self.orig_node_from_closest_edge(g_node, flip=True)
self.get_logger().info("Planning from the closest EDGE: {}".format(the_edge["edge_id"]))

self.get_logger().info("Navigating From Origin {} to Target {} ".format(o_node["node"]["name"], target))
Expand Down Expand Up @@ -945,7 +949,7 @@ def publish_feedback_exec_policy(self, nav_outcome=None):
self._as_exec_policy_action_feedback_pub.publish(self._feedback_exec_policy)


def orig_node_from_closest_edge(self, g_node):
def orig_node_from_closest_edge(self, g_node, flip=False):

name_1, _ = get_node_names_from_edge_id_2(self.lnodes, self.closest_edges.edge_ids[0])
name_2, _ = get_node_names_from_edge_id_2(self.lnodes, self.closest_edges.edge_ids[1])
Expand All @@ -965,11 +969,18 @@ def orig_node_from_closest_edge(self, g_node):
d2 = get_route_distance(self.lnodes, o_node_2, g_node)
else: # Use the destination node of the closest edge.
d1 = 0; d2 = 1
if d1 <= d2:
return o_node_1, edge_1
if flip:
if d1 <= d2:
return o_node_1, edge_1
else:
return o_node_2, edge_2
else:
return o_node_2, edge_2

o_node_1b = self.rsearch.get_node_from_tmap2(name_1)
o_node_2b = self.rsearch.get_node_from_tmap2(name_2)
if d1 <= d2:
return o_node_1b, edge_1
else:
return o_node_2b, edge_2

def to_goal_node(self, g_node, the_edge=None):
self.get_logger().info("Target and Origin Nodes are the same")
Expand Down Expand Up @@ -1316,4 +1327,4 @@ def main():
rclpy.shutdown()

if __name__ == '__main__':
main()
main()