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 <gary.bey@kabam.ai>

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

Signed-off-by: Bey Hao Yun <gary.bey@kabam.ai>

---------

Signed-off-by: Bey Hao Yun <gary.bey@kabam.ai>
Co-authored-by: Luca Della Vedova <lucadv@intrinsic.ai>

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: