Fix for issue #462 (#508)

* 🔨 Explicit cast waypoint name in building_map_server.py to avoid ROS 2 message Assertion Error.

Signed-off-by: Bey Hao Yun <[email protected]>

* 🔨 Refactored with new function, create_graph_node, in building_map_server.py.

Signed-off-by: Bey Hao Yun <[email protected]>

---------

Signed-off-by: Bey Hao Yun <[email protected]>
Co-authored-by: Luca Della Vedova <[email protected]>

rmf_building_map_tools/building_map_server/building_map_server.py

@@ -195,10 +195,7 @@ def level_msg(self, level):
             graph_msg.name = str(i)  # todo: someday, string names...
             print(f"graph {i} has {len(g['vertices'])} vertices")
             for v in g['vertices']:
-                gn = GraphNode()
-                gn.x = v[0]
-                gn.y = v[1]
-                gn.name = v[2]['name']
+                gn = self.create_graph_node(v[0], v[1], v[2]['name'])
 
                 # add specific params to builidng_map_msg
                 for str_param in ["dock_name",
@@ -245,10 +242,7 @@ def level_msg(self, level):
         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']
+            gn = self.create_graph_node(v[0], v[1], v[2]['name'])
 
             # Ignore any other vertex params - not needed for wall graph
             msg.wall_graph.vertices.append(gn)
@@ -324,6 +318,14 @@ def get_building_map(self, request, response):
         response.building_map = self.map_msg
         return response
 
+    def create_graph_node(self, wp_x, wp_y, wp_name):
+        gn = GraphNode()
+        gn.x = wp_x
+        gn.y = wp_y
+        gn.name = str(wp_name)
+
+        return gn
+
 
 def main():
     if len(sys.argv) > 1: