Added building map to api server

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

rmf_demos_panel/rmf_demos_panel/api_server.py

@@ -90,6 +90,13 @@ def robots():
     return robot_status
 
 
+@app.route('/building_map', methods=['GET'])
+def building_map():
+    building_map_data = jsonify(dispatcher_client.get_building_map_data())
+    logging.debug(f" ROS Time: {dispatcher_client.ros_time()} | \
+        building_map_data: {building_map_data}")
+    return building_map_data
+
 ###############################################################################
 
 def web_server_spin():

rmf_demos_panel/rmf_demos_panel/dispatcher_client.py

@@ -25,6 +25,7 @@
 from rclpy.qos import QoSProfile
 
 from rmf_task_msgs.srv import SubmitTask, GetTaskList, CancelTask
+from rmf_building_map_msgs.srv import GetBuildingMap
 from rmf_task_msgs.msg import TaskDescription, TaskSummary
 from rmf_task_msgs.msg import TaskType, Delivery, Loop
 from rmf_fleet_msgs.msg import FleetState, RobotMode
@@ -40,6 +41,9 @@ def __init__(self):
         self.cancel_task_srv = self.create_client(CancelTask, '/cancel_task')
         self.get_tasks_srv = self.create_client(GetTaskList, '/get_tasks')
 
+        self.get_building_map_srv = self.create_client(GetBuildingMap, '/get_building_map')
+
+
         qos_profile = QoSProfile(depth=20)
 
         # to show robot states
@@ -164,6 +168,26 @@ def get_robot_states(self):
             agg_robot_states = agg_robot_states + robots
         return agg_robot_states
 
+    def get_building_map_data(self):
+        """
+        Get building map data - This fn will trigger a ros srv call to acquire
+        building map data. Fn returns an object of tasks
+        """
+        req = GetBuildingMap.Request()
+        try:
+            future = self.get_building_map_srv.call_async(req)
+            rclpy.spin_until_future_complete(self, future, timeout_sec=0.5)
+            response = future.result()
+            if response is None:
+                self.get_logger().warn('/get_building_map srv call failed')
+            else:
+                # Return building map
+                map_data = self.__convert_building_map_msg(response.building_map)
+                return map_data
+        except Exception as e:
+            self.get_logger().error('Error! GetBuildingMap Srv failed %r' % (e,))
+        return {}  # empty dict
+
 ###############################################################################
 
     def __convert_task_status_msg(self, task_summaries, is_done=True):
@@ -214,6 +238,8 @@ def __convert_task_status_msg(self, task_summaries, is_done=True):
                     " --> " + desc.delivery.dropoff_place_name
             elif status["task_type"] == "Charging":
                 status["description"] = "Back to Charging Station"
+            else:
+                status["description"] = ""
 
             # Current hack to generate a progress percentage
             duration = abs(task.end_time.sec - task.start_time.sec)
@@ -330,3 +356,73 @@ def __convert_task_description(self, task_json):
         except Exception as ex:
             return None, str(ex)
         return task_desc, ""
+
+    def __convert_building_map_msg(self, msg):
+        map_data = {}
+
+        map_data["name"] = msg.name
+        map_data["levels"] = []
+
+        for level in msg.levels:
+            level_data = {}
+            level_data["name"] = level.name
+            level_data["elevation"] = level.elevation
+
+            # TODO: Images, places, doors?
+            level_data["nav_graphs"] = [self.__convert_graph_msg(msg) for msg in level.nav_graphs]
+            level_data["wall_graph"] = self.__convert_graph_msg(level.wall_graph)
+
+            map_data["levels"].append(level_data)
+
+        return map_data
+
+    def __convert_graph_msg(self, graph_msg):
+        graph_data = {}
+        graph_data["name"] = graph_msg.name
+        graph_data["vertices"] = []
+        graph_data["edges"] = []
+
+        for vertex in graph_msg.vertices:
+            vertex_data = {}
+            vertex_data["x"] = vertex.x
+            vertex_data["y"] = vertex.y
+            vertex_data["name"] = vertex.name
+
+            vertex_data["params"] = [self.__convert_param_msg(msg) for msg in vertex.params]
+
+            graph_data["vertices"].append(vertex_data)
+
+        for edge in graph_msg.edges:
+            edge_data = {}
+            edge_data["v1_idx"] = edge.v1_idx
+            edge_data["v2_idx"] = edge.v2_idx
+            edge_data["edge_type"] = edge.edge_type
+
+            edge_data["params"] = [self.__convert_param_msg(msg) for msg in edge.params]
+
+            graph_data["edges"].append(edge_data)
+
+        return graph_data
+
+    def __convert_param_msg(self, param_msg):
+        param_data = {}
+        param_data["name"] = param_msg.name
+
+        # TODO: how to handle TYPE_UNDEFINED?
+        if param_msg.type == param_msg.TYPE_STRING:
+            param_data["type"] = "string"
+            param_data["value"] = str(param_msg.value_string)
+        elif param_msg.type == param_msg.TYPE_INT:
+            param_data["type"] = "int"
+            param_data["value"] = str(param_msg.value_int)
+        elif param_msg.type == param_msg.TYPE_DOUBLE:
+            param_data["type"] = "double"
+            param_data["value"] = str(param_msg.value_float)
+        elif param_msg.type == param_msg.TYPE_BOOL:
+            param_data["type"] = "bool"
+            param_data["value"] = str(param_msg.value_bool)
+        else:
+            param_data["value"] = None
+
+        return param_data
+