From 0f0f26d4b8c7e3be3616744a7e203bbcb40d7613 Mon Sep 17 00:00:00 2001 From: "James R. Heselden" Date: Wed, 15 Apr 2026 14:30:12 +0100 Subject: [PATCH] prevent gotonode system crash --- .../topological_navigation/edge_action_manager2.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/topological_navigation/topological_navigation/edge_action_manager2.py b/topological_navigation/topological_navigation/edge_action_manager2.py index 93af6631..3159cd9d 100644 --- a/topological_navigation/topological_navigation/edge_action_manager2.py +++ b/topological_navigation/topological_navigation/edge_action_manager2.py @@ -345,6 +345,13 @@ def initialise(self, bt_trees, edge, destination_node, origin_node=None, self.action_status = 0 if self.action_name == self.ACTIONS.NAVIGATE_TO_POSE: + self.destination_node_str = {} + name = destination_node['node']['name'] + x_coord = destination_node['node']['pose']['position']['x'] + y_coord = destination_node['node']['pose']['position']['y'] + coordinates = (x_coord, y_coord) + self.destination_node_str[coordinates] = name + self.get_logger().warn(f"Destination nodes: {self.destination_node_str}") self.destination_node = destination_node self.origin_node = origin_node self.get_logger().info(f"Processing edge {self.edge['edge_id']}") @@ -1321,10 +1328,10 @@ def execute(self): self.get_logger().warn("Executing action : {} ".format(target_action)) # Publish segment destination for visualisation purposes - current_destination = (target_goal.poses[-1].pose.position.x, target_goal.poses[-1].pose.position.y) + current_destination = (target_goal.pose.pose.position.x, target_goal.pose.pose.position.y) self.get_logger().warn("Current destination : {} ".format(self.destination_node_str[current_destination])) self.current_dest.publish(String(data=self.destination_node_str[current_destination])) - + # Handles both lists and dictionaries control_server_config = None if isinstance(self.control_server_configs, list):