[FEAT]: Topological Map Structure

[Iranaphor]

Description

I have opened this as a forum on redefining the exlicit structure of the topological map.

This is to encompass both the method of defining the map (json schema, ros2 msg), and the content to be contained within. Below is a short sample of the currently active topological map.

meta:
  last_updated: 2022-06-23_13-10-11
metric_map: riseholme
name: strawberry_polytunnel
nodes:
- meta:
    map: riseholme
    node: WayPoint140
    pointset: riseholme_restrict
  node:
    edges:
    - action: NavigateToPose
      action_type: geometry_msgs/PoseStamped
      config: []
      edge_id: WayPoint140_WayPoint74
      fail_policy: fail
      fluid_navigation: true
      goal:
        target_pose:
          header:
            frame_id: $node.parent_frame
          pose: $node.pose
      node: WayPoint74
      recovery_behaviours_config: ''
      restrictions_planning: 'True'
      restrictions_runtime: obstacleFree_1
    - action: NavigateToPose
      action_type: geometry_msgs/PoseStamped
      config: []
      edge_id: WayPoint140_WayPoint141
      fail_policy: fail
      fluid_navigation: true
      goal:
        target_pose:
          header:
            frame_id: $node.parent_frame
          pose: $node.pose
      node: WayPoint141
      recovery_behaviours_config: ''
      restrictions_planning: 'True'
      restrictions_runtime: obstacleFree_1
    - action: NavigateToPose
      action_type: geometry_msgs/PoseStamped
      config: []
      edge_id: WayPoint140_WayPoint142
      fail_policy: fail
      fluid_navigation: true
      goal:
        target_pose:
          header:
            frame_id: $node.parent_frame
          pose: $node.pose
      node: WayPoint142
      recovery_behaviours_config: ''
      restrictions_planning: 'True'
      restrictions_runtime: obstacleFree_1
    localise_by_topic: ''
    name: WayPoint140
    parent_frame: map
    pose:
      orientation:
        w: 0.7897749049165983
        x: 0.0
        y: 0.0
        z: -0.6133967717260812
      position:
        x: 20.7508434296
        y: -4.37950954437
        z: 0.0
    properties:
      xy_goal_tolerance: 0.3
      yaw_goal_tolerance: 0.1
    restrictions_planning: 'True'
    restrictions_runtime: obstacleFree_1
    verts:
    - x: -0.389276951551
      y: 0.63791257143
    - x: -0.725811064243
      y: 0.177953019738
    - x: -0.63791257143
      y: -0.389276951551
    - x: -0.177953019738
      y: -0.725811064243
    - x: 0.389276951551
      y: -0.63791257143
    - x: 0.725811064243
      y: -0.177953019738
    - x: 0.63791257143
      y: 0.389276951551
    - x: 0.177953019738
      y: 0.725811064243
