Added wall graph to building map message

Signed-off-by: Matthew Booker <mattbooker97@gmail.com>

rmf_building_map_tools/building_map/level.py

@@ -487,3 +487,46 @@ def add_lanes_from(self, other):
             lane_out.start_idx += v_idx_offset
             lane_out.end_idx += v_idx_offset
             self.lanes.append(lane_out)
+
+    def generate_wall_graph(self):
+        """ Genereate a wall graph without unnecessary (non-wall) vertices"""
+
+        # first remap the vertices. Store both directions; we'll need them
+        next_idx = 0
+        vidx_to_mapped_idx = {}
+        mapped_idx_to_vidx = {}
+
+        for w in self.walls:
+            if w.start_idx not in vidx_to_mapped_idx:
+                vidx_to_mapped_idx[w.start_idx] = next_idx
+                mapped_idx_to_vidx[next_idx] = w.start_idx
+                next_idx += 1
+            if w.end_idx not in vidx_to_mapped_idx:
+                vidx_to_mapped_idx[w.end_idx] = next_idx
+                mapped_idx_to_vidx[next_idx] = w.end_idx
+                next_idx += 1
+
+        wall_data = {}
+        wall_data['vertices'] = []
+
+        for i in range(next_idx):
+            v = self.transformed_vertices[mapped_idx_to_vidx[i]]
+            p = {'name': v.name}
+
+            for param_name, param_value in v.params.items():
+                p[param_name] = param_value.value
+
+            wall_data['vertices'].append([v.x, v.y, p])
+
+        wall_data['walls'] = []
+
+        for w in self.walls:
+            v1 = self.vertices[w.start_idx]
+            v2 = self.vertices[w.end_idx]
+
+            start_idx = vidx_to_mapped_idx[w.start_idx]
+            end_idx = vidx_to_mapped_idx[w.end_idx]
+
+            wall_data['walls'].append([start_idx, end_idx, w.params])
+
+        return wall_data

rmf_building_map_tools/building_map_server/building_map_server.py

@@ -170,6 +170,44 @@ def level_msg(self, level):
                     ge.edge_type = GraphEdge.EDGE_TYPE_UNIDIRECTIONAL
                 graph_msg.edges.append(ge)
             msg.nav_graphs.append(graph_msg)
+
+
+        # Populate the wall graph
+        wall_graph = level.generate_wall_graph()
+        msg.wall_graph.name = "WallGraph"
+        for v in wall_graph['vertices']:
+            gn = GraphNode()
+            gn.x = v[0]
+            gn.y = v[1]
+            gn.name = v[2]['name']
+
+            # Ignore any other vertex params - not needed for wall graph
+            msg.wall_graph.vertices.append(gn)
+
+        for w in wall_graph['walls']:
+            ge = GraphEdge()
+            ge.edge_type = GraphEdge.EDGE_TYPE_BIDIRECTIONAL
+            ge.v1_idx = w[0]
+            ge.v2_idx = w[1]
+
+            for param_name, param_obj in w[2].items():
+                p = Param()
+                p.name = param_name
+                p.type = param_obj.type
+
+                if p.type == Param.TYPE_STRING:
+                    p.value_string = str(param_obj.value)
+                elif p.type == Param.TYPE_INT:
+                    p.value_int = int(param_obj.value)
+                elif p.type == Param.TYPE_DOUBLE:
+                    p.value_float = float(param_obj.value)
+                elif p.type == Param.TYPE_BOOL:
+                    p.value_bool = bool(param_obj.value)
+
+                ge.params.append(p)
+
+            msg.wall_graph.edges.append(ge)
+
         return msg
 
     def lift_msg(self, lift):