Merge pull request #122 from osrf/fix/building_map_server

Fix/building map server

building_map_msgs/msg/Door.msg

@@ -4,12 +4,13 @@ string name
 # ===========
 # single hinge doors:
 #   * hinge is located at (v1_x, v1_y)
+#   * door extends till (v2_x, v2_y)
 #   * motion_range = door swing range in DEGREES
 #   * there are two possible motions: clockwise and anti-clockwise
 #     selected by the motion_direction parameter, which is +1 or -1
 #
 # double hinge doors:
-#   * hinges are located at both (v1_x) and (v2_x)
+#   * hinges are located at both (v1_x, v1_y) and (v2_x, v2_y)
 #   * motion range = door swing ranges in DEGREES (assume symmetric)
 #   * same motion-direction selection as single hinge
 #
@@ -24,6 +25,8 @@ string name
 #   * common in elevators; same parameters as sliding doors; they just
 #     open/close faster and take up less space inside the wall.
 
+string door_name
+
 float32 v1_x
 float32 v1_y
 

building_map_msgs/msg/Lift.msg

@@ -8,3 +8,7 @@ Graph wall_graph
 float32 ref_x
 float32 ref_y
 float32 ref_yaw
+
+# width and depth of the cabin
+float32 width
+float32 depth

building_map_tools/building_map/building.py

@@ -5,6 +5,7 @@
 from ament_index_python.packages import get_package_share_directory
 
 from .level import Level
+from .lift import Lift
 
 
 class Building:
@@ -19,6 +20,10 @@ def __init__(self, yaml_node):
         for level_name, level_yaml in yaml_node['levels'].items():
             self.levels[level_name] = Level(level_yaml, level_name)
 
+        self.lifts = {}
+        for lift_name, lift_yaml in yaml_node['lifts'].items():
+            self.lifts[lift_name] = Lift(lift_yaml, lift_name)
+
         if 'reference_level_name' in yaml_node:
             self.reference_level_name = yaml_node['reference_level_name']
         else:

building_map_tools/building_map/level.py

@@ -32,7 +32,7 @@ def __init__(self, yaml_node, name):
 
         self.elevation = 0.0
         if 'elevation' in yaml_node:
-            self.elevation = yaml_node['elevation']
+            self.elevation = float(yaml_node['elevation'])
 
         self.fiducials = []
         if 'fiducials' in yaml_node:

building_map_tools/building_map/lift.py

@@ -0,0 +1,42 @@
+
+
+class LiftDoor:
+    def __init__(self, yaml_node, name):
+        self.name = name
+        self.door_type = yaml_node['door_type']
+        # x & y coordinates are with respect to the centre of the cabin
+        self.x = float(yaml_node['x'])
+        self.y = float(yaml_node['y'])
+        self.motion_axis_orientation = float(
+            yaml_node['motion_axis_orientation'])
+        self.width = float(yaml_node['width'])
+
+
+class Lift:
+    def __init__(self, yaml_node, name):
+        self.name = name
+        print(f'parsing lift {name}')
+
+        self.reference_floor_name = yaml_node['reference_floor_name']
+        self.depth = float(yaml_node['depth'])
+        self.width = float(yaml_node['width'])
+        self.x = float(yaml_node['x'])
+        self.y = float(yaml_node['y'])
+        self.yaw = float(yaml_node['yaw'])
+
+        self.level_names = []
+        self.door_names = []
+        if 'level_doors' in yaml_node:
+            for level_name, door_name in yaml_node['level_doors'].items():
+                self.level_names.append(level_name)
+                self.door_names.append(door_name)
+
+        self.doors = []
+        if 'doors' in yaml_node:
+            self.doors = self.parse_lift_doors(yaml_node['doors'])
+
+    def parse_lift_doors(self, yaml_node):
+        doors = []
+        for lift_door_name, lift_door_yaml in yaml_node.items():
+            doors.append(LiftDoor(lift_door_yaml, lift_door_name))
+        return doors

building_map_tools/building_map/vertex.py

@@ -3,9 +3,9 @@
 
 class Vertex:
     def __init__(self, yaml_node):