- meta:
    map: riseholme
    node: WayPoint141
    pointset: riseholme_restrict
  node:
    edges:
    - action: NavigateToPose
      action_type: geometry_msgs/PoseStamped
      config: []
      edge_id: WayPoint141_WayPoint73
      fail_policy: fail
      fluid_navigation: true
      goal:
        target_pose:
          header:
            frame_id: $node.parent_frame
          pose: $node.pose
      node: WayPoint73
      recovery_behaviours_config: ''
      restrictions_planning: 'True'
      restrictions_runtime: obstacleFree_1
    - action: NavigateToPose
      action_type: geometry_msgs/PoseStamped
      config: []
      edge_id: WayPoint141_WayPoint140
      fail_policy: fail
      fluid_navigation: true
      goal:
        target_pose:
          header:
            frame_id: $node.parent_frame
          pose: $node.pose
      node: WayPoint140
      recovery_behaviours_config: ''
      restrictions_planning: 'True'
      restrictions_runtime: obstacleFree_1
    - action: NavigateToPose
      action_type: geometry_msgs/PoseStamped
      config: []
      edge_id: WayPoint141_WayPoint144
      fail_policy: fail
      fluid_navigation: true
      goal:
        target_pose:
          header:
            frame_id: $node.parent_frame
          pose: $node.pose
      node: WayPoint144
      recovery_behaviours_config: ''
      restrictions_planning: 'True'
      restrictions_runtime: 'True'
    localise_by_topic: ''
    name: WayPoint141
    parent_frame: map
    pose:
      orientation:
        w: 0.8029703158157347
        x: 0.0
        y: 0.0
        z: -0.5960190197626072
      position:
        x: 23.3039474487
        y: -3.95409524918
        z: 0.0
    properties:
      xy_goal_tolerance: 0.3
      yaw_goal_tolerance: 0.1
    restrictions_planning: 'True'
    restrictions_runtime: obstacleFree_1
    verts:
    - x: -0.389276951551
      y: 0.63791257143
    - x: -0.725811064243
      y: 0.177953019738
    - x: -0.63791257143
      y: -0.389276951551
    - x: -0.177953019738
      y: -0.725811064243
    - x: 0.389276951551
      y: -0.63791257143
    - x: 0.725811064243
      y: -0.177953019738
    - x: 0.63791257143
      y: 0.389276951551
    - x: 0.177953019738
      y: 0.725811064243

Additional Information

No response

[Iranaphor]

As can be seen there are many fields which have not been used in some time, and a lot of repeated information making the file much larger than necessary.

I would thus like to propose firstly, that we include a new section at the root of the map to define default values, essentially a sample node which defines the values which should be used if there is not a value provided.

defaults:
  meta:
    map: riseholme
    pointset: riseholme_restrict
  node:
    edges:
    - action: NavigateToPose
      action_type: geometry_msgs/PoseStamped
      config: []
      fail_policy: fail
      fluid_navigation: true
      goal:
        target_pose:
          header:
            frame_id: $node.parent_frame
          pose: $node.pose
      recovery_behaviours_config: ''
      restrictions_planning: 'True'
      restrictions_runtime: obstacleFree_1
    - action: NavigateToPose
      action_type: geometry_msgs/PoseStamped
      config: []
      fail_policy: fail
      fluid_navigation: true
      goal:
        target_pose:
          header:
            frame_id: $node.parent_frame
          pose: $node.pose
      recovery_behaviours_config: ''
      restrictions_planning: 'True'
      restrictions_runtime: obstacleFree_1
    - action: NavigateToPose
      action_type: geometry_msgs/PoseStamped
      config: []
      fail_policy: fail
      fluid_navigation: true
      goal:
        target_pose:
          header:
            frame_id: $node.parent_frame
          pose: $node.pose
      recovery_behaviours_config: ''
      restrictions_planning: 'True'
      restrictions_runtime: 'True'
    localise_by_topic: ''
    parent_frame: map
    pose:
      orientation:
        x: 0.0
        y: 0.0
      position:
        z: 0.0
    properties:
      xy_goal_tolerance: 0.3
      yaw_goal_tolerance: 0.1
    restrictions_planning: 'True'
    restrictions_runtime: obstacleFree_1

meta:
  last_updated: 2022-06-23_13-10-11
metric_map: riseholme
name: strawberry_polytunnel
nodes:
- meta:
    node: WayPoint140
  node:
    edges:
    - edge_id: WayPoint140_WayPoint74
      node: WayPoint74
    - edge_id: WayPoint140_WayPoint141
      node: WayPoint141
    - edge_id: WayPoint140_WayPoint142
      node: WayPoint142
    name: WayPoint140
    pose:
      orientation:
        w: 0.7897749049165983
        z: -0.6133967717260812
      position:
        x: 20.7508434296
        y: -4.37950954437
    verts:
    - x: -0.389276951551
      y: 0.63791257143
    - x: -0.725811064243
      y: 0.177953019738
    - x: -0.63791257143
      y: -0.389276951551
    - x: -0.177953019738
      y: -0.725811064243
    - x: 0.389276951551
      y: -0.63791257143
    - x: 0.725811064243
      y: -0.177953019738
    - x: 0.63791257143
      y: 0.389276951551
    - x: 0.177953019738
      y: 0.725811064243
