From 27afff6bb679952fa555fbbb5ca0a7de6385542f Mon Sep 17 00:00:00 2001 From: hunter2 Date: Fri, 25 Jul 2025 15:41:45 +0100 Subject: [PATCH 01/23] update route pub --- .../scripts/navigation2.py | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/topological_navigation/topological_navigation/scripts/navigation2.py b/topological_navigation/topological_navigation/scripts/navigation2.py index ad562d72..6b51b0de 100755 --- a/topological_navigation/topological_navigation/scripts/navigation2.py +++ b/topological_navigation/topological_navigation/scripts/navigation2.py @@ -19,7 +19,7 @@ from topological_navigation.navigation_stats import nav_stats from topological_navigation.scripts.param_processing import ParameterUpdaterNode from topological_navigation.tmap_utils import * -from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy, QoSDurabilityPolicy +from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy from rclpy.action import ActionServer from rclpy import Parameter from topological_navigation.edge_action_manager2 import EdgeActionManager @@ -77,6 +77,9 @@ def __init__(self, name, update_params_control_server, edge_action_manager_serve self.fluid_navigation = True self.final_goal = False self.update_params_control_server = update_params_control_server + + self.route = None + self.target = None self.current_node = "Unknown" self.closest_node = "Unknown" @@ -170,9 +173,12 @@ def __init__(self, name, update_params_control_server, edge_action_manager_serve self.navigation_actions.append(self.navigation_action_name) self.latching_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + self.keep_history_qos = QoSProfile(reliability = ReliabilityPolicy.BEST_EFFORT, history=QoSHistoryPolicy.KEEP_LAST, depth=10, ) self.stat = None self.stats_pub = self.create_publisher(NavStatistics, "topological_navigation/Statistics", qos_profile=self.latching_qos) - self.route_pub = self.create_publisher(TopologicalRoute, "topological_navigation/Route", qos_profile=self.latching_qos) + self.route_pub = self.create_publisher(TopologicalRoute, "topological_navigation/Route", qos_profile= self.keep_history_qos) + self.route_pub_timer = self.create_timer( 2.0 , self.router_pub_timer_callback) + self.stroute = None self.cur_edge = self.create_publisher(String, "current_edge", qos_profile=self.latching_qos) self.move_act_pub = self.create_publisher(String, "topological_navigation/move_action_status", qos_profile=self.latching_qos) self._map_received = False @@ -834,6 +840,8 @@ def navigate(self, target): route = self.enforce_navigable_route(route, target) if route.source: self.get_logger().info("Navigating Case 1: Following route") + self.route = route + self.target = target self.publish_route(route, target) if(self.use_nav2_follow_route): result, inc, status = self.navigate_to_poses(route, target, 0) @@ -1027,8 +1035,13 @@ def publish_route(self, route, target): for i in route.source: stroute.nodes.append(i) stroute.nodes.append(target) + self.stroute = stroute self.route_pub.publish(stroute) + + def router_pub_timer_callback(self): + if (self.stroute is not None and self.stroute.nodes): + self.route_pub.publish(self.stroute) def publish_stats(self): pubst = NavStatistics() From 615a85f2baab5e788b40c2f9a94c199d8f253f06 Mon Sep 17 00:00:00 2001 From: Ibrahim Hroob <47870260+ibrahimhroob@users.noreply.github.com> Date: Wed, 10 Sep 2025 11:26:47 +0100 Subject: [PATCH 02/23] Update QoS profile for TopoMap2Vis subscription and ensure float-type tolerances in map_manager_2 (#206) * Update QoS profile for TopoMap2Vis subscription and ensure float-type tolerances in map_manager_2 * Add goal tolerance parameters to mapping --- .../topological_navigation/manager2.py | 2 +- .../scripts/visualise_map_ros2.py | 21 +++++++++++++++++-- 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/topological_navigation/topological_navigation/manager2.py b/topological_navigation/topological_navigation/manager2.py index 49795cea..d0b11a83 100644 --- a/topological_navigation/topological_navigation/manager2.py +++ b/topological_navigation/topological_navigation/manager2.py @@ -48,7 +48,7 @@ def construct_mapping(self, node, deep=False): # this can be extended to test the validity of the tmap2 # as well at load time (or add missing keys) - for key in ['x', 'y', 'z', 'w']: + for key in ['x', 'y', 'z', 'w', 'yaw_goal_tolerance', 'xy_goal_tolerance']: if key in mapping and isinstance(mapping[key], int): mapping[key] = float(mapping[key]) diff --git a/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py b/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py index a45210bf..2132b2ca 100755 --- a/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py +++ b/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py @@ -24,6 +24,7 @@ from rclpy.task import Future import threading import yaml +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy # this ensures that all the poses and translates # are float-type and not int-type as there is an @@ -131,8 +132,24 @@ def __init__(self, name, interactive_marker_server) : self.get_logger().info("End generating map...") break - self.topo_route_sub = self.create_subscription(TopologicalRoute, "topological_navigation/Route" - , self.route_cb, qos_profile=self.latching_qos, callback_group=self.callback_goto_subs) + # 1. Define a QoS profile that matches the publisher's settings + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=QoSDurabilityPolicy.VOLATILE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10 # Match the publisher's depth + ) + + # 2. Create the subscriber with the new QoS profile + self.topo_route_sub = self.create_subscription( + TopologicalRoute, + 'topological_navigation/Route', + self.route_cb, + qos_profile # Pass the matching QoS profile here + ) + + # self.topo_route_sub = self.create_subscription(TopologicalRoute, "topological_navigation/Route" + # , self.route_cb, qos_profile=self.latching_qos, callback_group=self.callback_goto_subs) self.get_logger().info("All Done ...") def topo_map_cb(self, msg): From 57ea88e20900758d1ae8a977882869a799b1e690 Mon Sep 17 00:00:00 2001 From: Ibrahim Hroob <47870260+ibrahimhroob@users.noreply.github.com> Date: Fri, 21 Nov 2025 20:07:17 +0000 Subject: [PATCH 03/23] headder added to executepolicymodegoal (#210) Co-authored-by: Luca Castri Co-authored-by: James R Heselden Co-authored-by: James R. Heselden <34611419+Iranaphor@users.noreply.github.com> --- topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg b/topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg index 19b5c7a1..26b442c9 100644 --- a/topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg +++ b/topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg @@ -1,2 +1,3 @@ #goal definition +std_msgs/Header header topological_navigation_msgs/NavRoute route From ecaf2c91edc85ca09209d6680799385c527d20d3 Mon Sep 17 00:00:00 2001 From: ibrahim Date: Thu, 27 Nov 2025 10:13:41 +0000 Subject: [PATCH 04/23] Add navigation area subscription and tunnel detection to EdgeActionManager --- .../edge_action_manager2.py | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/topological_navigation/topological_navigation/edge_action_manager2.py b/topological_navigation/topological_navigation/edge_action_manager2.py index 4c5f2b38..fc4b6285 100644 --- a/topological_navigation/topological_navigation/edge_action_manager2.py +++ b/topological_navigation/topological_navigation/edge_action_manager2.py @@ -155,10 +155,12 @@ def init(self, ACTIONS, route_search, update_params_control_server, inrow_step_s self.update_params_control_server = update_params_control_server self.current_robot_pose = None + self.is_inside_tunnel = False self.odom_sub = self.create_subscription(Odometry, '/odometry/global', self.odom_callback, QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT)) self.get_current_node_sub = self.create_subscription(String, 'closest_node', self.set_current_pose , QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT)) + self.robot_nav_area_sub = self.create_subscription(String, '/robot_navigation_area', self.nav_area_callback, 10) self.boundary_publisher = self.create_publisher(Path, '/boundary_checker', qos_profile=self.latching_qos) self.robot_current_status_pub = self.create_publisher(String, '/robot_operation_current_status', qos_profile=self.latching_qos) @@ -177,7 +179,12 @@ def set_current_pose(self, msg): def odom_callback(self, msg): self.current_robot_pose = msg.pose - + + + def nav_area_callback(self, msg): + self.nav_area = msg.data + self.is_inside_tunnel = (self.nav_area == 'INSIDE_POLYTUNNEL') + def get_nav_action_server_status(self, ): return self.ACTIONS.status_mapping @@ -346,17 +353,19 @@ def get_goal_align_if(self, edge_id, current_action, next_edge_id=None): if len(next_edge_ids) == 2: next_goal_stage = next_edge_ids[1].split("-") if len(next_goal_stage) == 2: - if (next_goal_stage[1] in self.ACTIONS.GOAL_ALIGN_INDEX) or (next_goal_stage[1] not in self.ACTIONS.GOAL_ALIGN_GOAL): + if (next_goal_stage[1] in self.ACTIONS.GOAL_ALIGN_INDEX) or \ + (next_goal_stage[1] not in self.ACTIONS.GOAL_ALIGN_GOAL): return current_action elif len(next_goal_stage) == 1: - if(current_action == self.ACTIONS.ROW_TRAVERSAL): + if current_action == self.ACTIONS.ROW_TRAVERSAL: return current_action if len(edges) == 2: goal = edges[1] goal_stage = goal.split("-") if len(goal_stage) == 2: - if goal_stage[1] in self.ACTIONS.GOAL_ALIGN_INDEX: - return self.ACTIONS.GOAL_ALIGN + if goal_stage[1] in self.ACTIONS.GOAL_ALIGN_INDEX and not self.is_inside_tunnel: + return self.ACTIONS.GOAL_ALIGN + return current_action From ffa48acb976fcdf0d285d931448306da0be9346d Mon Sep 17 00:00:00 2001 From: ibrahim Date: Thu, 27 Nov 2025 11:42:08 +0000 Subject: [PATCH 05/23] Add target edge path publishing to EdgeActionManager - Introduced a new publisher for target edge paths. - Implemented method to convert selected edges dictionary to a ROS Path and publish it. - Enhanced logging for missing node poses and invalid target poses. --- .../edge_action_manager2.py | 62 ++++++++++++++++++- 1 file changed, 61 insertions(+), 1 deletion(-) diff --git a/topological_navigation/topological_navigation/edge_action_manager2.py b/topological_navigation/topological_navigation/edge_action_manager2.py index fc4b6285..bf6d9c49 100644 --- a/topological_navigation/topological_navigation/edge_action_manager2.py +++ b/topological_navigation/topological_navigation/edge_action_manager2.py @@ -165,7 +165,8 @@ def init(self, ACTIONS, route_search, update_params_control_server, inrow_step_s self.boundary_publisher = self.create_publisher(Path, '/boundary_checker', qos_profile=self.latching_qos) self.robot_current_status_pub = self.create_publisher(String, '/robot_operation_current_status', qos_profile=self.latching_qos) self.current_dest = self.create_publisher(String, '/topological_navigation/current_destination', qos_profile=self.latching_qos) - + self.target_edge_path_pub = self.create_publisher(Path, "/target_edge_path", qos_profile=self.latching_qos) + self.robot_current_behavior_pub = None self.current_node = None self.action_status = 0 @@ -515,6 +516,63 @@ def two_smallest_indices(self, lst): def extract_number(self, s): return float(s.split('-')[0][1:]) + def publish_target_edges_as_path(self, selected_edges_dict): + """ + Convert selected edges dictionary to a ROS Path and publish. + Safely handles placeholder values like '$node.pose' by substituting + with actual node pose from selected_edges_dict['node']['pose']. + """ + path_msg = Path() + node_info = selected_edges_dict.get("node", {}) + node_pose = node_info.get("pose", {}) + parent_frame = node_info.get("parent_frame", "map") # fallback to 'map' + + path_msg.header.frame_id = parent_frame + path_msg.header.stamp = self.get_clock().now().to_msg() + + if not node_pose: + self.get_logger().warn("Node pose missing, cannot publish path") + return + + valid_poses_count = 0 + + for edge in node_info.get("edges", []): + target_pose_data = edge.get("goal", {}).get("target_pose", {}) + + # Check if pose is placeholder + if target_pose_data.get("pose") == "$node.pose": + pose_source = node_pose + elif isinstance(target_pose_data.get("pose"), dict): + pose_source = target_pose_data["pose"] + else: + self.get_logger().warn( + f"Skipping edge {edge.get('edge_id')} due to invalid target_pose" + ) + continue + + # Create PoseStamped + try: + pose_msg = PoseStamped() + pose_msg.header.frame_id = parent_frame + pose_msg.header.stamp = self.get_clock().now().to_msg() + pose_msg.pose.position.x = pose_source["position"]["x"] + pose_msg.pose.position.y = pose_source["position"]["y"] + pose_msg.pose.position.z = pose_source["position"]["z"] + pose_msg.pose.orientation.x = pose_source["orientation"]["x"] + pose_msg.pose.orientation.y = pose_source["orientation"]["y"] + pose_msg.pose.orientation.z = pose_source["orientation"]["z"] + pose_msg.pose.orientation.w = pose_source["orientation"]["w"] + except KeyError as e: + self.get_logger().warn( + f"Skipping edge {edge.get('edge_id')} due to missing key: {e}" + ) + continue + + path_msg.poses.append(pose_msg) + valid_poses_count += 1 + + self.get_logger().info(f"Publishing Path with {valid_poses_count} poses") + self.target_edge_path_pub.publish(path_msg) def get_navigate_through_poses_goal(self, poses, actions, edge_ids, is_execpolicy=False): @@ -527,6 +585,8 @@ def handle_row_operation(): self.get_logger().info("Action in_row_operation ") self.get_logger().info("Edge id and tag id {} {}".format(self.target_row_edge_id, tag_id)) cen = self.route_search.get_node_from_tmap2(self.target_row_edge_id) + self.publish_target_edges_as_path(cen) + children = self.route_search.get_connected_nodes_tmap2(cen) selected_row_edge_nodes = {} for next_edge in children: From 37a01ef2475690b3821c33798eda344ee8801266 Mon Sep 17 00:00:00 2001 From: Your Name Date: Thu, 27 Nov 2025 17:37:40 +0000 Subject: [PATCH 06/23] Add intermediate orientation override for smoother navigation flow --- .../scripts/navigation2.py | 49 ++++++++++++++++++- 1 file changed, 48 insertions(+), 1 deletion(-) diff --git a/topological_navigation/topological_navigation/scripts/navigation2.py b/topological_navigation/topological_navigation/scripts/navigation2.py index c28587f4..982c93e6 100755 --- a/topological_navigation/topological_navigation/scripts/navigation2.py +++ b/topological_navigation/topological_navigation/scripts/navigation2.py @@ -7,7 +7,7 @@ import rclpy, json, yaml - +import math from topological_navigation_msgs.msg import NavStatistics, CurrentEdge, ClosestEdges, TopologicalRoute, GotoNodeFeedback, ExecutePolicyModeFeedback from topological_navigation_msgs.srv import EvaluateEdge, EvaluateNode from topological_navigation_msgs.action import GotoNode, ExecutePolicyMode @@ -116,6 +116,9 @@ def __init__(self, name, update_params_control_server, edge_action_manager_serve self.declare_parameter(self.ACTIONS.BT_GOAL_ALIGN, Parameter.Type.STRING) self.declare_parameter(self.ACTIONS.BT_IN_ROW_OPERATION, Parameter.Type.STRING) self.declare_parameter(self.ACTIONS.BT_IN_ROW_RECOVERY, Parameter.Type.STRING) + + self.declare_parameter("allow_intermediate_orientation_override", Parameter.Type.BOOL) + self.allow_intermediate_orientation_override = self.get_parameter_or("allow_intermediate_orientation_override", Parameter('bool', Parameter.Type.BOOL, False)).value self.navigation_action_name = self.get_parameter_or("navigation_action_name", Parameter('str', Parameter.Type.STRING, self.ACTIONS.NAVIGATE_TO_POSE)).value self.navigation_actions = self.get_parameter_or("navigation_actions", Parameter('str', Parameter.Type.STRING_ARRAY, self.ACTIONS.navigation_actions)).value @@ -752,6 +755,50 @@ def navigate_to_poses(self, route, target, exec_policy): rindex = rindex + 1 self.get_logger().info(" ========== Action list {} ".format(route_actions_list)) + + if self.allow_intermediate_orientation_override: + # ====================================================================== + # ## NEW LOGIC START: Realign Orientations for Continuous Flow + # ====================================================================== + # We iterate through all destinations except the very last one. + # We point each node to look at the NEXT node. + + for i in range(len(route_dests) - 1): + curr_node = route_dests[i] + next_node = route_dests[i+1] + + # 1. Get positions + # Note: Adjust keys ["pose"]["position"] if your dictionary structure is different + cx = curr_node["node"]["pose"]["position"]["x"] + cy = curr_node["node"]["pose"]["position"]["y"] + nx = next_node["node"]["pose"]["position"]["x"] + ny = next_node["node"]["pose"]["position"]["y"] + + # 2. Calculate the specific angle (Yaw) to the next node + dx = nx - cx + dy = ny - cy + yaw = math.atan2(dy, dx) + + # 3. Convert Yaw to Quaternion manually (to avoid extra dependencies) + # Formula for Z-axis rotation + qz = math.sin(yaw * 0.5) + qw = math.cos(yaw * 0.5) + + # 4. Overwrite the orientation of the intermediate node + # Now the planner thinks this node is "facing" the path, so it won't stop to turn. + curr_node["node"]["pose"]["orientation"]["x"] = 0.0 + curr_node["node"]["pose"]["orientation"]["y"] = 0.0 + curr_node["node"]["pose"]["orientation"]["z"] = qz + curr_node["node"]["pose"]["orientation"]["w"] = qw + + self.get_logger().info(f"Realigned node {i} to yaw: {yaw:.2f}") + + # The LAST node (route_dests[-1]) is left untouched so it keeps + # the final desired docking/goal orientation. + # ====================================================================== + # ## NEW LOGIC END + # ====================================================================== + nav_ok, inc, status = self.execute_actions(route_edges, route_dests, route_origins, action_name=self.ACTIONS.NAVIGATE_THROUGH_POSES, is_execpolicy=exec_policy) From a3d6d18ec1559082d917c469daa7f3d9fe14cfd6 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Fri, 28 Nov 2025 18:35:04 +0000 Subject: [PATCH 07/23] Initial plan From 73ec810006b92a220d3588b51c507ce592fe4b7e Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Fri, 28 Nov 2025 18:39:41 +0000 Subject: [PATCH 08/23] Add flexible properties system for nodes and edges Co-authored-by: marc-hanheide <1153084+marc-hanheide@users.noreply.github.com> --- .../config/template_edge.yaml | 1 + .../config/tmap-schema.yaml | 8 + topological_navigation/doc/PROPERTIES.md | 157 ++++++++++++++++++ 3 files changed, 166 insertions(+) create mode 100644 topological_navigation/doc/PROPERTIES.md diff --git a/topological_navigation/config/template_edge.yaml b/topological_navigation/config/template_edge.yaml index 12bd5a47..04fb1b76 100644 --- a/topological_navigation/config/template_edge.yaml +++ b/topological_navigation/config/template_edge.yaml @@ -10,6 +10,7 @@ goal: frame_id: $node.parent_frame pose: $node.pose node: destination +properties: {} recovery_behaviours_config: '' restrictions_planning: 'True' restrictions_runtime: 'True' \ No newline at end of file diff --git a/topological_navigation/config/tmap-schema.yaml b/topological_navigation/config/tmap-schema.yaml index f372675d..412a262e 100644 --- a/topological_navigation/config/tmap-schema.yaml +++ b/topological_navigation/config/tmap-schema.yaml @@ -64,9 +64,17 @@ properties: type: string node: type: string + properties: + type: object + additionalProperties: true + description: Flexible dictionary of application-specific edge properties (e.g., max_speed, priority, surface_type) name: type: string parent_frame: type: string + properties: + type: object + additionalProperties: true + description: Flexible dictionary of application-specific node properties (e.g., xy_goal_tolerance, yaw_goal_tolerance, semantics, zone) pointset: type: string diff --git a/topological_navigation/doc/PROPERTIES.md b/topological_navigation/doc/PROPERTIES.md new file mode 100644 index 00000000..c10f979f --- /dev/null +++ b/topological_navigation/doc/PROPERTIES.md @@ -0,0 +1,157 @@ +# Flexible Node and Edge Properties System + +This document describes the flexible properties system for topological maps, which allows application-specific metadata to be attached to both nodes and edges without requiring schema modifications. + +## Overview + +The topological map schema supports optional `properties` dictionaries for both nodes and edges. These properties enable domain-specific customisation while maintaining backwards compatibility with existing maps. + +## Node Properties + +Node properties are defined in `nodes[].node.properties` as a YAML dictionary. The schema allows any key-value pairs. + +### Default Properties + +The following properties are commonly used for navigation control: + +| Property | Type | Description | +|----------|------|-------------| +| `xy_goal_tolerance` | float | XY position tolerance for goal reaching (metres) | +| `yaw_goal_tolerance` | float | Yaw orientation tolerance for goal reaching (radians) | + +### Example Custom Properties + +| Property | Type | Description | +|----------|------|-------------| +| `row` | integer | Row identifier (e.g., for agricultural polytunnel scenarios) | +| `semantics` | string | Semantic meaning of the node (e.g., "charging_station", "inspection_point") | +| `zone` | string | Operational zone designation | +| `access_level` | string | Permission level required for access | +| `capacity` | integer | Maximum number of robots that can occupy the node | + +### Node Properties Example + +```yaml +nodes: +- meta: + map: riseholme + node: ChargingStation1 + pointset: riseholme + node: + name: ChargingStation1 + parent_frame: map + pose: + position: {x: 10.0, y: 5.0, z: 0.0} + orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0} + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + semantics: "charging_station" + row: 3 + zone: "A" + capacity: 2 + edges: [] +``` + +## Edge Properties + +Edge properties are defined in `nodes[].node.edges[].properties` as a YAML dictionary. The schema allows any key-value pairs. + +### Example Edge Properties + +| Property | Type | Description | +|----------|------|-------------| +| `max_speed` | float | Maximum traversal speed (m/s) | +| `priority` | integer | Preference weight for path planning (higher = more preferred) | +| `width` | float | Physical width of the traversable path (metres) | +| `surface_type` | string | Terrain classification (e.g., "concrete", "grass", "gravel") | +| `bidirectional` | boolean | Whether the edge can be traversed in both directions | +| `weather_restrictions` | list | Conditions under which edge should not be used | + +### Edge Properties Example + +```yaml +edges: +- edge_id: ChargingStation1_WayPoint2 + node: WayPoint2 + action: NavigateToPose + action_type: geometry_msgs/PoseStamped + properties: + max_speed: 0.5 + priority: 10 + surface_type: "concrete" + bidirectional: true + weather_restrictions: ["heavy_rain", "snow"] + # ... other edge fields +``` + +## Backwards Compatibility + +The `properties` field is optional for both nodes and edges. Existing topological maps without properties remain valid and will continue to work without modification. + +When properties are not specified: +- Node properties default to standard navigation tolerances if needed by the navigation system +- Edge properties are simply absent (empty dictionary) + +## Usage Guidelines + +### Naming Conventions + +- Use `snake_case` for property names +- Use descriptive, domain-appropriate names +- Avoid abbreviations unless they are widely understood + +### Type Flexibility + +Properties support various data types: +- **Strings**: `"charging_station"`, `"concrete"` +- **Numbers**: `0.5`, `10`, `3.14` +- **Booleans**: `true`, `false` +- **Lists**: `["heavy_rain", "snow"]` +- **Nested Objects**: `{min: 0.1, max: 1.0}` + +### Application-Specific Properties + +Applications can define their own property schemas and document them appropriately. The topological navigation system will safely ignore properties it does not recognise, allowing different applications to coexist on the same map. + +### Accessing Properties in Code + +When accessing properties programmatically, always check for property existence before use: + +```python +# Safe access to node properties +node_props = node["node"].get("properties", {}) +xy_tolerance = node_props.get("xy_goal_tolerance", 0.3) # Default to 0.3 +semantics = node_props.get("semantics") # Returns None if not present + +# Safe access to edge properties +edge_props = edge.get("properties", {}) +max_speed = edge_props.get("max_speed") # Returns None if not present +if max_speed is not None: + # Use max_speed for navigation control + pass +``` + +## Schema Reference + +The properties fields are defined in `config/tmap-schema.yaml`: + +```yaml +# Node properties (at nodes[].node.properties) +properties: + type: object + additionalProperties: true + description: Flexible dictionary of application-specific node properties + +# Edge properties (at nodes[].node.edges[].properties) +properties: + type: object + additionalProperties: true + description: Flexible dictionary of application-specific edge properties +``` + +## Related Resources + +- [Topological Map Schema](../config/tmap-schema.yaml) +- [Node Template](../config/template_node_2.yaml) +- [Edge Template](../config/template_edge.yaml) From b86eea40b1d8627bd72ea0fa9197e811e2969962 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Fri, 28 Nov 2025 18:59:27 +0000 Subject: [PATCH 09/23] Add namespace guidance and map validation script Co-authored-by: marc-hanheide <1153084+marc-hanheide@users.noreply.github.com> --- topological_navigation/doc/PROPERTIES.md | 115 +++++++++++ topological_navigation/setup.py | 1 + .../scripts/validate_map.py | 192 ++++++++++++++++++ 3 files changed, 308 insertions(+) create mode 100644 topological_navigation/topological_navigation/scripts/validate_map.py diff --git a/topological_navigation/doc/PROPERTIES.md b/topological_navigation/doc/PROPERTIES.md index c10f979f..e3925c79 100644 --- a/topological_navigation/doc/PROPERTIES.md +++ b/topological_navigation/doc/PROPERTIES.md @@ -101,6 +101,100 @@ When properties are not specified: - Use descriptive, domain-appropriate names - Avoid abbreviations unless they are widely understood +### Using Namespaces for Properties + +To organise properties by functional area or application, use **namespaces**. A namespace is simply a nested dictionary that groups related properties together. This approach: + +- Prevents naming conflicts between different systems +- Makes it clear which application or module owns each property +- Improves readability and maintainability + +#### Namespace Structure + +```yaml +properties: + namespace_name: + property1: value1 + property2: value2 +``` + +#### Common Namespace Examples + +| Namespace | Purpose | +|-----------|---------| +| `navigation` | Core navigation parameters (tolerances, speeds) | +| `fleet_management` | Multi-robot coordination properties | +| `logistics` | Warehouse/delivery application properties | +| `agriculture` | Agricultural robotics properties | +| `safety` | Safety-related constraints and limits | +| `semantics` | Semantic labels and classifications | + +#### Namespaced Node Properties Example + +```yaml +node: + name: ChargingStation1 + properties: + navigation: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + fleet_management: + capacity: 2 + priority_zone: true + queue_enabled: true + semantics: + type: "charging_station" + zone: "A" + logistics: + pickup_point: false + dropoff_point: false +``` + +#### Namespaced Edge Properties Example + +```yaml +edges: +- edge_id: ChargingStation1_WayPoint2 + node: WayPoint2 + properties: + navigation: + max_speed: 0.5 + bidirectional: true + safety: + width: 1.2 + restricted_hours: ["22:00-06:00"] + logistics: + priority: 10 + surface_type: "concrete" +``` + +#### Accessing Namespaced Properties in Code + +```python +# Safe access to namespaced node properties +node_props = node["node"].get("properties", {}) + +# Access fleet_management namespace +fleet_props = node_props.get("fleet_management", {}) +capacity = fleet_props.get("capacity", 1) + +# Access navigation namespace +nav_props = node_props.get("navigation", {}) +xy_tolerance = nav_props.get("xy_goal_tolerance", 0.3) + +# Access namespaced edge properties +edge_props = edge.get("properties", {}) +safety_props = edge_props.get("safety", {}) +max_width = safety_props.get("width") +``` + +#### Guidelines for Namespace Usage + +1. **Use namespaces for application-specific properties**: If your properties are only used by a specific application or module, group them under a namespace. +2. **Keep core navigation properties at root level**: For backwards compatibility, standard navigation properties like `xy_goal_tolerance` can remain at the root level. +3. **Document your namespaces**: If you introduce a new namespace, document its purpose and expected properties. +4. **Avoid deep nesting**: One level of namespace nesting is usually sufficient. Avoid creating deeply nested structures. + ### Type Flexibility Properties support various data types: @@ -150,6 +244,27 @@ properties: description: Flexible dictionary of application-specific edge properties ``` +## Validating Topological Maps + +The `validate_map.py` script can be used to validate topological map YAML files against the schema: + +```bash +# Basic validation +ros2 run topological_navigation validate_map.py my_map.tmap2.yaml + +# With verbose output +ros2 run topological_navigation validate_map.py my_map.tmap2.yaml -v + +# With custom schema file +ros2 run topological_navigation validate_map.py my_map.tmap2.yaml --schema custom_schema.yaml +``` + +The validator will: +- Check the map structure against the JSON schema +- Report any missing required fields +- Warn about duplicate node names +- Warn about edges pointing to non-existent nodes + ## Related Resources - [Topological Map Schema](../config/tmap-schema.yaml) diff --git a/topological_navigation/setup.py b/topological_navigation/setup.py index 1b61be9c..0cb18c5a 100644 --- a/topological_navigation/setup.py +++ b/topological_navigation/setup.py @@ -52,6 +52,7 @@ 'manual_topomapping.py = topological_navigation.scripts.manual_topomapping:main', 'occupancy_checker.py = topological_navigation.scripts.occupancy_checker:main', 'topological_visual.py = topological_navigation.scripts.topological_visual:main', + 'validate_map.py = topological_navigation.scripts.validate_map:main', ], }, diff --git a/topological_navigation/topological_navigation/scripts/validate_map.py b/topological_navigation/topological_navigation/scripts/validate_map.py new file mode 100644 index 00000000..9a94b213 --- /dev/null +++ b/topological_navigation/topological_navigation/scripts/validate_map.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python3 +""" +Topological Map Schema Validator + +This script validates topological map YAML files against the tmap-schema.yaml schema. +It can be used standalone (without ROS) to verify map files are correctly formatted. + +Usage: + python3 validate_map.py [schema_file.yaml] + +If schema_file is not provided, the script will look for tmap-schema.yaml in the +standard config directory. + +Exit codes: + 0 - Validation successful + 1 - Validation failed + 2 - File not found or other error +""" + +import sys +import os +import argparse + +try: + import yaml +except ImportError: + print("Error: PyYAML is required. Install with: pip install pyyaml") + sys.exit(2) + +try: + import jsonschema +except ImportError: + print("Error: jsonschema is required. Install with: pip install jsonschema") + sys.exit(2) + + +def find_schema_file(): + """Find the schema file in standard locations.""" + # Try relative to this script + script_dir = os.path.dirname(os.path.abspath(__file__)) + + # Possible locations for the schema file + possible_paths = [ + os.path.join(script_dir, '..', '..', 'config', 'tmap-schema.yaml'), + os.path.join(script_dir, '..', 'config', 'tmap-schema.yaml'), + os.path.join(script_dir, 'config', 'tmap-schema.yaml'), + ] + + # Try ROS2 package share directory if ament_index is available + try: + from ament_index_python.packages import get_package_share_directory + package_path = get_package_share_directory('topological_navigation') + possible_paths.insert(0, os.path.join(package_path, 'config', 'tmap-schema.yaml')) + except (ImportError, Exception): + pass + + for path in possible_paths: + normalized_path = os.path.normpath(path) + if os.path.isfile(normalized_path): + return normalized_path + + return None + + +def load_yaml_file(filepath): + """Load a YAML file and return its contents.""" + try: + with open(filepath, 'r', encoding='utf-8') as f: + return yaml.safe_load(f) + except yaml.YAMLError as e: + raise ValueError(f"Invalid YAML syntax: {e}") + except FileNotFoundError: + raise FileNotFoundError(f"File not found: {filepath}") + except Exception as e: + raise RuntimeError(f"Error reading file: {e}") + + +def validate_map(map_file, schema_file=None, verbose=False): + """ + Validate a topological map file against the schema. + + Args: + map_file: Path to the topological map YAML file + schema_file: Path to the schema YAML file (optional) + verbose: Print detailed validation information + + Returns: + Tuple of (is_valid: bool, message: str) + """ + # Find schema file if not provided + if schema_file is None: + schema_file = find_schema_file() + if schema_file is None: + return False, "Could not find tmap-schema.yaml. Please specify schema file path." + + if verbose: + print(f"Using schema: {schema_file}") + print(f"Validating map: {map_file}") + + # Load schema + try: + schema = load_yaml_file(schema_file) + except Exception as e: + return False, f"Error loading schema: {e}" + + # Load map + try: + tmap = load_yaml_file(map_file) + except Exception as e: + return False, f"Error loading map: {e}" + + # Validate + try: + jsonschema.validate(tmap, schema) + + # Additional validation checks + warnings = [] + + # Check for node count + node_count = len(tmap.get('nodes', [])) + if verbose: + print(f"Found {node_count} nodes") + + # Check for duplicate node names + node_names = set() + node_names_list = [] # Keep a list for edge validation + for node_entry in tmap.get('nodes', []): + node = node_entry.get('node', {}) + name = node.get('name') + if name: + if name in node_names: + warnings.append(f"Duplicate node name: {name}") + node_names.add(name) + node_names_list.append(name) + + # Check for edges pointing to non-existent nodes + for node_entry in tmap.get('nodes', []): + node = node_entry.get('node', {}) + for edge in node.get('edges', []): + target = edge.get('node') + if target and target not in node_names: + warnings.append(f"Edge '{edge.get('edge_id')}' points to non-existent node: {target}") + + message = "Validation successful" + if warnings: + message += f"\n\nWarnings ({len(warnings)}):\n" + "\n".join(f" - {w}" for w in warnings) + + return True, message + + except jsonschema.ValidationError as e: + # Format a user-friendly error message + path = " -> ".join(str(p) for p in e.absolute_path) if e.absolute_path else "root" + return False, f"Validation failed at '{path}':\n {e.message}" + except jsonschema.SchemaError as e: + return False, f"Schema error: {e.message}" + + +def main(): + parser = argparse.ArgumentParser( + description='Validate topological map YAML files against the schema.', + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog=""" +Examples: + %(prog)s my_map.tmap2.yaml + %(prog)s my_map.tmap2.yaml --schema custom_schema.yaml + %(prog)s my_map.tmap2.yaml -v + """ + ) + parser.add_argument('map_file', help='Path to the topological map YAML file') + parser.add_argument('--schema', '-s', help='Path to the schema YAML file (optional)') + parser.add_argument('--verbose', '-v', action='store_true', help='Print detailed information') + + args = parser.parse_args() + + # Check if map file exists + if not os.path.isfile(args.map_file): + print(f"Error: Map file not found: {args.map_file}") + sys.exit(2) + + # Validate + is_valid, message = validate_map(args.map_file, args.schema, args.verbose) + + if is_valid: + print(f"✓ {message}") + sys.exit(0) + else: + print(f"✗ {message}") + sys.exit(1) + + +if __name__ == '__main__': + main() From a079c5621cb3861bc663aff1dc02421da5e54aba Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Fri, 28 Nov 2025 19:10:59 +0000 Subject: [PATCH 10/23] Add convention for using package names as namespaces Added a guideline for using package names as namespaces in properties. --- topological_navigation/doc/PROPERTIES.md | 1 + 1 file changed, 1 insertion(+) diff --git a/topological_navigation/doc/PROPERTIES.md b/topological_navigation/doc/PROPERTIES.md index e3925c79..77b5975b 100644 --- a/topological_navigation/doc/PROPERTIES.md +++ b/topological_navigation/doc/PROPERTIES.md @@ -194,6 +194,7 @@ max_width = safety_props.get("width") 2. **Keep core navigation properties at root level**: For backwards compatibility, standard navigation properties like `xy_goal_tolerance` can remain at the root level. 3. **Document your namespaces**: If you introduce a new namespace, document its purpose and expected properties. 4. **Avoid deep nesting**: One level of namespace nesting is usually sufficient. Avoid creating deeply nested structures. +5. By convention, ROS packages should use their package name as the namespace. E.g. a package called "topfleets_coordinator" should use this as the namespace for properties it uses. ### Type Flexibility From 21ef38744b44113643b84ea6fadaefe79ce782c2 Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Tue, 16 Dec 2025 12:18:38 +0000 Subject: [PATCH 11/23] Update README files to enhance documentation and add flexible properties system details --- .gitignore | 1 + README.md | 13 ++++++++++++- topological_navigation/README.md | 6 +++++- 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 89525881..70e59f66 100644 --- a/.gitignore +++ b/.gitignore @@ -67,3 +67,4 @@ coverage.xml # Nano auto-backups **/*.*~ **/*.*.swp +.DS_Store diff --git a/README.md b/README.md index 8afd62fe..8333bb1b 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,14 @@ # topological_navigation -A topological navigation planning framework +A topological navigation planning framework for ROS 2. + +## Packages + +This repository contains the following ROS 2 packages: + +- **[topological_navigation](topological_navigation/README.md)** - Core topological navigation and mapping functionality +- **[topological_navigation_msgs](topological_navigation_msgs/)** - Message, service, and action definitions for topological navigation +- **[topological_utils](topological_utils/)** - Utility tools for creating and managing topological maps +- **[topological_rviz_tools](topological_rviz_tools/README.md)** - RViz-based tools for interactive topological map construction and editing + +Please refer to the individual package README files for detailed documentation and usage instructions. diff --git a/topological_navigation/README.md b/topological_navigation/README.md index b9e97d0f..240f9eed 100644 --- a/topological_navigation/README.md +++ b/topological_navigation/README.md @@ -3,7 +3,11 @@ Topological Navigation **These instructions are intended for the legacy branch of topological navigation. Instructions for the master branch (topological navigation 2) are to do.** -This node provides support for topological navigation in the STRANDS system. +This package provides support for topological navigation, originally devised for the STRANDS system. + +## Flexible Properties System + +The topological map schema now supports a flexible properties system that allows you to attach custom metadata to nodes and edges without requiring schema modifications. This enables domain-specific customization such as setting navigation tolerances, semantic labels, operational zones, and application-specific attributes. See [doc/PROPERTIES.md](doc/PROPERTIES.md) for detailed documentation. This module requires: * move_base From 22f557217e2a4652175286bb8748cd25bc529963 Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Tue, 16 Dec 2025 12:19:57 +0000 Subject: [PATCH 12/23] Update CI workflow to include 'aoc' branch for push and pull request triggers --- .github/workflows/ci.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index e2504dc6..4380dadc 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -2,9 +2,9 @@ name: ros CI on: push: - branches: [ humble-dev ] + branches: [ humble-dev, aoc ] pull_request: - branches: [ humble-dev ] + branches: [ humble-dev, aoc ] jobs: test_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions. From ea27f9953cb0a9fa2c633a94d896f00947a71d3c Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Tue, 16 Dec 2025 12:20:27 +0000 Subject: [PATCH 13/23] Add tag trigger to CI workflow for push events --- .github/workflows/ci.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 4380dadc..078a646f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -3,6 +3,8 @@ name: ros CI on: push: branches: [ humble-dev, aoc ] + tags: # Also run CI on tags. + - '*' pull_request: branches: [ humble-dev, aoc ] From d818dbbb4d93785314e5342534dadcc69fc619cb Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Tue, 16 Dec 2025 12:34:37 +0000 Subject: [PATCH 14/23] Enhance documentation for flexible properties system with namespace organization guidelines and examples --- topological_navigation/doc/PROPERTIES.md | 144 +++++++++++++++++++---- 1 file changed, 123 insertions(+), 21 deletions(-) diff --git a/topological_navigation/doc/PROPERTIES.md b/topological_navigation/doc/PROPERTIES.md index 77b5975b..fc7de101 100644 --- a/topological_navigation/doc/PROPERTIES.md +++ b/topological_navigation/doc/PROPERTIES.md @@ -108,6 +108,7 @@ To organise properties by functional area or application, use **namespaces**. A - Prevents naming conflicts between different systems - Makes it clear which application or module owns each property - Improves readability and maintainability +- Supports both domain-based and package-based organisation #### Namespace Structure @@ -118,19 +119,61 @@ properties: property2: value2 ``` +#### Namespace Organisation Approaches + +You can organise namespaces in different ways depending on your needs: + +1. **Domain-based namespaces** (by concept or functional area): + ```yaml + properties: + restrictions: # Physical and access constraints + capacity: 2 + max_external_width: 0.8 + semantics: # Semantic labels and classifications + zone: "warehouse" + type: "charging_station" + ``` + +2. **Package-based namespaces** (by ROS package): + ```yaml + properties: + my_fleet_manager: + priority_zone: true + my_safety_module: + emergency_stop_point: false + ``` + +3. **Flat structure** (no namespaces): + ```yaml + properties: + capacity: 2 + zone: "warehouse" + ``` + +All three approaches are valid. Choose the organisation that best fits your application's needs and team conventions. + #### Common Namespace Examples -| Namespace | Purpose | -|-----------|---------| -| `navigation` | Core navigation parameters (tolerances, speeds) | -| `fleet_management` | Multi-robot coordination properties | -| `logistics` | Warehouse/delivery application properties | -| `agriculture` | Agricultural robotics properties | -| `safety` | Safety-related constraints and limits | -| `semantics` | Semantic labels and classifications | +**Domain-based namespaces:** + +| Namespace | Purpose | Example Properties | +|-----------|---------|-------------------| +| `restrictions` | Physical and access constraints | `capacity`, `max_external_width`, `max_external_height`, `access_level` | +| `semantics` | Semantic labels and classifications | `zone`, `type`, `features`, `environmental_conditions` | +| `navigation` | Navigation-specific parameters | `xy_goal_tolerance`, `yaw_goal_tolerance` | +| `safety` | Safety-related constraints | `restricted_hours`, `emergency_access`, `hazard_zones` | + +**Application/package-based namespaces:** + +| Namespace | Purpose | Example Properties | +|-----------|---------|-------------------| +| `fleet_management` | Multi-robot coordination properties | `priority_zone`, `queue_enabled` | +| `logistics` | Warehouse/delivery application properties | `pickup_point`, `dropoff_point`, `storage_type` | +| `agriculture` | Agricultural robotics properties | `row_id`, `crop_type`, `irrigation_zone` | #### Namespaced Node Properties Example +**Package-based organisation:** ```yaml node: name: ChargingStation1 @@ -150,8 +193,33 @@ node: dropoff_point: false ``` +**Domain-based organisation (alternative):** +```yaml +node: + name: PolytunnelRow4_Col2 + properties: + restrictions: + capacity: 1 + max_external_width: 0.8 # metres + max_external_height: 1.5 # metres + min_internal_width: 1.2 # metres + min_internal_height: 2.0 # metres + access_level: "operator" + semantics: + zone: + labels: ['polytunnel', 'growing_area'] + details: {tunnel_id: 4, row_id: 5, column_idx: 2} + features: + labels: ['irrigation_point', 'sensor_node'] + details: {sensor_types: ['humidity', 'temperature']} + environmental: + labels: ['soil', 'shade'] + details: {soil_type: 'loam', 'shade_percentage': 30} +``` + #### Namespaced Edge Properties Example +**Package-based organisation:** ```yaml edges: - edge_id: ChargingStation1_WayPoint2 @@ -159,7 +227,6 @@ edges: properties: navigation: max_speed: 0.5 - bidirectional: true safety: width: 1.2 restricted_hours: ["22:00-06:00"] @@ -168,33 +235,68 @@ edges: surface_type: "concrete" ``` +**Domain-based organisation (alternative):** +```yaml +edges: +- edge_id: Row4_Col2_Row4_Col3 + node: Row4_Col3 + properties: + restrictions: + max_external_width: 0.75 # metres - narrow passage + max_external_height: 1.8 # metres - low overhead + capacity: 1 # single robot at a time + semantics: + zone: + labels: ['polytunnel_interior', 'growing_area'] + details: {tunnel_id: 4} + environmental: + labels: ['grass', 'uneven'] + details: {surface_quality: 'variable'} +``` + #### Accessing Namespaced Properties in Code ```python # Safe access to namespaced node properties node_props = node["node"].get("properties", {}) -# Access fleet_management namespace +# Access domain-based namespace +restrictions = node_props.get("restrictions", {}) +capacity = restrictions.get("capacity", 1) +max_width = restrictions.get("max_external_width") + +# Access package-based namespace fleet_props = node_props.get("fleet_management", {}) -capacity = fleet_props.get("capacity", 1) +priority_zone = fleet_props.get("priority_zone", False) -# Access navigation namespace -nav_props = node_props.get("navigation", {}) -xy_tolerance = nav_props.get("xy_goal_tolerance", 0.3) +# Access nested semantic properties +semantics = node_props.get("semantics", {}) +zone_info = semantics.get("zone", {}) +zone_labels = zone_info.get("labels", []) +zone_details = zone_info.get("details", {}) # Access namespaced edge properties edge_props = edge.get("properties", {}) -safety_props = edge_props.get("safety", {}) -max_width = safety_props.get("width") +restrictions = edge_props.get("restrictions", {}) +max_width = restrictions.get("max_external_width") ``` #### Guidelines for Namespace Usage -1. **Use namespaces for application-specific properties**: If your properties are only used by a specific application or module, group them under a namespace. -2. **Keep core navigation properties at root level**: For backwards compatibility, standard navigation properties like `xy_goal_tolerance` can remain at the root level. -3. **Document your namespaces**: If you introduce a new namespace, document its purpose and expected properties. -4. **Avoid deep nesting**: One level of namespace nesting is usually sufficient. Avoid creating deeply nested structures. -5. By convention, ROS packages should use their package name as the namespace. E.g. a package called "topfleets_coordinator" should use this as the namespace for properties it uses. +1. **Choose the right organisation for your use case**: + - Use **domain-based namespaces** (like `restrictions`, `semantics`) when organising by conceptual categories + - Use **package-based namespaces** when multiple ROS packages share the same map and need to avoid conflicts + - Use **flat structure** for simple cases with few properties + +2. **Be consistent within your project**: Choose one organisational approach and stick with it across your maps. + +3. **Document your namespace conventions**: If you introduce new namespaces, document their purpose and expected properties in your project documentation. + +4. **Avoid deep nesting**: One level of namespace nesting is usually sufficient. Deeper nesting (like `properties.semantics.zone.labels`) should be reserved for truly hierarchical data. + +5. **Backwards compatibility**: For core navigation properties like `xy_goal_tolerance`, either keep them at root level or consistently place them in a `navigation` namespace across all maps. + +6. **Package name convention** (optional): ROS packages that define properties can use their package name as a namespace to avoid conflicts. E.g., a package called "topfleets_coordinator" could use this as the namespace. ### Type Flexibility From 7e025ea39da485af611174277f8b24d3a58a3117 Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Tue, 16 Dec 2025 12:36:10 +0000 Subject: [PATCH 15/23] Add GitHub Actions workflow to validate YAML schema --- .github/workflows/validate-schema.yaml | 51 ++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 .github/workflows/validate-schema.yaml diff --git a/.github/workflows/validate-schema.yaml b/.github/workflows/validate-schema.yaml new file mode 100644 index 00000000..d4642f21 --- /dev/null +++ b/.github/workflows/validate-schema.yaml @@ -0,0 +1,51 @@ +name: Validate Schema + +on: + push: + branches: [ humble-dev, aoc ] + tags: + - '*' + pull_request: + branches: [ humble-dev, aoc ] + +jobs: + validate_schema: + name: Validate YAML Schema + runs-on: ubuntu-latest + steps: + - name: Checkout code + uses: actions/checkout@v4 + + - name: Set up Python + uses: actions/setup-python@v6 + with: + python-version: '3.12' + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install jsonschema pyyaml check-jsonschema + + - name: Validate schema file + run: | + # Validate that the schema itself is valid JSON Schema Draft 7 + check-jsonschema --check-metaschema topological_navigation/config/tmap-schema.yaml + + - name: Test schema with Python + run: | + python -c " + import yaml + import jsonschema + + # Load the schema + with open('topological_navigation/config/tmap-schema.yaml', 'r') as f: + schema = yaml.safe_load(f) + + # Verify it's a valid JSON Schema by creating a validator + try: + jsonschema.validators.Draft7Validator.check_schema(schema) + print('✓ Schema is valid JSON Schema Draft 7') + except jsonschema.exceptions.SchemaError as e: + print(f'✗ Schema validation failed: {e}') + exit(1) + " From 725b830a24eab465cf2f76f0693cdc8fa6eaf5f5 Mon Sep 17 00:00:00 2001 From: Ibrahim Hroob <47870260+ibrahimhroob@users.noreply.github.com> Date: Tue, 16 Dec 2025 12:54:24 +0000 Subject: [PATCH 16/23] Enhance EdgeActionManager with row operation handling and boundary node selection logic (#217) --- .../edge_action_manager2.py | 417 ++++++++++++++---- 1 file changed, 325 insertions(+), 92 deletions(-) diff --git a/topological_navigation/topological_navigation/edge_action_manager2.py b/topological_navigation/topological_navigation/edge_action_manager2.py index bf6d9c49..65bbd638 100644 --- a/topological_navigation/topological_navigation/edge_action_manager2.py +++ b/topological_navigation/topological_navigation/edge_action_manager2.py @@ -101,9 +101,12 @@ def getBoundary(self, ): header.frame_id = self.target_frame_id path.header = header if(len(self.side_edges)>0): + print(" >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> getBoundary") for key, val in self.side_edges.items(): + print(key) for pose in val: path.poses.append(pose) + print(" >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>") return path @@ -204,7 +207,58 @@ def get_goal_cancel_error_msg(self, status_code): except Exception as e: self.get_logger().error("Goal cancel code {}".format(status_code)) return self.ACTIONS.goal_cancel_error_codes[0] - + + def _is_waypoint_name(self, name: str) -> bool: + n = (name or "").lower() + return ("waypoint" in n) or n.startswith("wp") or ("_wp" in n) + + def _is_row_node_name(self, name: str) -> bool: + # Uses your existing constant that you already rely on elsewhere + # (you used startswith(ROW_COLUMN_START_INDEX) in execute_row_operation_action) + if not name: + return False + n = str(name) + return n.split("-")[-1].startswith(self.ACTIONS.ROW_COLUMN_START_INDEX) and (not self._is_waypoint_name(n)) + + def _yaw_from_quat(self, q) -> float: + # q: dict with x,y,z,w + x, y, z, w = q["x"], q["y"], q["z"], q["w"] + siny = 2.0 * (w * z + x * y) + cosy = 1.0 - 2.0 * (y * y + z * z) + return math.atan2(siny, cosy) + + def _set_pose_yaw(self, pose_stamped: PoseStamped, yaw: float): + pose_stamped.pose.orientation.x = 0.0 + pose_stamped.pose.orientation.y = 0.0 + pose_stamped.pose.orientation.z = math.sin(yaw / 2.0) + pose_stamped.pose.orientation.w = math.cos(yaw / 2.0) + + def _pick_last_row_goal(self, nodes): + """ + nodes: list of dict goals (each has target_pose.pose.position) + Prefer the last goal that corresponds to a ROW node (not a waypoint). + """ + last_row = None + for n in nodes: + p = n["target_pose"]["pose"]["position"] + xy = (p["x"], p["y"]) + name = getattr(self, "destination_node_str", {}).get(xy) + if name and self._is_row_node_name(name): + last_row = n + return last_row if last_row is not None else nodes[-1] + + def _compute_row_yaw(self, cen_pose_dict, last_goal_dict) -> float: + cpos = cen_pose_dict["node"]["pose"]["position"] + lpos = last_goal_dict["target_pose"]["pose"]["position"] + dx = lpos["x"] - cpos["x"] + dy = lpos["y"] - cpos["y"] + if abs(dx) + abs(dy) > 1e-6: + return math.atan2(dy, dx) + + # fallback: center node orientation yaw + cor = cen_pose_dict["node"]["pose"]["orientation"] + return self._yaw_from_quat(cor) + def _adjust_orientations_for_next_wp(self, poses_dict): """ @@ -239,7 +293,6 @@ def _adjust_orientations_for_next_wp(self, poses_dict): self.get_logger().info("[_adjust_orientations_for_next_wp] Adjusted orientation for pose {} to face next goal".format(i)) return poses_dict - def _process_and_segment_edges(self, edge, destination_node, origin_node, is_execpolicy): """ @@ -497,10 +550,8 @@ def check_edges_area_same(self, side_edges): def check_target_is_same(self, node1, node2): target1 = np.array([node1["pose"]["position"]["x"], node1["pose"]["position"]["y"]]) - target2 = np.array([node2["pose"]["position"]["x"], node1["pose"]["position"]["y"]]) - if(np.linalg.norm(target1-target2) < 0.001): - return True - return False + target2 = np.array([node2["pose"]["position"]["x"], node2["pose"]["position"]["y"]]) + return np.linalg.norm(target1 - target2) < 0.001 def two_smallest_indices(self, lst): if len(lst) < 1: @@ -576,96 +627,278 @@ def publish_target_edges_as_path(self, selected_edges_dict): def get_navigate_through_poses_goal(self, poses, actions, edge_ids, is_execpolicy=False): + def handle_row_operation(): - self.target_row_edge_id = edge_id.split("_")[0] - tag_id = self.target_row_edge_id.split("-")[1] - self.target_row_edge_id = self.target_row_edge_id[:-1] + self.ACTIONS.ROW_START_INDEX - tag_id = tag_id[:-1] + self.ACTIONS.ROW_START_INDEX - - self.get_logger().info("Action in_row_operation ") - self.get_logger().info("Edge id and tag id {} {}".format(self.target_row_edge_id, tag_id)) - cen = self.route_search.get_node_from_tmap2(self.target_row_edge_id) - self.publish_target_edges_as_path(cen) - + """ + Build row boundaries (selected_edges) ONLY from row-related boundary nodes, + and force boundary pose orientations to align with the row direction. + + Returns: (action, action_msg) + """ + + # --------------------------- + # Small local helpers + # --------------------------- + def yaw_from_quat(q): + x, y, z, w = q["x"], q["y"], q["z"], q["w"] + siny = 2.0 * (w * z + x * y) + cosy = 1.0 - 2.0 * (y * y + z * z) + return math.atan2(siny, cosy) + + def set_pose_yaw(ps: PoseStamped, yaw: float): + ps.pose.orientation.x = 0.0 + ps.pose.orientation.y = 0.0 + ps.pose.orientation.z = math.sin(yaw / 2.0) + ps.pose.orientation.w = math.cos(yaw / 2.0) + + def is_waypoint_name(name: str) -> bool: + n = (name or "").lower() + return ("waypoint" in n) or n.startswith("wp") or ("_wp" in n) + + def is_row_node_name(name: str) -> bool: + # Uses your existing convention used elsewhere in the code + # (ROW_COLUMN_START_INDEX = row column prefix) + if not name: + return False + n = str(name) + suffix = n.split("-")[-1] + return suffix.startswith(self.ACTIONS.ROW_COLUMN_START_INDEX) and (not is_waypoint_name(n)) + + def publish_empty_boundary(frame_id: str): + empty = Path() + empty.header.frame_id = frame_id + empty.header.stamp = self.get_clock().now().to_msg() + self.boundary_publisher.publish(empty) + + # --------------------------- + # Validate inputs / frame id + # --------------------------- + if not nodes or len(nodes) == 0: + self.get_logger().error("[handle_row_operation] nodes list is empty. Clearing boundary.") + publish_empty_boundary("map") + action_msg.setSideEdges({}, "map") + return self.ACTIONS.ROW_OPERATION, action_msg + + target_pose_frame_id = nodes[0]["target_pose"]["header"].get("frame_id", "map") + + if not edge_id: + self.get_logger().error("[handle_row_operation] edge_id is empty. Clearing boundary.") + publish_empty_boundary(target_pose_frame_id) + action_msg.setSideEdges({}, target_pose_frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg + + # --------------------------- + # Derive "row start" center id + # --------------------------- + try: + target_row_edge_id_raw = edge_id.split("_")[0] + tag_id = target_row_edge_id_raw.split("-")[1] + # Force ROW_START_INDEX by replacing the last character (keeps your existing convention) + target_row_edge_id = target_row_edge_id_raw[:-1] + self.ACTIONS.ROW_START_INDEX + tag_id = tag_id[:-1] + self.ACTIONS.ROW_START_INDEX + except Exception as e: + self.get_logger().error(f"[handle_row_operation] Failed to parse row ids from edge_id='{edge_id}': {e}. Clearing boundary.") + publish_empty_boundary(target_pose_frame_id) + action_msg.setSideEdges({}, target_pose_frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg + + self.get_logger().info(f"[handle_row_operation] edge_id={edge_id} -> center_row_id={target_row_edge_id} tag_id={tag_id}") + + cen = self.route_search.get_node_from_tmap2(target_row_edge_id) + if not cen or "node" not in cen or "pose" not in cen["node"]: + self.get_logger().error(f"[handle_row_operation] Could not resolve center node '{target_row_edge_id}'. Clearing boundary.") + publish_empty_boundary(target_pose_frame_id) + action_msg.setSideEdges({}, target_pose_frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg + + # Publish /target_edge_path for the center node (your existing behavior) + try: + self.publish_target_edges_as_path(cen) + except Exception as e: + self.get_logger().warn(f"[handle_row_operation] publish_target_edges_as_path failed: {e}") + + # --------------------------- + # Collect candidate boundary nodes around the row center + # (keeps your existing connectivity logic but adds filters) + # --------------------------- children = self.route_search.get_connected_nodes_tmap2(cen) - selected_row_edge_nodes = {} - for next_edge in children: - if(next_edge.startswith(self.ACTIONS.OUTSIDE_EDGE_START_INDEX)): - upper_nodes = self.route_search.get_node_from_tmap2(next_edge)["node"]["edges"] - for edges_all in upper_nodes: - if((self.ACTIONS.GOAL_ALIGN_INDEX[0] in edges_all["node"]) - and (self.target_row_edge_id not in edges_all["node"])): - - targte_pose = self.route_search.get_node_from_tmap2(edges_all["node"])["node"]["pose"] - targte_pose_x_y = np.array([targte_pose["position"]["x"], targte_pose["position"]["y"]]) - selected_row_edge_nodes[edges_all["node"]] = (targte_pose, targte_pose_x_y) - - center_pose = self.route_search.get_node_from_tmap2(self.target_row_edge_id)["node"]["pose"]["position"] - center_pose = np.array([center_pose["x"], center_pose["y"]]) - distance_vector = [] - distance_with_edge_ids = {} - index_dis = 0 - for the_key, the_value in selected_row_edge_nodes.items(): - distance_vector.append(np.linalg.norm(center_pose- the_value[1])) - distance_with_edge_ids[index_dis] = the_key - index_dis += 1 - min_indices = self.two_smallest_indices(distance_vector) - children = [] - for index in min_indices: - children.append(distance_with_edge_ids[index]) - - # Sort the list based on the extracted number in descending order - children = sorted(children, key=self.extract_number, reverse=True) - - self.get_logger().info("Children edges {}".format(children)) - - target_pose_frame_id = nodes[0]["target_pose"]["header"]["frame_id"] - last_goal = nodes[-1] - - if(len(nodes) > 1): - if(self.check_target_is_same(cen["node"], last_goal["target_pose"])): - last_goal = nodes[0] - - selected_last_node = last_goal - if(len(nodes) == 1): - edges = edge_id.split("_") - if(len(edges) == 2): - edge_0, edge_1 = edges[0], edges[1] - tag_0, tag_1 = edge_0.split("-")[1], edge_1.split("-")[1] - if(tag_1 == tag_id): - selected_last_node["target_pose"]["pose"] = self.route_search.get_node_from_tmap2(edge_0)["node"]["pose"] - elif(tag_0 == tag_id): - selected_last_node["target_pose"]["pose"] = self.route_search.get_node_from_tmap2(edge_1)["node"]["pose"] - elif(len(selected_last_node) == 0 and self.current_node is not None): - selected_last_node["target_pose"]["pose"] = self.route_search.get_node_from_tmap2(self.current_node)["node"]["pose"] - else: - self.get_logger().error("Cound not find bounday edge...") - - self.selected_edges = {} - for child in children: - if tag_id in child: - child_node = self.route_search.get_node_from_tmap2(child) - side_intermediate_pose = self.get_intermediate_pose(cen, child_node, target_pose_frame_id) - side_last_intermediate_pose = self.get_last_intermediate_pose(side_intermediate_pose, cen, selected_last_node) - self.selected_edges[child] = [side_intermediate_pose, side_last_intermediate_pose] - - if(len(self.selected_edges) == 1): + selected_row_edge_nodes = {} # node_id -> (pose_dict, xy_np) + + for next_edge in (children or []): + if not next_edge.startswith(self.ACTIONS.OUTSIDE_EDGE_START_INDEX): + continue + + next_edge_node = self.route_search.get_node_from_tmap2(next_edge) + if not next_edge_node or "node" not in next_edge_node: + continue + + upper_edges = next_edge_node["node"].get("edges", []) + for edges_all in upper_edges: + node_id = edges_all.get("node", "") + if not isinstance(node_id, str) or not node_id: + continue + + # keep your original constraint + if (self.ACTIONS.GOAL_ALIGN_INDEX[0] not in node_id): + continue + if (target_row_edge_id in node_id): + continue + + # Exclude waypoints explicitly + if is_waypoint_name(node_id): + continue + + node_obj = self.route_search.get_node_from_tmap2(node_id) + if not node_obj or "node" not in node_obj or "pose" not in node_obj["node"]: + continue + + pose = node_obj["node"]["pose"] + xy = np.array([pose["position"]["x"], pose["position"]["y"]], dtype=float) + selected_row_edge_nodes[node_id] = (pose, xy) + + if len(selected_row_edge_nodes) == 0: + self.get_logger().error("[handle_row_operation] No candidate boundary nodes found. Clearing boundary.") + publish_empty_boundary(target_pose_frame_id) + action_msg.setSideEdges({}, target_pose_frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg + + # --------------------------- + # Choose last goal INSIDE the row (avoid waypoint outside-row corrupting direction) + # Uses destination_node_str if available; otherwise falls back to nodes[-1] + # --------------------------- + selected_last_node = None + try: + # Prefer the last goal that is a ROW node (not a waypoint) + for g in nodes: + p = g["target_pose"]["pose"]["position"] + xy = (p["x"], p["y"]) + name = getattr(self, "destination_node_str", {}).get(xy) + if name and is_row_node_name(name): + selected_last_node = g + except Exception: + selected_last_node = None + + if selected_last_node is None: + selected_last_node = nodes[-1] + + # If last goal is exactly the center, use first goal as fallback + try: + cpos = cen["node"]["pose"]["position"] + lpos = selected_last_node["target_pose"]["pose"]["position"] + if np.linalg.norm(np.array([lpos["x"] - cpos["x"], lpos["y"] - cpos["y"]], dtype=float)) < 1e-6: + selected_last_node = nodes[0] + except Exception: + pass + + # --------------------------- + # Compute row direction (yaw) + # --------------------------- + cpos = cen["node"]["pose"]["position"] + lpos = selected_last_node["target_pose"]["pose"]["position"] + dx = lpos["x"] - cpos["x"] + dy = lpos["y"] - cpos["y"] + + if abs(dx) + abs(dy) > 1e-6: + row_yaw = math.atan2(dy, dx) + else: + row_yaw = yaw_from_quat(cen["node"]["pose"]["orientation"]) + + row_dir = np.array([math.cos(row_yaw), math.sin(row_yaw)], dtype=float) + center_xy = np.array([cpos["x"], cpos["y"]], dtype=float) + + # --------------------------- + # Pick closest LEFT and closest RIGHT boundary node (relative to row direction) + # Also enforce "lateral" boundary: candidate vector should be ~perpendicular to row_dir + # --------------------------- + candidates = [] # (side_sign, dist, node_id) + + for node_id, (_pose, txy) in selected_row_edge_nodes.items(): + # Must be for this row tag + if tag_id not in node_id: + continue + + v = (txy - center_xy) + dist = float(np.linalg.norm(v)) + if dist < 1e-6: + continue + + # lateral filter: reject candidates that are too aligned with row_dir + cosang = abs(float(np.dot(row_dir, v) / dist)) + if cosang > 0.6: + continue + + crossz = float(row_dir[0] * v[1] - row_dir[1] * v[0]) + side = 1.0 if crossz > 0.0 else -1.0 + candidates.append((side, dist, node_id)) + + if len(candidates) == 0: + self.get_logger().error("[handle_row_operation] Candidates exist but none passed lateral/tag filters. Clearing boundary.") + publish_empty_boundary(target_pose_frame_id) + action_msg.setSideEdges({}, target_pose_frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg + + left = min((c for c in candidates if c[0] > 0.0), default=None, key=lambda x: x[1]) + right = min((c for c in candidates if c[0] < 0.0), default=None, key=lambda x: x[1]) + + picked = [] + if left: + picked.append(left[2]) + if right: + picked.append(right[2]) + + # Fallback: if only one side found, take the next closest (still filtered) + if len(picked) < 2: + for _, _, nid in sorted(candidates, key=lambda x: x[1]): + if nid not in picked: + picked.append(nid) + if len(picked) == 2: + break + + # --------------------------- + # Build selected_edges (front/back) and force yaw to row_yaw + # --------------------------- + self.selected_edges = {} + + for child in picked: + child_node = self.route_search.get_node_from_tmap2(child) + if not child_node or "node" not in child_node: + continue + + side_intermediate_pose = self.get_intermediate_pose(cen, child_node, target_pose_frame_id) + side_last_intermediate_pose = self.get_last_intermediate_pose(side_intermediate_pose, cen, selected_last_node) + + # Force boundary direction to be aligned with the row + set_pose_yaw(side_intermediate_pose, row_yaw) + set_pose_yaw(side_last_intermediate_pose, row_yaw) + + self.selected_edges[child] = [side_intermediate_pose, side_last_intermediate_pose] + + # If only one wall found, mirror to create opposite wall (and align yaw) + if len(self.selected_edges) == 1: self.selected_edges["side_wall"] = self.get_intermediate_poses_interpolated(self.selected_edges, cen, selected_last_node) + for ps in self.selected_edges["side_wall"]: + set_pose_yaw(ps, row_yaw) + + if len(self.selected_edges) == 0: + self.get_logger().error("[handle_row_operation] selected_edges ended empty. Clearing boundary.") + publish_empty_boundary(target_pose_frame_id) + action_msg.setSideEdges({}, target_pose_frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg + + # --------------------------- + # Publish /boundary_checker once (latched) + attach to action_msg + # --------------------------- + action_msg.setSideEdges(self.selected_edges, target_pose_frame_id) + + if self.is_row_boundary_published is False: + boundary_info = action_msg.getBoundary() + boundary_info.header.stamp = self.get_clock().now().to_msg() + self.boundary_publisher.publish(boundary_info) + self.is_row_boundary_published = True + + return self.ACTIONS.ROW_OPERATION, action_msg + - if(self.check_edges_area_same(self.selected_edges)): - edge_action_is_valid = True - self.get_logger().error("Bounday edges are same") - else: - edge_action_is_valid = True - if edge_action_is_valid: - action_msg.setSideEdges(self.selected_edges, target_pose_frame_id) - if self.is_row_boundary_published == False: - boundary_info = action_msg.getBoundary() - self.boundary_publisher.publish(boundary_info) - self.is_row_boundary_published = True - action = self.ACTIONS.ROW_OPERATION - return action, action_msg - control_server_configs = {} action_msgs = [] From 04d5857b47ebf186e116f570dd6ed1bd0a6cdd87 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 7 Jan 2026 17:29:24 +0000 Subject: [PATCH 17/23] Fix closest node handling in TopologicalNavServer to improve navigation accuracy --- .../scripts/navigation2.py | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/topological_navigation/topological_navigation/scripts/navigation2.py b/topological_navigation/topological_navigation/scripts/navigation2.py index 982c93e6..0bef2339 100755 --- a/topological_navigation/topological_navigation/scripts/navigation2.py +++ b/topological_navigation/topological_navigation/scripts/navigation2.py @@ -845,6 +845,7 @@ 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) @@ -852,6 +853,9 @@ def navigate(self, target): 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)) @@ -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]) @@ -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") @@ -1316,4 +1327,4 @@ def main(): rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() From 03748ef1bf742c9c0e052fefd8524bafb6f981e0 Mon Sep 17 00:00:00 2001 From: Ibrahim Hroob <47870260+ibrahimhroob@users.noreply.github.com> Date: Sun, 18 Jan 2026 11:33:45 +0000 Subject: [PATCH 18/23] =?UTF-8?q?Refactor=20EdgeActionManager=20to=20simpl?= =?UTF-8?q?ify=20row=20operation=20handling=20and=20imp=E2=80=A6=20(#231)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Refactor EdgeActionManager to simplify row operation handling and improve boundary node selection logic * Fix publisher topic for center node pose in EdgeActionManager --- .../edge_action_manager2.py | 535 +++++++++--------- 1 file changed, 258 insertions(+), 277 deletions(-) diff --git a/topological_navigation/topological_navigation/edge_action_manager2.py b/topological_navigation/topological_navigation/edge_action_manager2.py index 65bbd638..818d45d4 100644 --- a/topological_navigation/topological_navigation/edge_action_manager2.py +++ b/topological_navigation/topological_navigation/edge_action_manager2.py @@ -101,12 +101,9 @@ def getBoundary(self, ): header.frame_id = self.target_frame_id path.header = header if(len(self.side_edges)>0): - print(" >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> getBoundary") for key, val in self.side_edges.items(): - print(key) for pose in val: path.poses.append(pose) - print(" >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>") return path @@ -169,7 +166,8 @@ def init(self, ACTIONS, route_search, update_params_control_server, inrow_step_s self.robot_current_status_pub = self.create_publisher(String, '/robot_operation_current_status', qos_profile=self.latching_qos) self.current_dest = self.create_publisher(String, '/topological_navigation/current_destination', qos_profile=self.latching_qos) self.target_edge_path_pub = self.create_publisher(Path, "/target_edge_path", qos_profile=self.latching_qos) - + self.center_node_pose_pub = self.create_publisher(PoseStamped, "/center_node/pose", qos_profile=self.latching_qos) + self.robot_current_behavior_pub = None self.current_node = None self.action_status = 0 @@ -233,32 +231,6 @@ def _set_pose_yaw(self, pose_stamped: PoseStamped, yaw: float): pose_stamped.pose.orientation.z = math.sin(yaw / 2.0) pose_stamped.pose.orientation.w = math.cos(yaw / 2.0) - def _pick_last_row_goal(self, nodes): - """ - nodes: list of dict goals (each has target_pose.pose.position) - Prefer the last goal that corresponds to a ROW node (not a waypoint). - """ - last_row = None - for n in nodes: - p = n["target_pose"]["pose"]["position"] - xy = (p["x"], p["y"]) - name = getattr(self, "destination_node_str", {}).get(xy) - if name and self._is_row_node_name(name): - last_row = n - return last_row if last_row is not None else nodes[-1] - - def _compute_row_yaw(self, cen_pose_dict, last_goal_dict) -> float: - cpos = cen_pose_dict["node"]["pose"]["position"] - lpos = last_goal_dict["target_pose"]["pose"]["position"] - dx = lpos["x"] - cpos["x"] - dy = lpos["y"] - cpos["y"] - if abs(dx) + abs(dy) > 1e-6: - return math.atan2(dy, dx) - - # fallback: center node orientation yaw - cor = cen_pose_dict["node"]["pose"]["orientation"] - return self._yaw_from_quat(cor) - def _adjust_orientations_for_next_wp(self, poses_dict): """ @@ -625,279 +597,287 @@ def publish_target_edges_as_path(self, selected_edges_dict): self.get_logger().info(f"Publishing Path with {valid_poses_count} poses") self.target_edge_path_pub.publish(path_msg) - def get_navigate_through_poses_goal(self, poses, actions, edge_ids, is_execpolicy=False): - + def _publish_empty_boundary(self, frame_id: str): + """Publish an empty boundary path.""" + empty = Path() + empty.header.frame_id = frame_id + empty.header.stamp = self.get_clock().now().to_msg() + self.boundary_publisher.publish(empty) - def handle_row_operation(): - """ - Build row boundaries (selected_edges) ONLY from row-related boundary nodes, - and force boundary pose orientations to align with the row direction. - - Returns: (action, action_msg) - """ - - # --------------------------- - # Small local helpers - # --------------------------- - def yaw_from_quat(q): - x, y, z, w = q["x"], q["y"], q["z"], q["w"] - siny = 2.0 * (w * z + x * y) - cosy = 1.0 - 2.0 * (y * y + z * z) - return math.atan2(siny, cosy) - - def set_pose_yaw(ps: PoseStamped, yaw: float): - ps.pose.orientation.x = 0.0 - ps.pose.orientation.y = 0.0 - ps.pose.orientation.z = math.sin(yaw / 2.0) - ps.pose.orientation.w = math.cos(yaw / 2.0) - - def is_waypoint_name(name: str) -> bool: - n = (name or "").lower() - return ("waypoint" in n) or n.startswith("wp") or ("_wp" in n) - - def is_row_node_name(name: str) -> bool: - # Uses your existing convention used elsewhere in the code - # (ROW_COLUMN_START_INDEX = row column prefix) - if not name: - return False - n = str(name) - suffix = n.split("-")[-1] - return suffix.startswith(self.ACTIONS.ROW_COLUMN_START_INDEX) and (not is_waypoint_name(n)) - - def publish_empty_boundary(frame_id: str): - empty = Path() - empty.header.frame_id = frame_id - empty.header.stamp = self.get_clock().now().to_msg() - self.boundary_publisher.publish(empty) - - # --------------------------- - # Validate inputs / frame id - # --------------------------- - if not nodes or len(nodes) == 0: - self.get_logger().error("[handle_row_operation] nodes list is empty. Clearing boundary.") - publish_empty_boundary("map") - action_msg.setSideEdges({}, "map") - return self.ACTIONS.ROW_OPERATION, action_msg - - target_pose_frame_id = nodes[0]["target_pose"]["header"].get("frame_id", "map") - - if not edge_id: - self.get_logger().error("[handle_row_operation] edge_id is empty. Clearing boundary.") - publish_empty_boundary(target_pose_frame_id) - action_msg.setSideEdges({}, target_pose_frame_id) - return self.ACTIONS.ROW_OPERATION, action_msg - - # --------------------------- - # Derive "row start" center id - # --------------------------- - try: - target_row_edge_id_raw = edge_id.split("_")[0] - tag_id = target_row_edge_id_raw.split("-")[1] - # Force ROW_START_INDEX by replacing the last character (keeps your existing convention) - target_row_edge_id = target_row_edge_id_raw[:-1] + self.ACTIONS.ROW_START_INDEX - tag_id = tag_id[:-1] + self.ACTIONS.ROW_START_INDEX - except Exception as e: - self.get_logger().error(f"[handle_row_operation] Failed to parse row ids from edge_id='{edge_id}': {e}. Clearing boundary.") - publish_empty_boundary(target_pose_frame_id) - action_msg.setSideEdges({}, target_pose_frame_id) - return self.ACTIONS.ROW_OPERATION, action_msg + def _get_row_center_node(self, edge_id: str): + """ + Parse edge_id to find the row center node. + Returns: (center_node, tag_id, target_row_edge_id) or (None, None, None) on failure. + """ + try: + target_row_edge_id_raw = edge_id.split("_")[0] + tag_id = target_row_edge_id_raw.split("-")[1] + # print trget_row_edge_id_raw, tag_id + self.get_logger().info(f"[_get_row_center_node] Parsed edge_id='{edge_id}' to target_row_edge_id_raw='{target_row_edge_id_raw}', tag_id='{tag_id}'") + # Force ROW_START_INDEX by replacing the last character + target_row_edge_id = target_row_edge_id_raw[:-1] + self.ACTIONS.ROW_START_INDEX + tag_id = tag_id[:-1] + self.ACTIONS.ROW_START_INDEX + #print target_row_edge_id, tag_id + self.get_logger().info(f"[_get_row_center_node] Adjusted to target_row_edge_id='{target_row_edge_id}', tag_id='{tag_id}'") + except Exception as e: + self.get_logger().error(f"[_get_row_center_node] Failed to parse edge_id='{edge_id}': {e}") + return None, None, None - self.get_logger().info(f"[handle_row_operation] edge_id={edge_id} -> center_row_id={target_row_edge_id} tag_id={tag_id}") + cen = self.route_search.get_node_from_tmap2(target_row_edge_id) + if not cen or "node" not in cen or "pose" not in cen["node"]: + self.get_logger().error(f"[_get_row_center_node] Could not resolve '{target_row_edge_id}'") + return None, None, None - cen = self.route_search.get_node_from_tmap2(target_row_edge_id) - if not cen or "node" not in cen or "pose" not in cen["node"]: - self.get_logger().error(f"[handle_row_operation] Could not resolve center node '{target_row_edge_id}'. Clearing boundary.") - publish_empty_boundary(target_pose_frame_id) - action_msg.setSideEdges({}, target_pose_frame_id) - return self.ACTIONS.ROW_OPERATION, action_msg + return cen, tag_id, target_row_edge_id - # Publish /target_edge_path for the center node (your existing behavior) - try: - self.publish_target_edges_as_path(cen) - except Exception as e: - self.get_logger().warn(f"[handle_row_operation] publish_target_edges_as_path failed: {e}") + def _collect_boundary_candidates(self, cen, target_row_edge_id: str): + """ + Collect candidate boundary nodes from connected WayPoint nodes. + Returns: dict of node_id -> (pose_dict, xy_np) + """ + candidates = {} + children = self.route_search.get_connected_nodes_tmap2(cen) or [] - # --------------------------- - # Collect candidate boundary nodes around the row center - # (keeps your existing connectivity logic but adds filters) - # --------------------------- - children = self.route_search.get_connected_nodes_tmap2(cen) - selected_row_edge_nodes = {} # node_id -> (pose_dict, xy_np) + for next_edge in children: + if not next_edge.startswith(self.ACTIONS.OUTSIDE_EDGE_START_INDEX): + continue - for next_edge in (children or []): - if not next_edge.startswith(self.ACTIONS.OUTSIDE_EDGE_START_INDEX): + next_edge_node = self.route_search.get_node_from_tmap2(next_edge) + if not next_edge_node or "node" not in next_edge_node: + continue + + for edge_info in next_edge_node["node"].get("edges", []): + node_id = edge_info.get("node", "") + if not node_id or not isinstance(node_id, str): + continue + # Must contain GOAL_ALIGN_INDEX and not be the target row itself + if self.ACTIONS.GOAL_ALIGN_INDEX[0] not in node_id: + continue + if target_row_edge_id in node_id: + continue + if self._is_waypoint_name(node_id): continue - next_edge_node = self.route_search.get_node_from_tmap2(next_edge) - if not next_edge_node or "node" not in next_edge_node: + node_obj = self.route_search.get_node_from_tmap2(node_id) + if not node_obj or "node" not in node_obj or "pose" not in node_obj["node"]: continue - upper_edges = next_edge_node["node"].get("edges", []) - for edges_all in upper_edges: - node_id = edges_all.get("node", "") - if not isinstance(node_id, str) or not node_id: - continue - - # keep your original constraint - if (self.ACTIONS.GOAL_ALIGN_INDEX[0] not in node_id): - continue - if (target_row_edge_id in node_id): - continue - - # Exclude waypoints explicitly - if is_waypoint_name(node_id): - continue - - node_obj = self.route_search.get_node_from_tmap2(node_id) - if not node_obj or "node" not in node_obj or "pose" not in node_obj["node"]: - continue - - pose = node_obj["node"]["pose"] - xy = np.array([pose["position"]["x"], pose["position"]["y"]], dtype=float) - selected_row_edge_nodes[node_id] = (pose, xy) - - if len(selected_row_edge_nodes) == 0: - self.get_logger().error("[handle_row_operation] No candidate boundary nodes found. Clearing boundary.") - publish_empty_boundary(target_pose_frame_id) - action_msg.setSideEdges({}, target_pose_frame_id) - return self.ACTIONS.ROW_OPERATION, action_msg - - # --------------------------- - # Choose last goal INSIDE the row (avoid waypoint outside-row corrupting direction) - # Uses destination_node_str if available; otherwise falls back to nodes[-1] - # --------------------------- - selected_last_node = None - try: - # Prefer the last goal that is a ROW node (not a waypoint) - for g in nodes: - p = g["target_pose"]["pose"]["position"] - xy = (p["x"], p["y"]) - name = getattr(self, "destination_node_str", {}).get(xy) - if name and is_row_node_name(name): - selected_last_node = g - except Exception: - selected_last_node = None + pose = node_obj["node"]["pose"] + xy = np.array([pose["position"]["x"], pose["position"]["y"]], dtype=float) + candidates[node_id] = (pose, xy) - if selected_last_node is None: - selected_last_node = nodes[-1] + return candidates - # If last goal is exactly the center, use first goal as fallback - try: - cpos = cen["node"]["pose"]["position"] - lpos = selected_last_node["target_pose"]["pose"]["position"] - if np.linalg.norm(np.array([lpos["x"] - cpos["x"], lpos["y"] - cpos["y"]], dtype=float)) < 1e-6: - selected_last_node = nodes[0] - except Exception: - pass + def _select_boundary_nodes(self, candidates, center_xy, row_dir, tag_id): + """ + Select left and right boundary nodes relative to the row direction. + Returns: list of selected node_ids (up to 2). + """ + scored = [] # (side_sign, dist, node_id) - # --------------------------- - # Compute row direction (yaw) - # --------------------------- - cpos = cen["node"]["pose"]["position"] - lpos = selected_last_node["target_pose"]["pose"]["position"] - dx = lpos["x"] - cpos["x"] - dy = lpos["y"] - cpos["y"] + for node_id, (_pose, txy) in candidates.items(): + if tag_id not in node_id: + continue - if abs(dx) + abs(dy) > 1e-6: - row_yaw = math.atan2(dy, dx) - else: - row_yaw = yaw_from_quat(cen["node"]["pose"]["orientation"]) + v = txy - center_xy + dist = float(np.linalg.norm(v)) + if dist < 1e-6: + continue - row_dir = np.array([math.cos(row_yaw), math.sin(row_yaw)], dtype=float) - center_xy = np.array([cpos["x"], cpos["y"]], dtype=float) + # Reject nodes too aligned with row direction (want perpendicular/lateral nodes) + cosang = abs(float(np.dot(row_dir, v) / dist)) + if cosang > 0.6: + continue - # --------------------------- - # Pick closest LEFT and closest RIGHT boundary node (relative to row direction) - # Also enforce "lateral" boundary: candidate vector should be ~perpendicular to row_dir - # --------------------------- - candidates = [] # (side_sign, dist, node_id) + # Determine which side (left=positive, right=negative) + crossz = float(row_dir[0] * v[1] - row_dir[1] * v[0]) + side = 1.0 if crossz > 0.0 else -1.0 + scored.append((side, dist, node_id)) - for node_id, (_pose, txy) in selected_row_edge_nodes.items(): - # Must be for this row tag - if tag_id not in node_id: - continue + if not scored: + return [] - v = (txy - center_xy) - dist = float(np.linalg.norm(v)) - if dist < 1e-6: - continue + # Pick closest on each side + left = min((c for c in scored if c[0] > 0.0), default=None, key=lambda x: x[1]) + right = min((c for c in scored if c[0] < 0.0), default=None, key=lambda x: x[1]) + + picked = [] + if left: + picked.append(left[2]) + if right: + picked.append(right[2]) + + # Fallback: fill up to 2 from remaining candidates + if len(picked) < 2: + for _, _, nid in sorted(scored, key=lambda x: x[1]): + if nid not in picked: + picked.append(nid) + if len(picked) == 2: + break - # lateral filter: reject candidates that are too aligned with row_dir - cosang = abs(float(np.dot(row_dir, v) / dist)) - if cosang > 0.6: - continue + return picked - crossz = float(row_dir[0] * v[1] - row_dir[1] * v[0]) - side = 1.0 if crossz > 0.0 else -1.0 - candidates.append((side, dist, node_id)) - - if len(candidates) == 0: - self.get_logger().error("[handle_row_operation] Candidates exist but none passed lateral/tag filters. Clearing boundary.") - publish_empty_boundary(target_pose_frame_id) - action_msg.setSideEdges({}, target_pose_frame_id) - return self.ACTIONS.ROW_OPERATION, action_msg - - left = min((c for c in candidates if c[0] > 0.0), default=None, key=lambda x: x[1]) - right = min((c for c in candidates if c[0] < 0.0), default=None, key=lambda x: x[1]) - - picked = [] - if left: - picked.append(left[2]) - if right: - picked.append(right[2]) - - # Fallback: if only one side found, take the next closest (still filtered) - if len(picked) < 2: - for _, _, nid in sorted(candidates, key=lambda x: x[1]): - if nid not in picked: - picked.append(nid) - if len(picked) == 2: - break + def _select_last_row_goal(self, nodes): + """Select the last goal that is a ROW node (not a waypoint).""" + selected = None + for g in nodes: + try: + p = g["target_pose"]["pose"]["position"] + xy = (p["x"], p["y"]) + name = getattr(self, "destination_node_str", {}).get(xy) + if name and self._is_row_node_name(name): + selected = g + except Exception: + continue + return selected if selected else nodes[-1] - # --------------------------- - # Build selected_edges (front/back) and force yaw to row_yaw - # --------------------------- - self.selected_edges = {} + def _handle_row_operation(self, nodes, edge_id, action_msg): + """ + Build row boundaries and return (action, action_msg). + Simplified logic for selecting boundary nodes. + """ + # Validate inputs + if not nodes: + self.get_logger().error("[_handle_row_operation] nodes list is empty") + self._publish_empty_boundary("map") + action_msg.setSideEdges({}, "map") + return self.ACTIONS.ROW_OPERATION, action_msg - for child in picked: - child_node = self.route_search.get_node_from_tmap2(child) - if not child_node or "node" not in child_node: - continue + frame_id = nodes[0]["target_pose"]["header"].get("frame_id", "map") - side_intermediate_pose = self.get_intermediate_pose(cen, child_node, target_pose_frame_id) - side_last_intermediate_pose = self.get_last_intermediate_pose(side_intermediate_pose, cen, selected_last_node) + if not edge_id: + self.get_logger().error("[_handle_row_operation] edge_id is empty") + self._publish_empty_boundary(frame_id) + action_msg.setSideEdges({}, frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg - # Force boundary direction to be aligned with the row - set_pose_yaw(side_intermediate_pose, row_yaw) - set_pose_yaw(side_last_intermediate_pose, row_yaw) + # Get row center node + cen, tag_id, target_row_edge_id = self._get_row_center_node(edge_id) + if not cen: + self._publish_empty_boundary(frame_id) + action_msg.setSideEdges({}, frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg - self.selected_edges[child] = [side_intermediate_pose, side_last_intermediate_pose] + self.get_logger().info(f"[_handle_row_operation] center={target_row_edge_id}, tag={tag_id}") + + # get target_row_edge_id pose and orientation + target_row_edge_node = self.route_search.get_node_from_tmap2(target_row_edge_id) + if target_row_edge_node and "node" in target_row_edge_node: + target_row_edge_pose = target_row_edge_node["node"]["pose"] + self.get_logger().info(f"Target Row Edge Pose: {target_row_edge_pose}") + else: + self.get_logger().warn(f"[_handle_row_operation] Could not find node for target_row_edge_id: {target_row_edge_id}") + + try: + # print out cen info + # self.get_logger().info(f"Center Node Info: {cen}") + # get target edges from cen and publish as pose stamped message + node_info = cen.get("node", {}) + node_pose = node_info.get("pose", {}) + parent_frame = node_info.get("parent_frame", "map") + # get the node IDs from cen edges + selected_edges_dict = {"node": {"parent_frame": parent_frame, "pose": node_pose, "edges": []}} + # print node pose + self.get_logger().info(f"Center Node Pose: {node_pose}") + + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'map' + msg.pose.position.x = node_pose["position"]["x"] + msg.pose.position.y = node_pose["position"]["y"] + msg.pose.position.z = node_pose["position"]["z"] + msg.pose.orientation.x = node_pose["orientation"]["x"] + msg.pose.orientation.y = node_pose["orientation"]["y"] + msg.pose.orientation.z = node_pose["orientation"]["z"] + msg.pose.orientation.w = node_pose["orientation"]["w"] + self.center_node_pose_pub.publish(msg) + self.get_logger().info('Published latched Center Node Pose') + + except Exception as e: + self.get_logger().error(f"[_handle_row_operation] Failed to extract center node info: {e}") + + # Collect boundary candidates + candidates = self._collect_boundary_candidates(cen, target_row_edge_id) + if not candidates: + self.get_logger().error("[_handle_row_operation] No boundary candidates found") + self._publish_empty_boundary(frame_id) + action_msg.setSideEdges({}, frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg - # If only one wall found, mirror to create opposite wall (and align yaw) - if len(self.selected_edges) == 1: - self.selected_edges["side_wall"] = self.get_intermediate_poses_interpolated(self.selected_edges, cen, selected_last_node) - for ps in self.selected_edges["side_wall"]: - set_pose_yaw(ps, row_yaw) + # Select last row goal and compute row direction + selected_last_node = self._select_last_row_goal(nodes) + cpos = cen["node"]["pose"]["position"] + lpos = selected_last_node["target_pose"]["pose"]["position"] - if len(self.selected_edges) == 0: - self.get_logger().error("[handle_row_operation] selected_edges ended empty. Clearing boundary.") - publish_empty_boundary(target_pose_frame_id) - action_msg.setSideEdges({}, target_pose_frame_id) - return self.ACTIONS.ROW_OPERATION, action_msg + # Avoid using center as last goal + if np.linalg.norm(np.array([lpos["x"] - cpos["x"], lpos["y"] - cpos["y"]])) < 1e-6: + selected_last_node = nodes[0] + lpos = selected_last_node["target_pose"]["pose"]["position"] - # --------------------------- - # Publish /boundary_checker once (latched) + attach to action_msg - # --------------------------- - action_msg.setSideEdges(self.selected_edges, target_pose_frame_id) + dx, dy = lpos["x"] - cpos["x"], lpos["y"] - cpos["y"] + row_yaw = math.atan2(dy, dx) if abs(dx) + abs(dy) > 1e-6 else self._yaw_from_quat(cen["node"]["pose"]["orientation"]) + row_dir = np.array([math.cos(row_yaw), math.sin(row_yaw)], dtype=float) + center_xy = np.array([cpos["x"], cpos["y"]], dtype=float) + + # Select boundary nodes + picked = self._select_boundary_nodes(candidates, center_xy, row_dir, tag_id) + if not picked: + self.get_logger().error("[_handle_row_operation] No valid boundary nodes selected") + self._publish_empty_boundary(frame_id) + action_msg.setSideEdges({}, frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg - if self.is_row_boundary_published is False: - boundary_info = action_msg.getBoundary() - boundary_info.header.stamp = self.get_clock().now().to_msg() - self.boundary_publisher.publish(boundary_info) - self.is_row_boundary_published = True + # Build selected_edges with poses aligned to row direction + self.selected_edges = {} + for child in picked: + child_node = self.route_search.get_node_from_tmap2(child) + if not child_node or "node" not in child_node: + continue + side_pose = self.get_intermediate_pose(cen, child_node, frame_id) + side_last_pose = self.get_last_intermediate_pose(side_pose, cen, selected_last_node) + + # self.get_logger().info(f"Side Pose: {side_pose}") + self.get_logger().info(f"Side Last Pose: {side_last_pose}") + self.get_logger().info(f"Row Yaw: {row_yaw}") + self.get_logger().info(f"Child Node ID: {child}") + # how to print the node orientation?? + self.get_logger().info(f"Child Node Orientation: {child_node['node']['pose']['orientation']}") + # print node orientation as yaw + child_yaw = self._yaw_from_quat(child_node["node"]["pose"]["orientation"]) + self.get_logger().info(f"Child Node Yaw: {child_yaw}") + + self._set_pose_yaw(side_pose, child_yaw) + self._set_pose_yaw(side_last_pose, child_yaw) + self.selected_edges[child] = [side_pose, side_last_pose] + + # Mirror if only one wall found + if len(self.selected_edges) == 1: + self.get_logger().info("[_handle_row_operation] Single wall found, creating mirror") + self.selected_edges["side_wall"] = self.get_intermediate_poses_interpolated( + self.selected_edges, cen, selected_last_node + ) + for ps in self.selected_edges["side_wall"]: + self._set_pose_yaw(ps, child_yaw) + + if not self.selected_edges: + self.get_logger().error("[_handle_row_operation] No edges built") + self._publish_empty_boundary(frame_id) + action_msg.setSideEdges({}, frame_id) return self.ACTIONS.ROW_OPERATION, action_msg + # Publish boundary + action_msg.setSideEdges(self.selected_edges, frame_id) + if not self.is_row_boundary_published: + boundary_info = action_msg.getBoundary() + boundary_info.header.stamp = self.get_clock().now().to_msg() + self.boundary_publisher.publish(boundary_info) + self.is_row_boundary_published = True + + return self.ACTIONS.ROW_OPERATION, action_msg + + def get_navigate_through_poses_goal(self, poses, actions, edge_ids, is_execpolicy=False): + control_server_configs = {} @@ -947,9 +927,9 @@ def publish_empty_boundary(frame_id: str): nav_goal.behavior_tree = self.bt_trees[action] edge_action_is_valid = True - if(action == self.ACTIONS.ROW_TRAVERSAL and self.in_row_operation == True and (len(nodes) > 0)): - self.get_logger().warn(f"Segment {seg_i} action == self.ACTIONS.ROW_TRAVERSAL and self.in_row_operation == True and (len(nodes) > 0)") - action, action_msg = handle_row_operation() + if action == self.ACTIONS.ROW_TRAVERSAL and self.in_row_operation and nodes: + self.get_logger().warn(f"Segment {seg_i}: ROW_TRAVERSAL with in_row_operation") + action, action_msg = self._handle_row_operation(nodes, edge_id, action_msg) self.get_logger().info(" Action {} Bt_tree : {}".format(action, nav_goal.behavior_tree)) if edge_action_is_valid: @@ -980,12 +960,13 @@ def publish_empty_boundary(frame_id: str): if action in self.ACTIONS.bt_tree_with_control_server_config: controller_plugin = self.ACTIONS.bt_tree_with_control_server_config[action] control_server_configs[action] = self.ACTIONS.planner_with_goal_checker_config[controller_plugin] - if(action in self.bt_trees): + if action in self.bt_trees: nav_goal.behavior_tree = self.bt_trees[action] edge_action_is_valid = True - if(action == self.ACTIONS.ROW_TRAVERSAL and self.in_row_operation == True and (len(nodes) > 0)): - action, action_msg = handle_row_operation() + if action == self.ACTIONS.ROW_TRAVERSAL and self.in_row_operation and nodes: + self.get_logger().warn(f"Segment {seg_i}: ROW_TRAVERSAL with in_row_operation") + action, action_msg = self._handle_row_operation(nodes, edge_id, action_msg) self.get_logger().info(" Action {} Bt_tree : {}".format(action, nav_goal.behavior_tree)) if edge_action_is_valid: From 4c4f48abc3d106f3eef8beb51b755b420ff50d91 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Tue, 20 Jan 2026 23:04:59 +0000 Subject: [PATCH 19/23] Initial plan From 10ecb07805b33c4de67e4d67cfee17bcc2911f6c Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Tue, 20 Jan 2026 23:11:27 +0000 Subject: [PATCH 20/23] Add Copilot instructions, AGENTS.md, and REVIEW.md for AI coding agents Co-authored-by: marc-hanheide <1153084+marc-hanheide@users.noreply.github.com> --- .github/copilot-instructions.md | 174 +++++++++++ AGENTS.md | 495 +++++++++++++++++++++++++++++++ REVIEW.md | 504 ++++++++++++++++++++++++++++++++ 3 files changed, 1173 insertions(+) create mode 100644 .github/copilot-instructions.md create mode 100644 AGENTS.md create mode 100644 REVIEW.md diff --git a/.github/copilot-instructions.md b/.github/copilot-instructions.md new file mode 100644 index 00000000..48ed6e5f --- /dev/null +++ b/.github/copilot-instructions.md @@ -0,0 +1,174 @@ +# GitHub Copilot Instructions for topological_navigation + +This repository contains a ROS 2 topological navigation framework for autonomous mobile robots. + +## Repository Overview + +This is a multi-package ROS 2 workspace with the following packages: + +- **topological_navigation**: Core navigation and mapping functionality (Python) +- **topological_navigation_msgs**: Message, service, and action definitions +- **topological_rviz_tools**: RViz-based interactive map construction tools (C++) +- **topological_utils**: Command-line utilities for map creation and management (Python) + +## Code Style and Conventions + +### Python Code +- **Style Guide**: Follow PEP 8 and PEP 257 for Python code +- **Linters**: Use `ament_flake8` and `ament_pep257` for validation +- **Testing**: Use `pytest` and `launch_pytest` for tests +- **ROS 2 Patterns**: Follow standard ROS 2 node patterns with lifecycle management +- **Type Hints**: Use type hints where appropriate for better code clarity + +### C++ Code +- **Style Guide**: Follow ROS 2 C++ style guidelines +- **Build System**: Uses `ament_cmake` for C++ packages +- **RViz Plugins**: Follow RViz plugin development patterns for tool and panel creation + +### File Organization +- Scripts go in `{package}/scripts/` directory +- Python modules in `{package}/{package}/` directory +- Launch files in `{package}/launch/` directory +- Configuration files (YAML schemas, templates) in `{package}/config/` directory +- Tests in `{package}/test/` or `{package}/tests/` directories + +## Key Concepts + +### Topological Maps +- Maps are stored as YAML files (`.tmap2.yaml` extension) +- Schema validation is available via `validate_map.py` +- Maps consist of **nodes** (waypoints) and **edges** (connections) +- Each node has a pose, properties, and edges to other nodes +- Each edge has an action (e.g., NavigateToPose), action type, and optional properties + +### Flexible Properties System +- Both nodes and edges support a flexible `properties` dictionary +- Properties can be namespaced for organization (e.g., `restrictions`, `semantics`, `navigation`) +- Properties are optional and domain-specific +- See `topological_navigation/doc/PROPERTIES.md` for detailed documentation + +### Core Components +- **Map Manager**: Manages topological map data and publishes map information +- **Localisation**: Determines robot's current topological node +- **Navigation**: Plans and executes routes through the topological map +- **Route Search**: A* based path planning on topological graphs +- **Edge Actions**: Pluggable actions for traversing edges (e.g., NavigateToPose) + +## Building and Testing + +### Build the Workspace +```bash +colcon build +``` + +### Run Linters +```bash +# Python linting +colcon test --packages-select topological_navigation --event-handlers console_direct+ --pytest-args -k "test_flake8 or test_pep257" + +# Run all tests for a package +colcon test --packages-select topological_navigation +``` + +### Run Specific Tests +```bash +# Run pytest tests directly +cd topological_navigation +pytest test/test_navigationcore.py -v +``` + +### Validate Topological Maps +```bash +ros2 run topological_navigation validate_map.py path/to/map.tmap2.yaml -v +``` + +## Common Tasks + +### Adding New Node Properties +1. Update documentation in `topological_navigation/doc/PROPERTIES.md` with new property descriptions +2. Access properties safely in code using `.get()` with defaults +3. Consider using namespaces to organize properties by domain or package + +### Creating New Scripts +1. Add Python script to `{package}/scripts/` +2. Update `entry_points` in `setup.py` to expose as ROS 2 executable +3. Follow ROS 2 node patterns: use `rclpy.init()`, create node class, spin +4. Add appropriate command-line argument parsing + +### Modifying Map Schema +1. Update schema in `topological_navigation/config/tmap-schema.yaml` +2. Update validation logic if needed +3. Update templates in `config/template_node_2.yaml` and `config/template_edge.yaml` +4. Update documentation in `doc/PROPERTIES.md` + +### Working with RViz Tools (C++) +1. RViz tools inherit from `rviz_common::Tool` +2. RViz panels inherit from `rviz_common::Panel` +3. Use Qt for UI components in panels +4. Register plugins in `plugin_description.xml` + +## Important Files + +### Configuration +- `config/tmap-schema.yaml`: JSON schema for topological map validation +- `config/template_node_2.yaml`: Template for creating new nodes +- `config/template_edge.yaml`: Template for creating new edges + +### Documentation +- `README.md`: Package-level documentation +- `doc/PROPERTIES.md`: Comprehensive guide to the properties system +- `CHANGELOG.rst`: Version history and changes + +### Testing +- `test/`: Unit tests and integration tests +- Uses `pytest` for Python tests +- Uses `launch_pytest` for ROS 2 launch-based tests + +## CI/CD + +- GitHub Actions workflows in `.github/workflows/` +- `ci.yaml`: Builds and tests on ROS 2 Humble and Iron distributions +- `validate-schema.yaml`: Validates topological map schema +- All PRs should pass CI checks before merging + +## Dependencies + +### ROS 2 Dependencies +- `nav2_msgs`: Navigation 2 message definitions +- `geometry_msgs`, `std_msgs`, `nav_msgs`, `sensor_msgs`: Standard ROS 2 messages +- `tf_transformations`: Coordinate transformations +- `visualization_msgs`: RViz markers and visualization + +### Python Dependencies +- Standard library modules are preferred +- Minimal external dependencies to maintain portability + +## Best Practices + +### When Adding Features +1. Maintain backward compatibility with existing topological maps +2. Make properties optional with sensible defaults +3. Document new properties or features in relevant documentation files +4. Add validation logic to catch misconfigurations early +5. Add tests for new functionality + +### When Modifying Core Logic +1. Understand the impact on navigation behavior +2. Test with sample topological maps +3. Consider edge cases (empty maps, disconnected graphs, invalid actions) +4. Update documentation if behavior changes + +### When Working with Maps +1. Always validate maps with `validate_map.py` before deployment +2. Use namespaced properties to avoid conflicts between applications +3. Access properties defensively with `.get()` and default values +4. Document custom properties in your application-specific docs + +## Tips for AI Assistants + +- This is a ROS 2 package (not ROS 1) - use ROS 2 APIs and patterns +- Topological navigation differs from metric navigation: it operates on graphs of nodes, not continuous space +- Properties are flexible but optional - don't assume they exist +- Many scripts are command-line tools - respect existing argument parsing patterns +- Tests should be added for new functionality, following existing test patterns +- The repository supports multiple ROS 2 distributions (Humble, Iron) - use compatible APIs diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 00000000..e1f14d4e --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,495 @@ +# AI Coding Agent Guide for topological_navigation + +This document provides comprehensive information about the topological_navigation repository to help AI coding agents understand and work effectively with the codebase. + +## Project Overview + +**topological_navigation** is a ROS 2 framework for topological navigation of autonomous mobile robots. Unlike traditional metric navigation that operates in continuous coordinate space, topological navigation represents the environment as a graph of discrete nodes (waypoints) connected by edges (paths). + +### Purpose +This system enables robots to: +- Navigate between named locations using high-level graph-based planning +- Represent complex environments efficiently as topological maps +- Execute domain-specific actions when traversing edges (e.g., opening doors, climbing ramps) +- Leverage flexible metadata for application-specific customization + +### Originally Developed For +STRANDS (Spatio-Temporal Representations and Activities for Cognitive Control in Long-term Scenarios) - a long-term autonomy project for mobile robots. + +## Repository Structure + +This is a multi-package ROS 2 workspace with four packages: + +``` +topological_navigation/ +├── topological_navigation/ # Core package (Python) +│ ├── topological_navigation/ # Python modules +│ │ ├── scripts/ # ROS 2 executable scripts +│ │ ├── *.py # Core classes (route_search, map_marker, etc.) +│ ├── config/ # YAML schemas and templates +│ ├── doc/ # Documentation (especially PROPERTIES.md) +│ ├── launch/ # ROS 2 launch files +│ └── test/, tests/ # Unit and integration tests +│ +├── topological_navigation_msgs/ # Message definitions +│ ├── msg/, srv/, action/ # ROS 2 interface definitions +│ +├── topological_rviz_tools/ # Interactive map editing (C++/Qt) +│ ├── src/ # RViz plugins (tools and panels) +│ ├── include/ # C++ headers +│ +└── topological_utils/ # Utilities (Python) + └── topological_utils/ # Map creation and management tools +``` + +## Technology Stack + +- **Language**: Python 3 (core), C++ (RViz tools) +- **Framework**: ROS 2 (Humble, Iron distributions) +- **Build System**: ament_python, ament_cmake +- **Testing**: pytest, launch_pytest, ament linters +- **GUI**: Qt (for RViz panels) +- **Data Format**: YAML for topological maps +- **Validation**: JSON Schema for map structure + +## Core Concepts + +### 1. Topological Maps + +Topological maps are the fundamental data structure in this system. + +**Structure**: +- **Nodes**: Represent discrete locations (waypoints) with: + - Unique name + - Pose (position + orientation) in a coordinate frame + - Influence zone (polygon defining the node's spatial extent) + - Flexible properties dictionary + - List of outgoing edges + +- **Edges**: Represent navigable connections with: + - Target node reference + - Action name (e.g., "NavigateToPose", "OpenDoor") + - Action type (ROS 2 message/action type) + - Flexible properties dictionary + +**File Format**: YAML with `.tmap2.yaml` extension + +**Example**: +```yaml +nodes: + - meta: + map: my_environment + node: ChargingStation + pointset: my_environment + node: + name: ChargingStation + parent_frame: map + pose: + position: {x: 10.0, y: 5.0, z: 0.0} + orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0} + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + semantics: "charging_station" + edges: + - edge_id: ChargingStation_WayPoint1 + node: WayPoint1 + action: NavigateToPose + action_type: geometry_msgs/PoseStamped + properties: + max_speed: 0.5 +``` + +### 2. Flexible Properties System + +Both nodes and edges support an optional `properties` dictionary for application-specific metadata. + +**Key Features**: +- Completely flexible schema (any key-value pairs) +- Optional - properties can be omitted entirely +- Supports namespacing for organization +- Enables domain-specific customization without schema changes + +**Common Property Patterns**: +- **Flat structure**: `{capacity: 2, zone: "A"}` +- **Domain namespaces**: `{restrictions: {capacity: 2}, semantics: {zone: "A"}}` +- **Package namespaces**: `{my_fleet_manager: {priority_zone: true}}` + +**Important**: Always access properties defensively: +```python +props = node["node"].get("properties", {}) +capacity = props.get("capacity", 1) # Default to 1 +``` + +See `topological_navigation/doc/PROPERTIES.md` for comprehensive documentation. + +### 3. Navigation Architecture + +**Key Components**: + +1. **Map Manager** (`map_manager2.py`) + - Loads topological maps from YAML files + - Publishes map structure to ROS 2 topics + - Handles map updates and modifications + +2. **Localisation** (`localisation2.py`) + - Determines robot's current topological node + - Publishes `/current_node` and `/closest_node` + - Supports pose-based and topic-based localization + +3. **Navigation** (`navigation2.py`) + - Executes topological navigation actions + - Coordinates with metric navigation (Nav2) + - Handles edge action execution + +4. **Route Search** (`route_search2.py`) + - A* based path planning on topological graph + - Finds optimal routes between nodes + - Considers edge properties for path optimization + +5. **Edge Action Manager** (`edge_action_manager.py`) + - Manages execution of edge-specific actions + - Pluggable action system for custom behaviors + +## Development Guidelines + +### Code Style + +**Python**: +- Follow PEP 8 style guide +- Use PEP 257 docstring conventions +- Validated with `ament_flake8` and `ament_pep257` +- Type hints encouraged for clarity + +**C++**: +- Follow ROS 2 C++ style guidelines +- Use modern C++ features (C++14 minimum) + +### Testing + +**Test Structure**: +- `test/` or `tests/` directories in each package +- Python: pytest framework +- ROS 2 integration: launch_pytest +- Run: `colcon test --packages-select ` + +**Important**: Maintain existing test infrastructure. Add tests for new features following established patterns. + +### Building + +```bash +# Build all packages +colcon build + +# Build specific package +colcon build --packages-select topological_navigation + +# Run tests +colcon test --packages-select topological_navigation +``` + +### Linting + +```bash +# Python linting (flake8, pep257) +colcon test --packages-select topological_navigation \ + --event-handlers console_direct+ \ + --pytest-args -k "test_flake8 or test_pep257" +``` + +## Common Tasks for AI Agents + +### Task: Add New Property to Topological Maps + +1. **Document the property** in `topological_navigation/doc/PROPERTIES.md` + - Add to appropriate table (node or edge properties) + - Specify type, description, and examples + - Include code examples for accessing the property + +2. **Update code to use the property**: + ```python + # Access safely with default + props = node["node"].get("properties", {}) + new_property = props.get("new_property", default_value) + ``` + +3. **Consider namespacing** if the property is application-specific + +4. **No schema changes needed** - properties are flexible by design + +### Task: Add New ROS 2 Script + +1. **Create script** in `{package}/scripts/my_script.py` + ```python + #!/usr/bin/env python3 + import rclpy + from rclpy.node import Node + + class MyNode(Node): + def __init__(self): + super().__init__('my_node') + # Initialization + + def main(args=None): + rclpy.init(args=args) + node = MyNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + if __name__ == '__main__': + main() + ``` + +2. **Register in setup.py**: + ```python + entry_points={ + 'console_scripts': [ + 'my_script = package_name.scripts.my_script:main', + ], + } + ``` + +3. **Make executable**: `chmod +x {package}/scripts/my_script.py` + +4. **Test**: `ros2 run package_name my_script` + +### Task: Modify Map Schema + +1. **Update schema** in `topological_navigation/config/tmap-schema.yaml` + +2. **Update templates**: + - `config/template_node_2.yaml` + - `config/template_edge.yaml` + +3. **Update validation logic** if needed in `scripts/validate_map.py` + +4. **Document changes** in `doc/PROPERTIES.md` or README + +5. **Test with validator**: `ros2 run topological_navigation validate_map.py map.tmap2.yaml -v` + +### Task: Fix Failing Tests + +1. **Identify failing test**: Check CI logs or run `colcon test` + +2. **Run test locally**: + ```bash + cd {package} + pytest test/test_name.py -v + ``` + +3. **Fix code or test** as appropriate + +4. **Verify fix**: Re-run tests + +5. **Run linters** to ensure code quality + +### Task: Add C++ RViz Plugin + +1. **Create class** inheriting from `rviz_common::Tool` or `rviz_common::Panel` + +2. **Implement required methods**: + - `onInitialize()` for setup + - Event handlers for interaction + +3. **Register plugin** in `plugin_description.xml` + +4. **Update CMakeLists.txt** to compile new source files + +5. **Test in RViz**: Load custom plugin from RViz GUI + +## Important Patterns + +### Pattern: Safe Property Access + +Always access properties defensively to handle missing properties: + +```python +# Node properties +node_props = node["node"].get("properties", {}) +tolerance = node_props.get("xy_goal_tolerance", 0.3) + +# Namespaced properties +restrictions = node_props.get("restrictions", {}) +capacity = restrictions.get("capacity", 1) + +# Check existence before use +semantics = node_props.get("semantics") +if semantics is not None: + # Use semantics + pass +``` + +### Pattern: ROS 2 Node Creation + +Follow standard ROS 2 patterns: + +```python +import rclpy +from rclpy.node import Node + +class MyNode(Node): + def __init__(self): + super().__init__('my_node_name') + self.get_logger().info('Node initialized') + + # Declare parameters + self.declare_parameter('param_name', 'default_value') + + # Create publishers/subscribers + self.publisher = self.create_publisher(MsgType, 'topic', 10) + self.subscription = self.create_subscription( + MsgType, 'topic', self.callback, 10) + + def callback(self, msg): + self.get_logger().info(f'Received: {msg}') +``` + +### Pattern: Map Validation + +Always validate maps before deployment: + +```bash +ros2 run topological_navigation validate_map.py my_map.tmap2.yaml -v +``` + +Validation checks: +- Schema compliance +- Duplicate node names +- Edges pointing to non-existent nodes +- Required fields present + +## Key Files Reference + +### Configuration +- `topological_navigation/config/tmap-schema.yaml` - JSON schema for map validation +- `topological_navigation/config/template_node_2.yaml` - Node template +- `topological_navigation/config/template_edge.yaml` - Edge template + +### Documentation +- `README.md` - Repository and package documentation +- `topological_navigation/doc/PROPERTIES.md` - **Essential** properties system guide +- `CHANGELOG.rst` - Version history + +### Core Python Modules +- `route_search2.py` - Path planning algorithm +- `map_marker.py` - Visualization markers +- `edge_action_manager.py` - Edge action execution +- `navigation_stats.py` - Navigation statistics tracking + +### Core Scripts +- `localisation2.py` - Topological localization +- `navigation2.py` - Navigation execution +- `map_manager2.py` - Map data management +- `validate_map.py` - Map validation tool +- `visualise_map2.py` - Map visualization + +### Testing +- `topological_navigation/test/test_navigationcore.py` - Core navigation tests + +## CI/CD + +**GitHub Actions** (`.github/workflows/`): +- `ci.yaml` - Builds and tests on multiple ROS 2 distributions (Humble, Iron) +- `validate-schema.yaml` - Validates topological map schema + +**Requirements**: +- All tests must pass +- Code must pass linters (flake8, pep257) +- Compatible with ROS 2 Humble and Iron + +## Dependencies + +**ROS 2 Packages**: +- `nav2_msgs` - Navigation 2 messages +- `geometry_msgs`, `std_msgs`, `nav_msgs`, `sensor_msgs` - Core ROS 2 messages +- `visualization_msgs` - RViz visualization +- `tf_transformations` - Coordinate transformations + +**Build Dependencies**: +- `ament_cmake` - C++ build system +- `ament_python` - Python build system + +**Test Dependencies**: +- `pytest` - Python testing +- `launch_pytest` - ROS 2 launch testing +- `ament_flake8`, `ament_pep257` - Code quality + +## Best Practices for AI Agents + +### When Modifying Code + +1. **Understand the context**: Read related documentation (especially PROPERTIES.md) +2. **Maintain compatibility**: Don't break existing topological maps +3. **Make properties optional**: Provide sensible defaults +4. **Add tests**: Follow existing test patterns +5. **Run linters**: Ensure code quality before committing +6. **Update documentation**: Reflect changes in relevant docs + +### When Working with Properties + +1. **Access defensively**: Use `.get()` with defaults +2. **Consider namespaces**: Avoid property name conflicts +3. **Document custom properties**: In PROPERTIES.md or app-specific docs +4. **Validate maps**: Use `validate_map.py` after modifications + +### When Adding Features + +1. **Follow ROS 2 patterns**: Use standard node/topic/service patterns +2. **Minimize dependencies**: Prefer standard library and existing dependencies +3. **Consider multiple distributions**: Code should work on Humble and Iron +4. **Test thoroughly**: Unit tests and integration tests +5. **Update CI if needed**: Ensure new features are tested + +### When Fixing Bugs + +1. **Identify root cause**: Understand why the bug occurs +2. **Add regression test**: Prevent the bug from reoccurring +3. **Consider edge cases**: Empty maps, invalid data, missing properties +4. **Run full test suite**: Ensure fix doesn't break other functionality + +## Common Pitfalls to Avoid + +1. **Assuming properties exist**: Always use `.get()` with defaults +2. **Breaking backward compatibility**: Old maps should still work +3. **Hardcoding values**: Use parameters or properties instead +4. **Forgetting ROS 2 lifecycle**: Initialize rclpy, shutdown properly +5. **Ignoring validation**: Always validate maps before deployment +6. **Not updating docs**: Changes should be reflected in documentation +7. **Mixing ROS 1 and ROS 2 APIs**: This is a ROS 2 project only + +## Getting Help + +- **Documentation**: Start with README files and PROPERTIES.md +- **Code examples**: Look at existing scripts for patterns +- **Tests**: Show expected behavior and usage +- **Schema**: `tmap-schema.yaml` defines valid map structure +- **Templates**: Show minimal valid nodes and edges + +## Quick Reference + +### Build & Test +```bash +colcon build # Build all packages +colcon test --packages-select topological_navigation # Test package +``` + +### Validate Map +```bash +ros2 run topological_navigation validate_map.py map.tmap2.yaml -v +``` + +### Run Node +```bash +ros2 run topological_navigation script_name.py [args] +``` + +### Access Properties +```python +props = node["node"].get("properties", {}) +value = props.get("key", default) +``` + +--- + +**Remember**: This is a topological (graph-based) navigation system, not metric (coordinate-based). Nodes are discrete locations, edges are connections. Properties are flexible and optional. Always code defensively. diff --git a/REVIEW.md b/REVIEW.md new file mode 100644 index 00000000..037c3732 --- /dev/null +++ b/REVIEW.md @@ -0,0 +1,504 @@ +# Repository Structure Review and Improvement Recommendations + +**Date**: January 2026 +**Repository**: LCAS/topological_navigation +**Version**: 3.0.5 (ROS 2 Humble/Iron) + +This document provides an analysis of the current repository structure, identifies potential issues, and recommends improvements to guide future development and AI-assisted contributions. + +--- + +## Executive Summary + +The topological_navigation repository is a mature ROS 2 project with a well-established foundation. However, analysis reveals several areas for improvement: + +1. **Code Organization**: Presence of legacy "version 2" files alongside original versions creates confusion +2. **Documentation**: Inconsistent documentation coverage across packages +3. **Testing**: Limited test coverage, especially for newer ROS 2 features +4. **Code Duplication**: Some functionality exists in multiple versions (v1 vs v2) +5. **Technical Debt**: Several TODOs and FIXMEs in critical navigation code + +**Overall Assessment**: The repository is functional and well-maintained, but would benefit from modernization and consolidation efforts. + +--- + +## Structural Analysis + +### 1. Code Organization and Versioning + +**Issue: Multiple File Versions** + +The repository contains **15+ files with "2" suffix** (e.g., `localisation2.py`, `manager2.py`, `route_search2.py`), indicating parallel implementations: + +``` +topological_navigation/ +├── manager.py # Legacy version +├── manager2.py # Current version (1538 lines) +├── route_search.py # Legacy version +├── route_search2.py # Current version +├── localisation.py # Legacy version +└── scripts/ + └── localisation2.py # Current version +``` + +**Impact**: +- **Confusion for Contributors**: Unclear which files are authoritative +- **Maintenance Burden**: Bug fixes may need to be applied to multiple versions +- **Code Bloat**: Doubles the codebase size unnecessarily +- **AI Agent Confusion**: AI tools may reference or modify deprecated code + +**Recommendations**: + +1. **Short-term (High Priority)**: + - Add clear deprecation notices in legacy files: + ```python + """ + DEPRECATED: This file is maintained for backward compatibility only. + New development should use manager2.py instead. + Will be removed in version 4.0.0. + """ + ``` + - Update documentation to clarify which versions to use + +2. **Medium-term**: + - Create migration guide for users still on legacy versions + - Rename "2" files to remove suffix once legacy is removed: + - `manager2.py` → `manager.py` + - `route_search2.py` → `route_search.py` + - Consider semantic versioning for major API changes instead of file suffixes + +3. **Long-term (Version 4.0.0)**: + - Remove all legacy files after deprecation period + - Consolidate to single implementation per module + +### 2. Package Structure + +**Current Structure**: Well-organized multi-package workspace + +``` +topological_navigation/ +├── topological_navigation/ # Core (Python, ~8k LOC) +├── topological_navigation_msgs/ # Messages +├── topological_rviz_tools/ # RViz tools (C++/Qt) +└── topological_utils/ # Utilities (Python, ~70+ scripts) +``` + +**Strengths**: +- Clear separation of concerns +- Standard ROS 2 package layout +- Logical grouping of functionality + +**Issues**: + +1. **Script Proliferation**: `topological_utils` contains **70+ Python scripts**, making it difficult to discover and maintain tools + +2. **Inconsistent Naming**: Some scripts use underscores, others don't; some have `.py` extension in the name, others don't + +**Recommendations**: + +1. **Organize Utilities by Function**: + ``` + topological_utils/ + └── topological_utils/ + ├── commands/ # CLI tools + │ ├── map_management/ + │ │ ├── add_node.py + │ │ ├── remove_node.py + │ │ └── crop_map.py + │ ├── visualization/ + │ │ ├── plot_map.py + │ │ └── draw_predictions.py + │ └── migration/ + │ ├── migrate.py + │ └── map_converter.py + └── lib/ # Shared library code + ``` + +2. **Create Unified CLI Tool**: + - Consider a single entry point: `topo` command with subcommands + - Example: `topo map add-node` instead of `add_node.py` + - Reduces cognitive load and improves discoverability + - Similar to `git`, `docker`, or `ros2` command structure + +3. **Consolidate Duplicate Functionality**: + - Multiple map plotting tools exist (`plot_yaml.py`, `plot_yaml2.py`, `plot_topo_map.py`, `plot_topo_map2.py`) + - Consider single `plot` command with options for different formats + +### 3. Documentation + +**Current State**: + +| Package | README Length | Quality | +|---------|---------------|---------| +| Root | 14 lines | Minimal - just package list | +| topological_navigation | 278 lines | Good - comprehensive | +| topological_rviz_tools | 79 lines | Good - usage examples | +| topological_utils | None | **Missing** | +| topological_navigation_msgs | None | **Missing** | + +**Issues**: + +1. **Missing Package READMEs**: Two packages lack dedicated documentation +2. **Outdated Content**: Main README references ROS 1 concepts (MongoDB, `roslaunch`) +3. **No Architecture Diagrams**: Complex system lacks visual documentation +4. **API Documentation**: No generated API docs (Sphinx, Doxygen) + +**Recommendations**: + +1. **Immediate (High Priority)**: + - Add READMEs to `topological_utils` and `topological_navigation_msgs` + - Update root README with: + - Quick start guide for ROS 2 + - System requirements + - Installation instructions + - Link to key documentation + +2. **Short-term**: + - Create `docs/` directory with: + - Architecture overview with diagrams (use Mermaid or PlantUML) + - Component interaction diagrams + - Data flow diagrams + - API reference (auto-generated from code) + +3. **Medium-term**: + - Set up automated documentation generation: + - Python: Sphinx with autodoc + - C++: Doxygen + - Add documentation to CI/CD pipeline + - Host documentation (GitHub Pages, Read the Docs) + +4. **Enhanced Documentation Examples**: + ```markdown + docs/ + ├── index.md # Main documentation hub + ├── getting-started/ + │ ├── installation.md + │ ├── quick-start.md + │ └── tutorials/ + ├── architecture/ + │ ├── overview.md + │ ├── components.md + │ └── diagrams/ + ├── user-guide/ + │ ├── creating-maps.md + │ ├── navigation.md + │ └── properties-guide.md + ├── developer-guide/ + │ ├── contributing.md + │ ├── coding-standards.md + │ └── testing.md + └── api/ + ├── python/ + └── cpp/ + ``` + +### 4. Testing Infrastructure + +**Current State**: + +- **Python**: Uses pytest and launch_pytest ✓ +- **Linters**: ament_flake8, ament_pep257 ✓ +- **CI/CD**: GitHub Actions testing on Humble and Iron ✓ +- **Test Coverage**: Only 1-2 test files found, mostly integration tests + +**Issues**: + +1. **Low Unit Test Coverage**: Very few unit tests for core modules +2. **Legacy Test Documentation**: `tests/README.md` references ROS 1 tools (catkin_make, roslaunch, MongoDB, MORSE simulator) +3. **No Coverage Metrics**: No measurement of code coverage +4. **Manual Testing Required**: Some functionality requires manual verification + +**Recommendations**: + +1. **Immediate**: + - Update `tests/README.md` for ROS 2 (replace catkin_make with colcon, roslaunch with ros2 launch) + - Add unit tests for critical modules: + - `route_search2.py` - path planning algorithms + - `tmap_utils.py` - map utilities + - `restrictions_impl.py` - validation logic + +2. **Short-term**: + - Add code coverage reporting: + ```bash + pytest --cov=topological_navigation --cov-report=html + ``` + - Set coverage targets (aim for 70%+ for core modules) + - Add coverage badge to README + +3. **Medium-term**: + - Create comprehensive test suite: + - Unit tests for all public APIs + - Integration tests for navigation scenarios + - Property validation tests + - Add performance/benchmark tests for route planning + - Mock external dependencies (Nav2, TF) + +4. **Example Test Structure**: + ``` + topological_navigation/ + └── test/ + ├── unit/ + │ ├── test_route_search.py + │ ├── test_map_validation.py + │ └── test_properties.py + ├── integration/ + │ ├── test_navigation_flow.py + │ └── test_map_loading.py + └── fixtures/ + ├── sample_maps/ + └── mock_data/ + ``` + +### 5. Code Quality Issues + +**Technical Debt Identified**: + +1. **TODOs and FIXMEs**: 8+ instances in production code + ```python + # localisation2.py + # TODO: remove Temporary arg until tags functionality is MongoDB independent + + # navigation2.py + # FIXME: not implemented + + # edge_action_manager2.py + #TODO change this to actual + ``` + +2. **Wildcard Imports**: Found in 10+ utility scripts + ```python + from module import * # Anti-pattern + ``` + +3. **Large Files**: Some modules exceed 1000 lines + - `edge_action_manager2.py`: 1362 lines + - `manager2.py`: 1538 lines + +**Recommendations**: + +1. **Immediate**: + - Create GitHub issues for each TODO/FIXME + - Add issue references in code: + ```python + # TODO(#123): remove Temporary arg until tags functionality is MongoDB independent + ``` + +2. **Short-term**: + - Refactor wildcard imports to explicit imports + - Add pre-commit hooks to prevent new wildcard imports + - Configure linters to flag these issues + +3. **Medium-term**: + - Refactor large files into smaller, focused modules: + ```python + # Instead of edge_action_manager2.py (1362 lines) + edge_action_manager/ + ├── __init__.py + ├── base.py # Base classes + ├── actions.py # Action implementations + ├── state.py # State management + └── utils.py # Helper functions + ``` + - Apply SOLID principles for better maintainability + +### 6. Properties System (Positive Example) + +**Strengths**: + +- Excellent documentation in `PROPERTIES.md` ✓ +- Flexible, extensible design ✓ +- Backward compatible ✓ +- Well-thought-out namespacing approach ✓ + +**Minor Recommendations**: + +1. Add JSON Schema for property validation (optional but helpful) +2. Create property builder/validator classes for common patterns +3. Add examples directory with real-world property configurations + +### 7. Dependency Management + +**Current State**: +- Uses standard ROS 2 dependencies ✓ +- Minimal external dependencies ✓ +- Clear dependency declarations in `package.xml` ✓ + +**Potential Issues**: + +1. **No Dependency Pinning**: Not using specific versions +2. **No Security Scanning**: No automated vulnerability checks + +**Recommendations**: + +1. Add Dependabot or Renovate for dependency updates +2. Add security scanning to CI/CD (e.g., `safety` for Python) +3. Document tested dependency versions + +### 8. Potential Performance Concerns + +**Areas to Monitor**: + +1. **Large Map Handling**: Route planning on graphs with hundreds of nodes +2. **Visualization Performance**: Marker updates with many nodes/edges +3. **Memory Usage**: Loading multiple large maps simultaneously + +**Recommendations**: + +1. Add performance benchmarks for route planning +2. Implement map caching strategies +3. Consider lazy loading for large maps +4. Profile memory usage in long-running scenarios + +--- + +## Priority Recommendations Summary + +### High Priority (Address Soon) + +1. ✅ **Add deprecation notices** to legacy "version 1" files +2. ✅ **Create READMEs** for topological_utils and topological_navigation_msgs +3. ✅ **Update test documentation** for ROS 2 +4. ✅ **Convert TODOs to GitHub issues** with tracking +5. ✅ **Add basic unit tests** for core modules + +### Medium Priority (Next 6 Months) + +1. 📋 **Plan legacy code removal** for version 4.0.0 +2. 📋 **Refactor large files** (>1000 lines) into smaller modules +3. 📋 **Organize utility scripts** into logical subdirectories +4. 📋 **Generate API documentation** (Sphinx/Doxygen) +5. 📋 **Add code coverage** reporting and targets + +### Low Priority (Future Enhancements) + +1. 💡 **Create unified CLI tool** (`topo` command) +2. 💡 **Add architecture diagrams** to documentation +3. 💡 **Implement performance benchmarks** +4. 💡 **Add dependency security scanning** +5. 💡 **Create migration guides** between versions + +--- + +## Code Modernization Opportunities + +### Python Code + +1. **Type Hints**: Add comprehensive type hints to all public APIs + ```python + def find_route(start: str, end: str, map_name: str) -> List[str]: + """Find route between nodes.""" + ... + ``` + +2. **Dataclasses**: Use dataclasses for data structures + ```python + from dataclasses import dataclass + + @dataclass + class Node: + name: str + pose: Pose + properties: Dict[str, Any] + edges: List[Edge] + ``` + +3. **Async/Await**: Consider async patterns for I/O-bound operations + +### C++ Code + +1. **Modern C++**: Ensure consistent use of C++14/17 features +2. **Smart Pointers**: Verify consistent use of shared_ptr/unique_ptr +3. **RAII**: Ensure proper resource management + +--- + +## AI Agent Friendliness Assessment + +**Current Score: 7/10** + +**Strengths**: +- ✅ Clear package structure +- ✅ Excellent properties documentation +- ✅ Standard ROS 2 patterns +- ✅ Consistent Python style (PEP 8) + +**Areas for Improvement**: +- ⚠️ Legacy code confusion (version 1 vs 2) +- ⚠️ Limited inline documentation +- ⚠️ Missing API documentation +- ⚠️ Few code examples for common tasks + +**To Improve AI Agent Experience**: + +1. **Add more docstrings** with examples: + ```python + def add_edge(node_from: str, node_to: str, action: str) -> None: + """Add edge between two nodes. + + Args: + node_from: Source node name + node_to: Target node name + action: Action to execute (e.g., "NavigateToPose") + + Example: + >>> add_edge("Start", "Goal", "NavigateToPose") + + Raises: + ValueError: If either node doesn't exist + """ + ``` + +2. **Create examples directory**: + ``` + examples/ + ├── basic_navigation.py + ├── custom_properties.py + ├── map_creation.py + └── edge_actions.py + ``` + +3. **Add inline comments for complex algorithms** (e.g., A* implementation) + +4. **Provide migration examples** between versions + +--- + +## Conclusion + +The topological_navigation repository is a well-maintained, functional ROS 2 project with a solid foundation. The primary recommendations focus on: + +1. **Reducing confusion** by deprecating and eventually removing legacy code +2. **Improving discoverability** through better documentation and organization +3. **Increasing confidence** through comprehensive testing +4. **Modernizing code** to leverage current best practices + +These improvements will benefit both human developers and AI coding agents, making the repository easier to understand, maintain, and extend. + +**Estimated Effort**: +- High Priority Items: ~2-3 weeks of focused work +- Medium Priority Items: ~1-2 months spread over multiple sprints +- Low Priority Items: Ongoing improvements over 6-12 months + +**Expected Benefits**: +- Reduced onboarding time for new contributors +- Fewer bugs from using deprecated code +- Better AI agent assistance +- Improved maintainability and extensibility +- Higher confidence in code quality + +--- + +## Appendix: Metrics Summary + +| Metric | Value | Target | +|--------|-------|--------| +| Total Python Files | 144 | - | +| Files with "2" suffix | 15 | 0 (eventually) | +| Utility Scripts | ~70 | Consolidated | +| Test Files | ~2 | 20+ | +| Code Coverage | Unknown | 70%+ | +| TODOs/FIXMEs | 8+ | 0 (converted to issues) | +| Packages with READMEs | 2/4 | 4/4 | +| API Documentation | No | Yes | +| CI/CD Pipelines | 2 | 3+ (add coverage) | + From 4da1944e21a126193cef1a021ad091dec8a67b1e Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Tue, 27 Jan 2026 21:30:20 +0000 Subject: [PATCH 21/23] Update AGENTS.md with comprehensive codebase analysis for agent_prep branch - Added detailed module descriptions and file sizes - Enhanced navigation architecture documentation - Added agricultural navigation specifics (row operations, boundary detection) - Included common patterns and pitfalls - Added debugging and troubleshooting sections - Documented ROS 2 integration patterns - Added CI/CD and quality assurance guidelines - Included agent-specific guidelines for analyzing and modifying codebase --- AGENTS.md | 799 ++++++++++++++++++++++++++++++++++++++---------------- 1 file changed, 558 insertions(+), 241 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index e1f14d4e..3e001aca 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -10,11 +10,20 @@ This document provides comprehensive information about the topological_navigatio This system enables robots to: - Navigate between named locations using high-level graph-based planning - Represent complex environments efficiently as topological maps -- Execute domain-specific actions when traversing edges (e.g., opening doors, climbing ramps) +- Execute domain-specific actions when traversing edges (e.g., opening doors, climbing ramps, row operations in agriculture) - Leverage flexible metadata for application-specific customization +- Support agricultural robot operations including in-row navigation and boundary detection ### Originally Developed For -STRANDS (Spatio-Temporal Representations and Activities for Cognitive Control in Long-term Scenarios) - a long-term autonomy project for mobile robots. +STRANDS (Spatio-Temporal Representations and Activities for Cognitive Control in Long-term Scenarios) - a long-term autonomy project for mobile robots. Currently actively used in agricultural robotics applications, particularly for autonomous navigation in vineyards, orchards, and crop rows. + +### Recent Development Focus (aoc branch) +The `aoc` branch focuses on: +- Enhanced edge action management for agricultural operations +- Row operation handling with boundary node selection +- Improved closest node detection algorithms +- AI agent preparation with comprehensive documentation +- Flexible node/edge property system for domain-specific metadata ## Repository Structure @@ -23,20 +32,35 @@ This is a multi-package ROS 2 workspace with four packages: ``` topological_navigation/ ├── topological_navigation/ # Core package (Python) -│ ├── topological_navigation/ # Python modules -│ │ ├── scripts/ # ROS 2 executable scripts -│ │ ├── *.py # Core classes (route_search, map_marker, etc.) +│ ├── topological_navigation/ # Python modules +│ │ ├── scripts/ # ROS 2 executable scripts +│ │ │ ├── navigation2.py # Main navigation action server +│ │ │ ├── map_manager.py # Map loading and publishing +│ │ │ ├── localisation2.py # Topological localisation +│ │ │ ├── map_publisher.py # Map topic publisher +│ │ │ └── actions_bt.py # Behaviour tree action types +│ │ ├── edge_action_manager2.py # Edge action execution (67KB, complex) +│ │ ├── manager2.py # Map management core (58KB) +│ │ ├── route_search2.py # A* path planning +│ │ ├── topological_map.py # Map data structures +│ │ ├── load_maps_from_yaml.py # YAML map loader +│ │ └── restrictions_impl.py # Navigation restrictions │ ├── config/ # YAML schemas and templates -│ ├── doc/ # Documentation (especially PROPERTIES.md) +│ │ └── schema2.json # JSON schema for map validation +│ ├── doc/ # Documentation +│ │ └── PROPERTIES.md # Properties system guide │ ├── launch/ # ROS 2 launch files │ └── test/, tests/ # Unit and integration tests │ ├── topological_navigation_msgs/ # Message definitions -│ ├── msg/, srv/, action/ # ROS 2 interface definitions +│ ├── msg/ # ROS 2 messages +│ ├── srv/ # ROS 2 services +│ └── action/ # ROS 2 actions │ ├── topological_rviz_tools/ # Interactive map editing (C++/Qt) │ ├── src/ # RViz plugins (tools and panels) │ ├── include/ # C++ headers +│ └── scripts/ # Python interface │ └── topological_utils/ # Utilities (Python) └── topological_utils/ # Map creation and management tools @@ -44,13 +68,19 @@ topological_navigation/ ## Technology Stack -- **Language**: Python 3 (core), C++ (RViz tools) +- **Language**: Python 3 (core navigation), C++ (RViz tools) - **Framework**: ROS 2 (Humble, Iron distributions) - **Build System**: ament_python, ament_cmake - **Testing**: pytest, launch_pytest, ament linters - **GUI**: Qt (for RViz panels) -- **Data Format**: YAML for topological maps -- **Validation**: JSON Schema for map structure +- **Data Format**: YAML for topological maps (`.tmap2.yaml` extension) +- **Validation**: JSON Schema (`config/schema2.json`) +- **Key Dependencies**: + - `nav2_msgs` - Nav2 navigation actions and messages + - `geometry_msgs`, `nav_msgs`, `sensor_msgs` - ROS 2 standard messages + - `tf_transformations` - Coordinate frame transformations + - `visualization_msgs` - RViz markers + - `topological_navigation_msgs` - Custom message definitions ## Core Concepts @@ -60,44 +90,50 @@ Topological maps are the fundamental data structure in this system. **Structure**: - **Nodes**: Represent discrete locations (waypoints) with: - - Unique name + - Unique name (string identifier) - Pose (position + orientation) in a coordinate frame + - Parent frame (typically "map") - Influence zone (polygon defining the node's spatial extent) - - Flexible properties dictionary + - Flexible properties dictionary (optional) - List of outgoing edges - **Edges**: Represent navigable connections with: - - Target node reference - - Action name (e.g., "NavigateToPose", "OpenDoor") + - Target node reference (node name) + - Action name (e.g., "NavigateToPose", "OpenDoor", "RowOperation") - Action type (ROS 2 message/action type) - - Flexible properties dictionary + - Edge ID (unique identifier) + - Flexible properties dictionary (optional) **File Format**: YAML with `.tmap2.yaml` extension -**Example**: +**Example Node**: ```yaml nodes: - meta: - map: my_environment - node: ChargingStation - pointset: my_environment + map: vineyard_01 + node: RowEntry_A1 + pointset: vineyard_01 node: - name: ChargingStation + name: RowEntry_A1 parent_frame: map pose: - position: {x: 10.0, y: 5.0, z: 0.0} - orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0} + position: {x: 10.5, y: 5.2, z: 0.0} + orientation: {w: 0.707, x: 0.0, y: 0.0, z: 0.707} properties: xy_goal_tolerance: 0.3 yaw_goal_tolerance: 0.1 - semantics: "charging_station" + semantics: "row_entry" + roboflow: + enabled: true + confidence: 0.7 edges: - - edge_id: ChargingStation_WayPoint1 - node: WayPoint1 - action: NavigateToPose - action_type: geometry_msgs/PoseStamped + - edge_id: RowEntry_A1_RowEnd_A1 + node: RowEnd_A1 + action: RowOperation + action_type: nav2_msgs/action/NavigateToPose properties: max_speed: 0.5 + row_type: "vineyard" ``` ### 2. Flexible Properties System @@ -109,16 +145,23 @@ Both nodes and edges support an optional `properties` dictionary for application - Optional - properties can be omitted entirely - Supports namespacing for organization - Enables domain-specific customization without schema changes +- Validated against JSON schema for structural correctness **Common Property Patterns**: - **Flat structure**: `{capacity: 2, zone: "A"}` - **Domain namespaces**: `{restrictions: {capacity: 2}, semantics: {zone: "A"}}` -- **Package namespaces**: `{my_fleet_manager: {priority_zone: true}}` +- **Package namespaces**: `{roboflow: {enabled: true, confidence: 0.7}}` +- **Navigation parameters**: `{xy_goal_tolerance: 0.3, max_speed: 0.8}` **Important**: Always access properties defensively: ```python +# Safe property access pattern props = node["node"].get("properties", {}) capacity = props.get("capacity", 1) # Default to 1 + +# Nested property access +roboflow_config = props.get("roboflow", {}) +enabled = roboflow_config.get("enabled", False) ``` See `topological_navigation/doc/PROPERTIES.md` for comprehensive documentation. @@ -127,29 +170,68 @@ See `topological_navigation/doc/PROPERTIES.md` for comprehensive documentation. **Key Components**: -1. **Map Manager** (`map_manager2.py`) +1. **Map Manager** (`scripts/map_manager.py`, `manager2.py`) - Loads topological maps from YAML files - - Publishes map structure to ROS 2 topics + - Publishes map structure to ROS 2 topics (`/topological_map_2`) - Handles map updates and modifications + - Validates map structure against JSON schema + - Manages node and edge metadata -2. **Localisation** (`localisation2.py`) +2. **Localisation** (`scripts/localisation2.py`) - Determines robot's current topological node - Publishes `/current_node` and `/closest_node` - Supports pose-based and topic-based localization + - Uses influence zones to determine node proximity + - Critical for navigation start conditions -3. **Navigation** (`navigation2.py`) +3. **Navigation** (`scripts/navigation2.py`) - Executes topological navigation actions - Coordinates with metric navigation (Nav2) - - Handles edge action execution + - Handles edge action execution via EdgeActionManager + - Implements navigation state machine + - Provides `/topological_navigation` action server 4. **Route Search** (`route_search2.py`) - A* based path planning on topological graph - Finds optimal routes between nodes - Considers edge properties for path optimization + - Handles node restrictions and blocked edges + - Returns ordered list of nodes and edges to traverse -5. **Edge Action Manager** (`edge_action_manager.py`) +5. **Edge Action Manager** (`edge_action_manager2.py`) - Manages execution of edge-specific actions - Pluggable action system for custom behaviors + - Handles Nav2 action clients (NavigateToPose, NavigateThroughPoses, etc.) + - Implements row operations for agricultural navigation + - Manages boundary detection and edge side edges + - **Complex module**: 1,363 lines, extensive ROS 2 action handling + +### 4. Key Classes and Their Responsibilities + +#### EdgeActionManager2 (`edge_action_manager2.py`) +- **Purpose**: Execute navigation actions for edges, handle agricultural row operations +- **Key Methods**: + - `execute_action()` - Main action execution entry point + - `navigate_to_pose()` - Send Nav2 NavigateToPose actions + - `navigate_through_poses()` - Send Nav2 NavigateThroughPoses actions + - `handle_row_operations()` - Agricultural row navigation logic + - `get_boundary_nodes()` - Determine entry/exit boundary nodes for rows +- **Important**: Integrates with Nav2 action server, handles goal status callbacks + +#### TopologicalMap (`topological_map.py`) +- **Purpose**: In-memory representation of topological map +- **Key Methods**: + - `load_from_yaml()` - Load map from YAML file + - `get_node()` - Retrieve node by name + - `get_edges_from_node()` - Get all edges from a node + - `get_node_neighbours()` - Get neighbouring nodes + +#### TopologicalRouteSearch2 (`route_search2.py`) +- **Purpose**: A* path planning on topological graph +- **Key Methods**: + - `search_route()` - Find optimal path between nodes + - `get_path_cost()` - Calculate path cost with property-based weighting + - `is_node_blocked()` - Check if node is restricted/blocked ## Development Guidelines @@ -158,338 +240,573 @@ See `topological_navigation/doc/PROPERTIES.md` for comprehensive documentation. **Python**: - Follow PEP 8 style guide - Use PEP 257 docstring conventions +- Type hints encouraged for clarity (especially in new code) - Validated with `ament_flake8` and `ament_pep257` -- Type hints encouraged for clarity +- Line length: 120 characters (flexible, follow existing file style) **C++**: - Follow ROS 2 C++ style guidelines - Use modern C++ features (C++14 minimum) +- Follow ROS 2 naming conventions (snake_case for functions/variables) + +**ROS 2 Naming Conventions**: +- Node names: lowercase with underscores (e.g., `topological_navigation`) +- Topic names: lowercase with slashes (e.g., `/topological_map_2`) +- Service/action names: CamelCase (e.g., `NavigateToPose`) ### Testing **Test Structure**: -- `test/` or `tests/` directories in each package -- Python: pytest framework -- ROS 2 integration: launch_pytest -- Run: `colcon test --packages-select ` +- `test/` - Unit tests (pytest) +- `tests/` - Integration tests (launch_pytest) +- Test files: `test_*.py` or `*_tester.py` + +**Running Tests**: +```bash +# Run all tests for a package +colcon test --packages-select topological_navigation + +# Run with verbose output +colcon test --packages-select topological_navigation --event-handlers console_direct+ -**Important**: Maintain existing test infrastructure. Add tests for new features following established patterns. +# Run specific test +colcon test --packages-select topological_navigation --pytest-args -k test_route_search +``` + +**Writing Tests**: +- Follow existing test patterns in `test/` and `tests/` directories +- Use pytest fixtures for common setup +- Integration tests should use launch_pytest for ROS 2 node lifecycle +- Mock external dependencies (Nav2 action servers, etc.) ### Building ```bash -# Build all packages +# Build all packages in workspace +cd /path/to/workspace colcon build # Build specific package colcon build --packages-select topological_navigation -# Run tests -colcon test --packages-select topological_navigation +# Build with symlink install (faster for Python changes) +colcon build --symlink-install + +# Clean build +colcon build --cmake-clean-cache +``` + +**Important**: After building, source the workspace: +```bash +source install/setup.bash ``` ### Linting ```bash -# Python linting (flake8, pep257) +# Python linting (flake8) colcon test --packages-select topological_navigation \ --event-handlers console_direct+ \ - --pytest-args -k "test_flake8 or test_pep257" + --pytest-args -k test_flake8 + +# PEP 257 docstring checks +colcon test --packages-select topological_navigation \ + --event-handlers console_direct+ \ + --pytest-args -k test_pep257 + +# Run all linters +colcon test --packages-select topological_navigation \ + --event-handlers console_direct+ ``` ## Common Tasks for AI Agents -### Task: Add New Property to Topological Maps +### Task 1: Add New Property to Topological Maps + +**Goal**: Add a new property to node/edge definitions -1. **Document the property** in `topological_navigation/doc/PROPERTIES.md` - - Add to appropriate table (node or edge properties) - - Specify type, description, and examples - - Include code examples for accessing the property +**Steps**: +1. **Update Map Files** (`.tmap2.yaml`): + ```yaml + properties: + my_new_property: value + ``` -2. **Update code to use the property**: +2. **Update Code to Use Property**: ```python - # Access safely with default props = node["node"].get("properties", {}) - new_property = props.get("new_property", default_value) + my_value = props.get("my_new_property", default_value) ``` -3. **Consider namespacing** if the property is application-specific +3. **Update Documentation**: Add property description to `doc/PROPERTIES.md` + +4. **Consider Schema Update**: If property should be validated, update `config/schema2.json` + +**Key Files**: +- Map YAML files (*.tmap2.yaml) +- `doc/PROPERTIES.md` +- `config/schema2.json` (optional) +- Code files using the property + +### Task 2: Modify Edge Action Behavior + +**Goal**: Change how edges are executed during navigation + +**Key File**: `topological_navigation/edge_action_manager2.py` + +**Common Modifications**: +- Add new action type support +- Modify Nav2 goal parameters +- Add pre/post-action behaviors +- Implement custom action handlers + +**Example Pattern**: +```python +def execute_custom_action(self, edge_data): + """Execute custom edge action""" + # Extract properties + props = edge_data.get("properties", {}) + + # Prepare action goal + goal = CustomAction.Goal() + goal.parameter = props.get("custom_param", default) + + # Send action + self._send_goal(goal) +``` -4. **No schema changes needed** - properties are flexible by design +**Testing**: Add integration test in `tests/` directory -### Task: Add New ROS 2 Script +### Task 3: Add New ROS 2 Node/Script -1. **Create script** in `{package}/scripts/my_script.py` +**Goal**: Create a new ROS 2 executable script + +**Steps**: +1. **Create Script**: Add to `topological_navigation/topological_navigation/scripts/` +2. **Add to setup.py**: + ```python + entry_points={ + 'console_scripts': [ + 'my_new_node = topological_navigation.scripts.my_new_node:main', + ], + }, + ``` +3. **Follow ROS 2 Node Pattern**: ```python - #!/usr/bin/env python3 import rclpy from rclpy.node import Node - - class MyNode(Node): + + class MyNewNode(Node): def __init__(self): - super().__init__('my_node') - # Initialization + super().__init__('my_new_node') + self.get_logger().info('Node started') def main(args=None): rclpy.init(args=args) - node = MyNode() + node = MyNewNode() rclpy.spin(node) node.destroy_node() rclpy.shutdown() - - if __name__ == '__main__': - main() - ``` - -2. **Register in setup.py**: - ```python - entry_points={ - 'console_scripts': [ - 'my_script = package_name.scripts.my_script:main', - ], - } ``` -3. **Make executable**: `chmod +x {package}/scripts/my_script.py` - -4. **Test**: `ros2 run package_name my_script` - -### Task: Modify Map Schema +4. **Add Launch File** (optional): `launch/my_new_node.launch.py` +5. **Add Test**: `test/test_my_new_node.py` -1. **Update schema** in `topological_navigation/config/tmap-schema.yaml` +### Task 4: Fix Navigation Issues -2. **Update templates**: - - `config/template_node_2.yaml` - - `config/template_edge.yaml` +**Common Issues**: -3. **Update validation logic** if needed in `scripts/validate_map.py` +1. **Robot not localising correctly**: + - Check `localisation2.py` - verify influence zone configuration + - Check `/closest_node` topic - is closest node being published? + - Verify robot pose is being published to correct topic -4. **Document changes** in `doc/PROPERTIES.md` or README +2. **Edge action not executing**: + - Check `edge_action_manager2.py` - verify action client is connected + - Check Nav2 action server is running + - Verify edge action type matches available action servers + - Check edge properties for correct action parameters -5. **Test with validator**: `ros2 run topological_navigation validate_map.py map.tmap2.yaml -v` +3. **Route planning fails**: + - Check `route_search2.py` - verify graph connectivity + - Check for blocked/restricted nodes + - Verify source and target nodes exist in map -### Task: Fix Failing Tests +**Debugging Tools**: +- `/topological_map_2` topic - view current map +- `/current_node` topic - view current localisation +- `/topological_navigation/feedback` - view navigation progress +- RViz markers - visualise topological map -1. **Identify failing test**: Check CI logs or run `colcon test` +### Task 5: Update Map Schema -2. **Run test locally**: - ```bash - cd {package} - pytest test/test_name.py -v - ``` +**Goal**: Modify the structure of topological maps -3. **Fix code or test** as appropriate +**Key File**: `config/schema2.json` -4. **Verify fix**: Re-run tests +**Steps**: +1. Update JSON schema with new fields +2. Update map loader (`load_maps_from_yaml.py`) +3. Update map validation logic +4. Update example maps in `tests/` directory +5. Update documentation (`doc/PROPERTIES.md`) +6. Run validation tests to ensure backward compatibility -5. **Run linters** to ensure code quality +## Important Patterns -### Task: Add C++ RViz Plugin +### Pattern 1: Safe Property Access -1. **Create class** inheriting from `rviz_common::Tool` or `rviz_common::Panel` +Always use defensive property access to avoid KeyErrors: -2. **Implement required methods**: - - `onInitialize()` for setup - - Event handlers for interaction +```python +# Node properties +node_data = topological_map.get_node("node_name") +props = node_data["node"].get("properties", {}) +tolerance = props.get("xy_goal_tolerance", 0.5) # Default 0.5 -3. **Register plugin** in `plugin_description.xml` +# Nested properties +roboflow = props.get("roboflow", {}) +enabled = roboflow.get("enabled", False) -4. **Update CMakeLists.txt** to compile new source files +# Check existence before use +if "custom_behavior" in props: + behavior = props["custom_behavior"] +else: + behavior = default_behavior +``` -5. **Test in RViz**: Load custom plugin from RViz GUI +### Pattern 2: ROS 2 Action Client Usage -## Important Patterns +Follow the pattern in `edge_action_manager2.py`: -### Pattern: Safe Property Access +```python +from rclpy.action import ActionClient +from nav2_msgs.action import NavigateToPose + +class MyActionHandler: + def __init__(self, node): + self._nav_client = ActionClient( + node, + NavigateToPose, + '/navigate_to_pose' + ) + + def send_goal(self, pose): + # Wait for action server + self._nav_client.wait_for_server() + + # Prepare goal + goal_msg = NavigateToPose.Goal() + goal_msg.pose = pose + + # Send goal with feedback/result callbacks + send_goal_future = self._nav_client.send_goal_async( + goal_msg, + feedback_callback=self.feedback_callback + ) + send_goal_future.add_done_callback(self.goal_response_callback) +``` -Always access properties defensively to handle missing properties: +### Pattern 3: Map Loading and Validation ```python -# Node properties -node_props = node["node"].get("properties", {}) -tolerance = node_props.get("xy_goal_tolerance", 0.3) +from topological_navigation.topological_map import TopologicalMap +from topological_navigation.load_maps_from_yaml import load_map_from_yaml -# Namespaced properties -restrictions = node_props.get("restrictions", {}) -capacity = restrictions.get("capacity", 1) +# Load map +map_data = load_map_from_yaml("path/to/map.tmap2.yaml") -# Check existence before use -semantics = node_props.get("semantics") -if semantics is not None: - # Use semantics - pass -``` +# Create TopologicalMap object +topo_map = TopologicalMap() +topo_map.load_from_dict(map_data) -### Pattern: ROS 2 Node Creation +# Access nodes +node = topo_map.get_node("node_name") +edges = topo_map.get_edges_from_node("node_name") +``` -Follow standard ROS 2 patterns: +### Pattern 4: ROS 2 Parameter Handling ```python -import rclpy -from rclpy.node import Node +from rcl_interfaces.msg import ParameterDescriptor class MyNode(Node): def __init__(self): - super().__init__('my_node_name') - self.get_logger().info('Node initialized') + super().__init__('my_node') - # Declare parameters - self.declare_parameter('param_name', 'default_value') + # Declare parameters with defaults + self.declare_parameter( + 'map_file', + '', + ParameterDescriptor(description='Path to topological map YAML') + ) - # Create publishers/subscribers - self.publisher = self.create_publisher(MsgType, 'topic', 10) - self.subscription = self.create_subscription( - MsgType, 'topic', self.callback, 10) - - def callback(self, msg): - self.get_logger().info(f'Received: {msg}') + # Get parameter value + map_file = self.get_parameter('map_file').value ``` -### Pattern: Map Validation +## Common Pitfalls -Always validate maps before deployment: +### Pitfall 1: Property Access Without Defaults +**Wrong**: +```python +tolerance = node["node"]["properties"]["xy_goal_tolerance"] # KeyError if missing! +``` -```bash -ros2 run topological_navigation validate_map.py my_map.tmap2.yaml -v +**Right**: +```python +props = node["node"].get("properties", {}) +tolerance = props.get("xy_goal_tolerance", 0.5) +``` + +### Pitfall 2: Not Checking Action Server Availability +**Wrong**: +```python +self._nav_client.send_goal_async(goal) # May fail silently +``` + +**Right**: +```python +if not self._nav_client.wait_for_server(timeout_sec=5.0): + self.get_logger().error("Nav2 action server not available") + return False +self._nav_client.send_goal_async(goal) ``` -Validation checks: -- Schema compliance -- Duplicate node names -- Edges pointing to non-existent nodes -- Required fields present +### Pitfall 3: Hardcoding Frame IDs +**Wrong**: +```python +pose.header.frame_id = "map" # May not match robot's frame +``` + +**Right**: +```python +pose.header.frame_id = node_data["node"]["parent_frame"] +``` + +### Pitfall 4: Ignoring ROS 2 Callback Groups +For concurrent operations in ROS 2, use appropriate callback groups: + +```python +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup + +# For actions that shouldn't block each other +self.callback_group = ReentrantCallbackGroup() +self._nav_client = ActionClient( + self, + NavigateToPose, + '/navigate_to_pose', + callback_group=self.callback_group +) +``` ## Key Files Reference -### Configuration -- `topological_navigation/config/tmap-schema.yaml` - JSON schema for map validation -- `topological_navigation/config/template_node_2.yaml` - Node template -- `topological_navigation/config/template_edge.yaml` - Edge template +### Core Navigation Files +- `scripts/navigation2.py` - Main navigation action server (topological navigation entry point) +- `edge_action_manager2.py` - Edge action execution (complex, 1363 lines) +- `route_search2.py` - A* path planning algorithm +- `scripts/localisation2.py` - Topological localisation node +- `scripts/map_manager.py` - Map loading and publishing node -### Documentation -- `README.md` - Repository and package documentation -- `topological_navigation/doc/PROPERTIES.md` - **Essential** properties system guide -- `CHANGELOG.rst` - Version history - -### Core Python Modules -- `route_search2.py` - Path planning algorithm -- `map_marker.py` - Visualization markers -- `edge_action_manager.py` - Edge action execution -- `navigation_stats.py` - Navigation statistics tracking - -### Core Scripts -- `localisation2.py` - Topological localization -- `navigation2.py` - Navigation execution -- `map_manager2.py` - Map data management -- `validate_map.py` - Map validation tool -- `visualise_map2.py` - Map visualization +### Map Data Structures +- `topological_map.py` - In-memory map representation +- `load_maps_from_yaml.py` - YAML map loader +- `manager2.py` - Map management core logic -### Testing -- `topological_navigation/test/test_navigationcore.py` - Core navigation tests +### Utility Classes +- `dict_tools` (in edge_action_manager2.py) - Nested dictionary operations +- `tmap_utils.py` - Map manipulation utilities +- `route_search.py` - Legacy route search (v1) -## CI/CD +### Configuration and Validation +- `config/schema2.json` - JSON schema for map validation +- `doc/PROPERTIES.md` - Properties system documentation -**GitHub Actions** (`.github/workflows/`): -- `ci.yaml` - Builds and tests on multiple ROS 2 distributions (Humble, Iron) -- `validate-schema.yaml` - Validates topological map schema +### Testing +- `test/test_navigationcore.py` - Navigation core tests +- `tests/topological_navigation_tester_critical.py` - Critical integration tests +- `tests/map_manager_tester.py` - Map manager tests -**Requirements**: -- All tests must pass -- Code must pass linters (flake8, pep257) -- Compatible with ROS 2 Humble and Iron +## Agricultural Navigation Specifics -## Dependencies +### Row Operations +The system includes specialized support for agricultural row navigation: -**ROS 2 Packages**: -- `nav2_msgs` - Navigation 2 messages -- `geometry_msgs`, `std_msgs`, `nav_msgs`, `sensor_msgs` - Core ROS 2 messages -- `visualization_msgs` - RViz visualization -- `tf_transformations` - Coordinate transformations +**Key Features**: +- Boundary node detection for row entry/exit +- Side edge support for parallel row paths +- Roboflow integration for vision-based row detection +- Configurable row operation parameters -**Build Dependencies**: -- `ament_cmake` - C++ build system -- `ament_python` - Python build system +**Usage Pattern**: +```yaml +edges: + - edge_id: entry_to_exit + node: row_exit + action: RowOperation + action_type: nav2_msgs/action/NavigateToPose + properties: + row_type: "vineyard" + roboflow: + enabled: true + confidence: 0.7 + max_speed: 0.5 +``` -**Test Dependencies**: -- `pytest` - Python testing -- `launch_pytest` - ROS 2 launch testing -- `ament_flake8`, `ament_pep257` - Code quality +**Implementation**: See `edge_action_manager2.py` → `RowOperations` class integration -## Best Practices for AI Agents +### Boundary Detection +- `get_boundary_nodes()` in EdgeActionManager2 +- Uses side_edges property to identify parallel paths +- Determines optimal entry/exit points for row operations -### When Modifying Code +## Dependencies and External Systems -1. **Understand the context**: Read related documentation (especially PROPERTIES.md) -2. **Maintain compatibility**: Don't break existing topological maps -3. **Make properties optional**: Provide sensible defaults -4. **Add tests**: Follow existing test patterns -5. **Run linters**: Ensure code quality before committing -6. **Update documentation**: Reflect changes in relevant docs +### ROS 2 Navigation Stack (Nav2) +- **Actions Used**: + - `NavigateToPose` - Navigate to single goal + - `NavigateThroughPoses` - Navigate through waypoints + - `FollowWaypoints` - Follow ordered waypoint list + - `ComputePathToPose` - Path planning only + - `ComputePathThroughPoses` - Multi-goal path planning -### When Working with Properties +- **Integration Point**: `edge_action_manager2.py` +- **Assumption**: Nav2 action servers are running and accessible -1. **Access defensively**: Use `.get()` with defaults -2. **Consider namespaces**: Avoid property name conflicts -3. **Document custom properties**: In PROPERTIES.md or app-specific docs -4. **Validate maps**: Use `validate_map.py` after modifications +### Coordinate Frame Requirements +- **Base Frame**: Robot's base link (typically `base_link`) +- **Map Frame**: Global reference frame (typically `map`) +- **TF Tree**: Must have valid transforms between frames +- **Node Poses**: Defined in parent_frame (usually `map`) -### When Adding Features +### Message Dependencies +- `geometry_msgs` - Pose, PoseStamped, Point, Quaternion +- `nav_msgs` - Odometry, Path +- `std_msgs` - Header, String +- `sensor_msgs` - Various sensor data +- `visualization_msgs` - Marker, MarkerArray (for RViz) -1. **Follow ROS 2 patterns**: Use standard node/topic/service patterns -2. **Minimize dependencies**: Prefer standard library and existing dependencies -3. **Consider multiple distributions**: Code should work on Humble and Iron -4. **Test thoroughly**: Unit tests and integration tests -5. **Update CI if needed**: Ensure new features are tested +## Debugging and Troubleshooting -### When Fixing Bugs +### Common ROS 2 Commands -1. **Identify root cause**: Understand why the bug occurs -2. **Add regression test**: Prevent the bug from reoccurring -3. **Consider edge cases**: Empty maps, invalid data, missing properties -4. **Run full test suite**: Ensure fix doesn't break other functionality +```bash +# List all nodes +ros2 node list -## Common Pitfalls to Avoid +# View node info +ros2 node info /topological_navigation -1. **Assuming properties exist**: Always use `.get()` with defaults -2. **Breaking backward compatibility**: Old maps should still work -3. **Hardcoding values**: Use parameters or properties instead -4. **Forgetting ROS 2 lifecycle**: Initialize rclpy, shutdown properly -5. **Ignoring validation**: Always validate maps before deployment -6. **Not updating docs**: Changes should be reflected in documentation -7. **Mixing ROS 1 and ROS 2 APIs**: This is a ROS 2 project only +# List topics +ros2 topic list -## Getting Help +# View topic data +ros2 topic echo /topological_map_2 +ros2 topic echo /current_node -- **Documentation**: Start with README files and PROPERTIES.md -- **Code examples**: Look at existing scripts for patterns -- **Tests**: Show expected behavior and usage -- **Schema**: `tmap-schema.yaml` defines valid map structure -- **Templates**: Show minimal valid nodes and edges +# Check action servers +ros2 action list -## Quick Reference +# Send test action goal +ros2 action send_goal /topological_navigation \ + topological_navigation_msgs/action/GotoNode \ + "{target: 'node_name'}" -### Build & Test -```bash -colcon build # Build all packages -colcon test --packages-select topological_navigation # Test package +# View logs +ros2 run rqt_console rqt_console ``` -### Validate Map +### Log Analysis +- **Info logs**: Normal operation, navigation progress +- **Warn logs**: Recoverable issues, fallback behaviors +- **Error logs**: Action failures, missing data, connectivity issues + +### RViz Visualization +Launch RViz to visualize topological map: ```bash -ros2 run topological_navigation validate_map.py map.tmap2.yaml -v +ros2 launch topological_rviz_tools topological_rviz.launch.py ``` -### Run Node +**Markers**: +- Red spheres: Nodes +- Green arrows: Edges +- Blue lines: Planned route +- Yellow highlight: Current node + +## CI/CD and Quality Assurance + +### GitHub Actions Workflows +- YAML schema validation +- Python linting (flake8, pep257) +- Unit and integration tests +- Build verification + +### Pre-commit Checks +Before committing, run: ```bash -ros2 run topological_navigation script_name.py [args] -``` +# Lint +colcon test --packages-select topological_navigation \ + --pytest-args -k "flake8 or pep257" -### Access Properties -```python -props = node["node"].get("properties", {}) -value = props.get("key", default) +# Unit tests +colcon test --packages-select topological_navigation ``` +## Additional Resources + +### Documentation +- `README.md` - Package overview +- `doc/PROPERTIES.md` - Properties system detailed guide +- `.github/copilot-instructions.md` - GitHub Copilot specific guidance +- `REVIEW.md` - Code review guidelines + +### Example Maps +- `tests/` directory contains example topological maps +- Look for `.tmap2.yaml` files + +### External Links +- ROS 2 Documentation: https://docs.ros.org/ +- Nav2 Documentation: https://navigation.ros.org/ +- ROS 2 Action Documentation: https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Actions/Understanding-ROS2-Actions.html + +## Getting Help + +### Internal Resources +1. Check existing code patterns in similar files +2. Read test files for usage examples +3. Review commit history for recent changes: `git log --oneline` + +### External Resources +1. ROS 2 Discourse: https://discourse.ros.org/ +2. Nav2 GitHub Issues: https://github.com/ros-planning/navigation2/issues +3. ROS Answers: https://answers.ros.org/ + +## Agent-Specific Guidelines + +### When Analyzing This Codebase +1. **Start with map structure**: Understand topological maps first +2. **Follow data flow**: Map → Route Search → Navigation → Edge Actions +3. **Check properties**: Always verify what properties are available/required +4. **Test incrementally**: Small changes, test frequently +5. **Respect ROS 2 patterns**: Use established action/topic/service patterns + +### When Making Changes +1. **Preserve backward compatibility**: Especially for map formats +2. **Add tests**: Every new feature needs tests +3. **Update documentation**: Keep AGENTS.md, PROPERTIES.md, README.md in sync +4. **Follow existing patterns**: Don't introduce new patterns without discussion +5. **Consider agricultural use cases**: Changes impact real-world robot operations + +### When Debugging +1. **Start with logs**: Check ROS 2 logs for errors/warnings +2. **Verify map structure**: Ensure YAML maps are valid +3. **Check action servers**: Ensure Nav2 is running +4. **Use RViz**: Visualize to understand spatial relationships +5. **Isolate components**: Test map loading, routing, actions separately + --- -**Remember**: This is a topological (graph-based) navigation system, not metric (coordinate-based). Nodes are discrete locations, edges are connections. Properties are flexible and optional. Always code defensively. +**Last Updated**: 2026-01-27 +**Branch**: agent_prep +**Maintainer**: AI Coding Agents From 4a0e62a15c67d2ece6ef7301722b89e79bc046d6 Mon Sep 17 00:00:00 2001 From: Ibrahim Hroob <47870260+ibrahimhroob@users.noreply.github.com> Date: Tue, 3 Feb 2026 09:39:22 +0000 Subject: [PATCH 22/23] Enhance EdgeActionManager and add flexible properties system (#235) * bug fix for edge action manager for inrow navigation, the bug is that it assumes the row node id has a single digit!!! * Clean up logs in edge_action_manager2.py Removed commented-out log statements for clarity. --------- Co-authored-by: Your Name Co-authored-by: Marc Hanheide --- .../topological_navigation/edge_action_manager2.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/topological_navigation/topological_navigation/edge_action_manager2.py b/topological_navigation/topological_navigation/edge_action_manager2.py index 818d45d4..93af6631 100644 --- a/topological_navigation/topological_navigation/edge_action_manager2.py +++ b/topological_navigation/topological_navigation/edge_action_manager2.py @@ -610,19 +610,21 @@ def _get_row_center_node(self, edge_id: str): Returns: (center_node, tag_id, target_row_edge_id) or (None, None, None) on failure. """ try: - target_row_edge_id_raw = edge_id.split("_")[0] - tag_id = target_row_edge_id_raw.split("-")[1] - # print trget_row_edge_id_raw, tag_id + # edge_id = r7.5-c20_r7.5-c19 + target_row_edge_id_raw = edge_id.split("_")[0] # e.g. 'r7.5-c20' + tag_id = target_row_edge_id_raw.split("-")[1] # e.g. 'c20' + # print trget_row_edge_id_raw, tag_id self.get_logger().info(f"[_get_row_center_node] Parsed edge_id='{edge_id}' to target_row_edge_id_raw='{target_row_edge_id_raw}', tag_id='{tag_id}'") # Force ROW_START_INDEX by replacing the last character - target_row_edge_id = target_row_edge_id_raw[:-1] + self.ACTIONS.ROW_START_INDEX - tag_id = tag_id[:-1] + self.ACTIONS.ROW_START_INDEX + # e.g. 'r7.5-c20' -> 'r7.5-c2a' if ROW_START_INDEX='a' + target_row_edge_id = target_row_edge_id_raw.split("c", 1)[0] + "c" + self.ACTIONS.ROW_START_INDEX # target_row_edge_id_raw[:-1] + self.ACTIONS.ROW_START_INDEX + tag_id = tag_id[0] + self.ACTIONS.ROW_START_INDEX #print target_row_edge_id, tag_id self.get_logger().info(f"[_get_row_center_node] Adjusted to target_row_edge_id='{target_row_edge_id}', tag_id='{tag_id}'") except Exception as e: self.get_logger().error(f"[_get_row_center_node] Failed to parse edge_id='{edge_id}': {e}") return None, None, None - + cen = self.route_search.get_node_from_tmap2(target_row_edge_id) if not cen or "node" not in cen or "pose" not in cen["node"]: self.get_logger().error(f"[_get_row_center_node] Could not resolve '{target_row_edge_id}'") From bd8ab5b2e56b59989761d60b0a478a359c8f3574 Mon Sep 17 00:00:00 2001 From: "James R. Heselden" <34611419+Iranaphor@users.noreply.github.com> Date: Sun, 19 Apr 2026 20:55:42 +0100 Subject: [PATCH 23/23] change default param to be more clearly functional (#246) --- .../topological_navigation/scripts/navigation2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/topological_navigation/topological_navigation/scripts/navigation2.py b/topological_navigation/topological_navigation/scripts/navigation2.py index 0bef2339..5f59f7d6 100755 --- a/topological_navigation/topological_navigation/scripts/navigation2.py +++ b/topological_navigation/topological_navigation/scripts/navigation2.py @@ -122,7 +122,7 @@ def __init__(self, name, update_params_control_server, edge_action_manager_serve self.navigation_action_name = self.get_parameter_or("navigation_action_name", Parameter('str', Parameter.Type.STRING, self.ACTIONS.NAVIGATE_TO_POSE)).value self.navigation_actions = self.get_parameter_or("navigation_actions", Parameter('str', Parameter.Type.STRING_ARRAY, self.ACTIONS.navigation_actions)).value - self.use_nav2_follow_route = self.get_parameter_or("use_nav2_follow_route", Parameter('bool', Parameter.Type.BOOL, False)).value + self.use_nav2_follow_route = self.get_parameter_or("use_nav2_follow_route", Parameter('bool', Parameter.Type.BOOL, True)).value self.use_in_row_operation = self.get_parameter_or("use_in_row_operation", Parameter('bool', Parameter.Type.BOOL, False)).value self.inrow_step_size = self.get_parameter_or("inrow_step_size", Parameter('double', Parameter.Type.DOUBLE, 2.0)).value self.inrow_step_intermediate_dis = self.get_parameter_or("inrow_step_intermediate_dis", Parameter('double', Parameter.Type.DOUBLE, -1.0)).value