-        self.x = yaml_node[0]
-        self.y = -yaml_node[1]
-        self.z = yaml_node[2]  # currently always 0
+        self.x = float(yaml_node[0])
+        self.y = float(-yaml_node[1])
+        self.z = float(yaml_node[2])  # currently always 0
         self.name = yaml_node[3]
 
         self.params = {}

building_map_tools/building_map_server/building_map_server.py

@@ -26,6 +26,8 @@
 
 from building_map.building import Building
 
+from building_map.transform import Transform
+
 
 class BuildingMapServer(Node):
     def __init__(self, map_path):
@@ -67,13 +69,14 @@ def building_map_msg(self, building):
         msg.name = building.name
         for _, level_data in building.levels.items():
             msg.levels.append(self.level_msg(level_data))
-        # todo: lifts
+        for _, lift_data in building.lifts.items():
+            msg.lifts.append(self.lift_msg(lift_data))
         return msg
 
     def level_msg(self, level):
         msg = Level()
         msg.name = level.name
-        msg.elevation = 0.0  # todo: actually capture this in traffic-editor
+        msg.elevation = level.elevation
         if level.drawing_name:
             image = AffineImage()
             image_filename = level.drawing_name
@@ -92,29 +95,87 @@ def level_msg(self, level):
                 len(image.data), image_filename))
             msg.images.append(image)
 
-            # for now, nav graphs are just single-digit numbers
-            for i in range(0, 9):
-                g = level.generate_nav_graph(i, always_unidirectional=False)
-                if not g['lanes']:
-                    continue  # empty graph :(
-                graph_msg = Graph()
-                graph_msg.name = str(i)  # todo: someday, string names...
-                for v in g['vertices']:
-                    gn = GraphNode()
-                    gn.x = v[0]
-                    gn.y = v[1]
-                    gn.name = v[2]['name']
-                    graph_msg.vertices.append(gn)
-                for l in g['lanes']:
-                    ge = GraphEdge()
-                    ge.v1_idx = l[0]
-                    ge.v2_idx = l[1]
-                    if l[2]['is_bidirectional']:
-                        ge.edge_type = GraphEdge.EDGE_TYPE_BIDIRECTIONAL
-                    else:
-                        ge.edge_type = GraphEdge.EDGE_TYPE_UNIDIRECTIONAL
-                    graph_msg.edges.append(ge)
-                msg.nav_graphs.append(graph_msg)
+        if (len(level.doors)):
+            for door in level.doors:
+                door_msg = Door()
+                door_msg.door_name = door.params['name'].value
+                door_msg.v1_x = level.vertices[door.start_idx].x
+                door_msg.v1_y = level.vertices[door.start_idx].y
+                door_msg.v2_x = level.vertices[door.end_idx].x
+                door_msg.v2_y = level.vertices[door.end_idx].y
+                door_msg.motion_range = math.pi * float(
+                    door.params['motion_degrees'].value) / 180.0
+                door_msg.motion_direction = door.params[
+                    'motion_direction'].value
+                door_type = door.params['type'].value
+                if door_type == 'sliding':
+                    door_msg.door_type = door_msg.DOOR_TYPE_SINGLE_SLIDING
+                elif door_type == 'hinged':
+                    door_msg.door_type = door_msg.DOOR_TYPE_SINGLE_SWING
+                elif door_type == 'double_sliding':
+                    door_msg.door_type = door_msg.DOOR_TYPE_DOUBLE_SLIDING
+                elif door_type == 'double_hinged':
+                    door_msg.door_type = door_msg.DOOR_TYPE_DOUBLE_SWING
+                else:
+                    door_msg.door_type = door_msg.DOOR_TYPE_UNDEFINED
+                msg.doors.append(door_msg)
+
+        # for now, nav graphs are just single-digit numbers
+        for i in range(0, 9):
+            g = level.generate_nav_graph(i, always_unidirectional=False)
+            if not g['lanes']:
+                continue  # empty graph :(
+            graph_msg = Graph()
+            graph_msg.name = str(i)  # todo: someday, string names...
+            for v in g['vertices']:
+                gn = GraphNode()
+                gn.x = v[0]
+                gn.y = v[1]
+                gn.name = v[2]['name']
+                graph_msg.vertices.append(gn)
+            for l in g['lanes']:
+                ge = GraphEdge()
+                ge.v1_idx = l[0]
+                ge.v2_idx = l[1]
+                if l[2]['is_bidirectional']:
+                    ge.edge_type = GraphEdge.EDGE_TYPE_BIDIRECTIONAL
+                else:
+                    ge.edge_type = GraphEdge.EDGE_TYPE_UNIDIRECTIONAL
+                graph_msg.edges.append(ge)
+            msg.nav_graphs.append(graph_msg)
+        return msg
+
+    def lift_msg(self, lift):
+        msg = Lift()
+        msg.name = lift.name
+        msg.levels = lift.level_names
+        msg.ref_x = lift.x
+        msg.ref_y = lift.y
+        msg.ref_yaw = lift.yaw
+        msg.width = lift.width
+        msg.depth = lift.depth
+        for door in lift.doors:
+            door_msg = Door()
+            door_msg.door_name = door.name
+            door_msg.door_type = door.door_type
+            # find the vertices of the lift in global frame
+            v1_x = -0.5 * door.width
+            v1_y = 0.0
+            v2_x = 0.5 * door.width
+            v2_y = 0.0
+            transform = Transform()
+            transform.set_rotation(lift.yaw + door.motion_axis_orientation)
+            transform.set_translation(lift.x + door.x, lift.y + door.y)
+            v1_x, v1_y = transform.transform_point([v1_x, v1_y])
+            v2_x, v2_y = transform.transform_point([v2_x, v2_y])
+            door_msg.v1_x = v1_x
+            door_msg.v1_y = v1_y
+            door_msg.v2_x = v2_x
+            door_msg.v2_y = v2_y
+            # todo add these fields to lift doors
+            door_msg.motion_range = 1.571
+            door_msg.motion_direction = -1
+            msg.doors.append(door_msg)
         return msg
 
     def get_building_map(self, request, response):