- meta:
    node: WayPoint141
  node:
    edges:
    - edge_id: WayPoint141_WayPoint73
      node: WayPoint73
    - edge_id: WayPoint141_WayPoint140
      node: WayPoint140
    - edge_id: WayPoint141_WayPoint144
      node: WayPoint144
    name: WayPoint141
    pose:
      orientation:
        w: 0.8029703158157347
        z: -0.5960190197626072
      position:
        x: 23.3039474487
        y: -3.95409524918
    verts:
    - x: -0.389276951551
      y: 0.63791257143
    - x: -0.725811064243
      y: 0.177953019738
    - x: -0.63791257143
      y: -0.389276951551
    - x: -0.177953019738
      y: -0.725811064243
    - x: 0.389276951551
      y: -0.63791257143
    - x: 0.725811064243
      y: -0.177953019738
    - x: 0.63791257143
      y: 0.389276951551
    - x: 0.177953019738
      y: 0.725811064243

It can be seen above how much this would reduce the size of the map file (reducing each node from 84 lines to 36).

[Iranaphor]

It would also be worth, in this process, looking into how else we might like to define the verts section. In practice, many of the verts we use are identical in each direction and circular, could we look into redefining these geometrically rather than polygonal.

Such an approach could look like below:

meta:
  last_updated: 2022-06-23_13-10-11
metric_map: riseholme
name: strawberry_polytunnel
nodes:
- meta:
    node: WayPoint140
  node:
    edges:
    - edge_id: WayPoint140_WayPoint74
      node: WayPoint74
    - edge_id: WayPoint140_WayPoint141
      node: WayPoint141
    - edge_id: WayPoint140_WayPoint142
      node: WayPoint142
    name: WayPoint140
    pose:
      orientation:
        w: 0.7897749049165983
        z: -0.6133967717260812
      position:
        x: 20.7508434296
        y: -4.37950954437
    verts:
      shape: circle
      radius: 5
      nodes: 8
      orientation: 0.0
- meta:
    node: WayPoint141
  node:
    edges:
    - edge_id: WayPoint141_WayPoint73
      node: WayPoint73
    - edge_id: WayPoint141_WayPoint140
      node: WayPoint140
    - edge_id: WayPoint141_WayPoint144
      node: WayPoint144
    name: WayPoint141
    pose:
      orientation:
        w: 0.8029703158157347
        z: -0.5960190197626072
      position:
        x: 23.3039474487
        y: -3.95409524918
    verts: 
        shape: polygon
        reference: polygonal_vert_0
        orientation: 0.0

This approach would have a few predefined shapes, (i.e. circle, square, diamond) and based on the selected shape and the properties provided, would populate the map with the given data to be published. Existing polygons could be incorporated through a polygonal verts container at the map root as below:

polygonal_verts:
    polygonal_vert_0:
    - x: -0.389276951551
      y: 0.63791257143
    - x: -0.725811064243
      y: 0.177953019738
    - x: 0.63791257143
      y: 0.389276951551
    - x: 0.177953019738
      y: 0.725811064243
    polygonal_vert_1:
    - x: -0.389276951551
      y: 0.63791257143
    - x: -0.725811064243
      y: 0.177953019738
    - x: 0.63791257143
      y: 0.389276951551
    - x: 0.177953019738
      y: 0.725811064243

With both the defaults section, and the verts rework, this could significantly improve working with topological maps, especially as the map files themselves get much larger.

I have used a informal approach to this in storing maps here which has worked well for keeping map sizes small.

[Iranaphor]

It is worth contextualising that our latest maps for risheolme are around 14k lines long at only 190 nodes.

(https://github.com/LCAS/environment_template/blob/riseholme_park_farm_strawberry_polytunnels/config/topological/network_autogen.tmap2.yaml)

[Iranaphor]

I will also include reference to the map uuid here also from #201