Function for converting topological maps into the new format in the m…

[adambinch]

…ap manager. When loading a map using the map manager, the conversion happens automatically and the new map is broadcast onto the topic /topological_map_2 as a string. Includes a map manager 2 class for handling topological maps in the new format. This is purely additive - existing functionality is unaffected.

New format:

metric_map: map2d                       # 2D Metric map name (not used currently)
pointset:   new_map                     # The name of the group of nodes that compose the topological map, typically the same as name
transformation:
  rotation:                        # Topological Map Rotation
    w: 1.0
    x: 0.0
    y: 0.0
    z: 0.0
  translation:                                  # Topological Map Translation
    x: 0.0
    y: 0.0
    z: 0.0
  child: topo_map
  parent: map
meta:                            # Just some meta information
  last_updated: 2020-02-18 12:00 am              # Last time the map was modified
nodes:
- meta:                            # Just some meta information
    map: map2d
    node: WayPoint001        
    pointset: new_map
  node:                                 # Actual Node Properties
    edges:                        # List of edges departing from the node
    - action: move_action                # Action Name
      node: WayPoint002
      config: &move_action_config_1             # Action Configuration
        inflation_radius: 0.0                   # Action Parameters
        top_vel: 0.55
        recovery_behaviours_config: ''
      action_type: 'move_action_msgs/MoveActionGoal'      # Goal type for the Action
      goal:
        target_pose: $node.pose
      fail_policy: retry_3, replan, fail        # What to do if execution fail (fail, replan, retry_#number_of_times)
      restrictions:
        robot_type: "Narrow"
    - action: move_action                # Action Name
      node: WayPoint003
      config: &move_action_config_2             # Action Configuration
        inflation_radius: 0.2                   # Action Parameters
        top_vel: 0.75
        recovery_behaviours_config: ''
      action_type: move_action_msgs/MoveActionGoal      # Goal type for the Action
      goal:
        target_pose: $node.pose
      fail_policy: fail                    # What to do if execution fail (fail, replan, retry_#number_of_times)
    localise_by_topic: ''                       # Localise by topic configuration string
    name: WayPoint001                        # Node name
    pose:                        # Node pose
      orientation:                              # Node orientation (quaternion) 
        w: 0.92388
        x: 0
        y: 0
        z: 0.38268
      position:                        # Node position
        x: 0.0
        y: 0.0
        z: 0.0
    verts:                        # Vetices of the influence zone relative to node position
    - x: 0.689
      y: 0.287
    - x: 0.287
      y: 0.689
    - x: -0.287
      y: 0.689
    - x: -0.689
      y: 0.287
    - x: -0.689
      y: -0.287
    - x: -0.287
      y: -0.689
    - x: 0.287
      y: -0.689
    - x: 0.689
      y: -0.287
    properties:
      xy_goal_tolerance: 0.3                # Goal tolerance in the XY axes
      yaw_goal_tolerance: 0.1                 # Angular goal tolerance
    restrictions:
      robot_type: "Tall"
- meta:                            # Just some meta information
    map: map2d
    node: WayPoint002
    pointset: new_map
  node:                                 # Actual Node Properties
    edges:                        # List of edges departing from the node
    - action: move_action                # Action Name
      node: WayPoint001
      config: *move_action_config_1             # Action Configuration
      action_type: move_action_msgs/MoveActionGoal      # Goal type for the Action
      goal:
        target_pose: $node.pose
      fail_policy: retry_3, replan, fail        # What to do if execution fail (fail, replan, retry_#number_of_times)
    - action: move_action                # Action Name
      node: WayPoint003
      config: *move_action_config_2             # Action Configuration
      action_type: move_action_msgs/MoveActionGoal      # Goal type for the Action
      goal:
        target_pose: $node.pose
      fail_policy: fail                    # What to do if execution fail (fail, replan, retry_#number_of_times)
    localise_by_topic: ''                       # Localise by topic configuration string
    name: WayPoint002                        # Node name
    pose:                        # Node pose
      orientation:                              # Node orientation (quaternion) 
        w: 0.92388
        x: 0
        y: 0
        z: 0.38268
      position:                        # Node position
        x: 1.0
        y: 1.0
        z: 0.0
    verts:                        # Vetices of the influence zone relative to node position
    - x: 0.689
      y: 0.287
    - x: 0.287
      y: 0.689
    - x: -0.287
      y: 0.689
    - x: -0.689
      y: 0.287
    - x: -0.689
      y: -0.287
    - x: -0.287
      y: -0.689
    - x: 0.287
      y: -0.689
    - x: 0.689
      y: -0.287
    properties:
      xy_goal_tolerance: 0.3                # Goal tolerance in the XY axes
      yaw_goal_tolerance: 0.1                 # Angular goal tolerance
    restrictions:
      robot_type: "Tall"
- meta:                            # Just some meta information
    map: map2d
    node: WayPoint003
    pointset: new_map
  node:                                 # Actual Node Properties
    edges:                        # List of edges departing from the node
    - action: move_action                # Action Name
      node: WayPoint001
      config: *move_action_config_1             # Action Configuration
      action_type: move_action_msgs/MoveActionGoal      # Goal type for the Action
      goal:
        target_pose: $node.pose
      fail_policy: retry_3, replan, fail        # What to do if execution fail (fail, replan, retry_#number_of_times)
    - action: move_action                # Action Name
      node: WayPoint002
      config: *move_action_config_2             # Action Configuration
      action_type: move_action_msgs/MoveActionGoal      # Goal type for the Action
      goal:
        target_pose: $node.pose
      fail_policy: fail                    # What to do if execution fail (fail, replan, retry_#number_of_times)
    localise_by_topic: ''                       # Localise by topic configuration string
    name: WayPoint003                        # Node name
    pose:                        # Node pose
      orientation:                              # Node orientation (quaternion) 
        w: 0.92388
        x: 0
        y: 0
        z: 0.38268
      position:                        # Node position
        x: -1.0
        y: -1.0
        z: 0.0
    verts:                        # Vetices of the influence zone relative to node position
    - x: 0.689
      y: 0.287
    - x: 0.287
      y: 0.689
    - x: -0.287
      y: 0.689
    - x: -0.689
      y: 0.287
    - x: -0.689
      y: -0.287
    - x: -0.287
      y: -0.689
    - x: 0.287
      y: -0.689
    - x: 0.689
      y: -0.287
    properties:
      xy_goal_tolerance: 0.3                # Goal tolerance in the XY axes
      yaw_goal_tolerance: 0.1                 # Angular goal tolerance
    restrictions:
      robot_type: "Narrow"

[Jailander]

Going to close this PR as it will go against toponav2-devel branch