building_map_tools/building_map_server/test/test_map_client.py

@@ -24,17 +24,43 @@ def __init__(self):
         self.create_subscription(
             BuildingMap, 'map', self.map_cb, qos_profile=qos)
 
+    def print_door(self, door):
+        print(f'        {door.door_name}, ' +
+              f'v1:[{round(door.v1_x, 2)},{round(door.v1_y,2)}], ' +
+              f'v2:[{round(door.v2_x, 2)},{round(door.v2_y,2)}], ' +
+              f'type:{door.door_type}, range:{round(door.motion_range, 2)}, ' +
+              f'dir:{door.motion_direction}')
+
     def map_cb(self, msg):
         print('received map!')
         print(f'building name: {msg.name}')
+        print(f'{len(msg.lifts)} lifts')
+        for lift in msg.lifts:
+            print(f'  lift: {lift.name}')
+            print(f'    {lift.levels} levels')
+            print(f'    ({round(lift.ref_x, 2)},{round(lift.ref_y, 2)},' +
+                  f'{round(lift.ref_yaw, 2)})')
+            print(f'    width:{round(lift.width, 2)} ' +
+                  f'depth:{round(lift.depth, 2)}')
+            print(f'    {len(lift.doors)} doors')
+            for door in lift.doors:
+                self.print_door(door)
+        print(f'{len(msg.levels)} levels')
         for level in msg.levels:
             print(f'  level: {level.name}')
+            print(f'    {level.elevation} elevation')
+            print(f'    {len(level.images)} images')
+            print(f'    {len(level.places)} places')
+            print(f'    {len(level.doors)} doors')
+            for door in level.doors:
+                self.print_door(door)
             print(f'    {len(level.nav_graphs)} navigation graphs')
             for nav_graph in level.nav_graphs:
                 print(f'    graph {nav_graph.name}:')
                 print(f'      {len(nav_graph.vertices)} vertices')
                 for vertex in nav_graph.vertices:
-                    print(f'        ({vertex.x}, {vertex.y}, "{vertex.name}")')
+                    print(f'        ({round(vertex.x, 2)}, ' +
+                          f'{round(vertex.y, 2)}, "{vertex.name}")')
 
                 print(f'      {len(nav_graph.edges)} edges')
                 for edge in nav_graph.edges: