Event based infrastructure state update

rmf_visualization_building_systems/rmf_visualization_building_systems/rmf_visualization_building_systems.py

@@ -34,44 +34,38 @@ def __init__(self, map_name):
         super().__init__('building_systems_visualizer')
         self.get_logger().info('Building systems visualizer started...')
 
-        qos = QoSProfile(
+        map_qos = QoSProfile(
             history=History.KEEP_LAST,
             depth=1,
             reliability=Reliability.RELIABLE,
             durability=Durability.TRANSIENT_LOCAL)
 
+        marker_qos = QoSProfile(
+            history=History.KEEP_LAST,
+            depth=5,
+            reliability=Reliability.RELIABLE,
+            durability=Durability.TRANSIENT_LOCAL)
+
         self.marker_pub = self.create_publisher(
             MarkerArray,
             'building_systems_markers',
-            qos_profile=qos)
+            qos_profile=marker_qos)
 
         self.create_subscription(
             BuildingMap,
             'map', self.map_cb,
-            qos_profile=qos)
-
-        self.create_subscription(
-            DoorState, 'door_states',
-            self.door_cb,
-            qos_profile=qos_profile_system_default)
-
-        self.create_subscription(
-            LiftState,
-            'lift_states',
-            self.lift_cb,
-            qos_profile=qos_profile_system_default)
+            qos_profile=map_qos)
 
         self.create_subscription(
             RvizParam,
             'rmf_visualization/parameters',
             self.param_cb,
             qos_profile=qos_profile_system_default)
 
-        self.initialized = False
-
         # data obtained from BuildingMap
         self.building_doors = {}
         self.building_lifts = {}
+        self.door_to_level_name = {}
 
         # name of the current map to display
         self.map_name = map_name
@@ -81,9 +75,36 @@ def __init__(self, map_name):
         self.door_states = {}
         self.door_states[self.map_name] = {}
 
-        # markers currently being displayed
+        # door markers currently being displayed
         self.active_markers = {}
 
+    def publish_rviz_markers(self, map_name):
+        marker_array = MarkerArray()
+        if map_name != self.map_name:
+            # deleting previously active door markers
+            for marker in self.active_markers.values():
+                marker.action = Marker.DELETE
+                marker_array.markers.append(marker)
+            self.active_markers = {}
+            if map_name in self.building_doors:
+                self.map_name = map_name
+            else:
+                self.marker_pub.publish(marker_array)
+                return
+
+        for msg in self.door_states[self.map_name].values():
+            marker = self.create_door_marker(msg)
+            text_marker = self.create_door_text_marker(msg)
+            self.active_markers[msg.door_name] = marker
+            self.active_markers[f'{msg.door_name}_text'] = text_marker
+            marker_array.markers.append(marker)
+            marker_array.markers.append(text_marker)
+            self.marker_pub.publish(marker_array)
+        for msg in self.lift_states.values():
+            marker_array.markers.append(self.create_lift_marker(msg))
+            marker_array.markers.append(self.create_lift_text_marker(msg))
+        self.marker_pub.publish(marker_array)
+
     def create_door_marker(self, msg):
         door_marker = Marker()
         door_marker.header.frame_id = 'map'
@@ -162,18 +183,15 @@ def create_door_text_marker(self, msg):
 
         return marker
 
-    def create_lift_marker(self, lift_name):
-        if lift_name not in self.building_lifts or \
-          lift_name not in self.lift_states:
-            return
+    def create_lift_marker(self, msg):
         marker = Marker()
         marker.header.frame_id = 'map'
-        marker.ns = lift_name
+        marker.ns = msg.lift_name
         marker.id = 1
         marker.type = Marker.CUBE
         marker.action = Marker.MODIFY
 
-        lift = self.building_lifts[lift_name]
+        lift = self.building_lifts[msg.lift_name]
         marker.pose.position.x = lift.ref_x
         marker.pose.position.y = lift.ref_y
         marker.pose.position.z = -0.5  # below lane markers
@@ -183,7 +201,7 @@ def create_lift_marker(self, lift_name):
         marker.scale.y = lift.depth
         marker.scale.z = 0.1
 
-        if self.lift_states[lift_name].door_state == 2:  # lift door open
+        if msg.door_state == 2:  # lift door open
             marker.color.r = 0.50
             marker.color.g = 0.70
             marker.color.b = 0.50
@@ -192,29 +210,26 @@ def create_lift_marker(self, lift_name):
             marker.color.g = 0.50
             marker.color.b = 0.65
 
-        if self.lift_states[lift_name].current_floor != self.map_name or \
-           self.lift_states[lift_name].motion_state == 1 or \
-           self.lift_states[lift_name].motion_state == 2:
+        if msg.current_floor != self.map_name or \
+           msg.motion_state == 1 or \
+           msg.motion_state == 2:
             # lift moving or not on current floor
             marker.color.a = 0.2
         else:
             marker.color.a = 0.8
 
         return marker
 
-    def create_lift_text_marker(self, lift_name):
-        if lift_name not in self.building_lifts or \
-          lift_name not in self.lift_states:
-            return
+    def create_lift_text_marker(self, msg):
         marker = Marker()
         marker.header.frame_id = 'map'
-        marker.ns = lift_name + '_text'
+        marker.ns = msg.lift_name + '_text'
         marker.id = 1
         marker.type = Marker.TEXT_VIEW_FACING
         marker.action = Marker.MODIFY
         marker.scale.z = 0.3
 
-        lift = self.building_lifts[lift_name]
+        lift = self.building_lifts[msg.lift_name]
 
         marker.pose.position.z = 0.0
         marker.pose.position.x = lift.ref_x + 0.4
@@ -225,18 +240,17 @@ def create_lift_text_marker(self, lift_name):
         marker.color.g = 1.0
         marker.color.b = 1.0
         marker.color.a = 1.0
-        lift_state = self.lift_states[lift_name]
-        marker.text = "Lift:" + lift_name
-        if self.lift_states[lift_name].motion_state != 0 and \
-           self.lift_states[lift_name].motion_state != 3:  # lift is moving
-            marker.text += "\n MovingTo:" + lift_state.destination_floor
+        marker.text = "Lift:" + msg.lift_name
+        if msg.motion_state != 0 and \
+           msg.motion_state != 3:  # lift is moving
+            marker.text += "\n MovingTo:" + msg.destination_floor
         else:
-            marker.text += "\n CurrentFloor:" + lift_state.current_floor
-        if lift_state.door_state == 0:
+            marker.text += "\n CurrentFloor:" + msg.current_floor
+        if msg.door_state == 0:
             marker.text += "\n DoorState:Closed"
-        elif lift_state.door_state == 1:
+        elif msg.door_state == 1:
             marker.text += "\n DoorState:Moving"
-        elif lift_state.door_state == 2:
+        elif msg.door_state == 2:
             marker.text += "\n DoorState:Open"
 
         return marker
@@ -254,79 +268,65 @@ def map_cb(self, msg):
             self.door_states[level.name] = {}
             for door in level.doors:
                 self.building_doors[level.name][door.name] = door
-        self.initialized = True
+                self.door_to_level_name[door.name] = level.name
+        self.init_subscriptions()
+
+    def init_subscriptions(self):
+        state_qos = QoSProfile(
+            history=History.KEEP_LAST,
+            depth=100,
+            reliability=Reliability.RELIABLE,
+            durability=Durability.VOLATILE)
+
+        self.create_subscription(
+            DoorState, 'door_states',
+            self.door_cb,
+            qos_profile=state_qos)
+
+        self.create_subscription(
+            LiftState,
+            'lift_states',
+            self.lift_cb,
+            qos_profile=state_qos)
 
     def door_cb(self, msg):
-        if not self.initialized or \
-          msg.door_name not in self.building_doors[self.map_name]:
+        if msg.door_name not in self.door_to_level_name:
             return
 
+        map_name = self.door_to_level_name[msg.door_name]
+
         publish_marker = False
-        door_state = self.door_states[self.map_name]
+        door_state = self.door_states[map_name]
         if msg.door_name not in door_state:
             door_state[msg.door_name] = msg
-            publish_marker = True
+            publish_marker = self.map_name == map_name
         else:
             if msg.current_mode.value != \
               door_state[msg.door_name].current_mode.value:
                 door_state[msg.door_name] = msg
-                publish_marker = True
+                publish_marker = self.map_name == map_name
 
         if publish_marker:
-            marker_array = MarkerArray()
-            marker = self.create_door_marker(msg)
-            text_marker = self.create_door_text_marker(msg)
-            if marker is not None:
-                marker_array.markers.append(marker)
-                self.active_markers[msg.door_name] = marker
-            if text_marker is not None:
-                marker_array.markers.append(text_marker)
-                self.active_markers[f'{msg.door_name}_text'] = text_marker
-            self.marker_pub.publish(marker_array)
+            self.publish_rviz_markers(self.map_name)
 
     def lift_cb(self, msg):
-        if not self.initialized:
+        if msg.lift_name not in self.building_lifts:
             return
-        publish_marker = False
+
         if msg.lift_name not in self.lift_states:
             self.lift_states[msg.lift_name] = msg
-            publish_marker = True
+            self.publish_rviz_markers(self.map_name)
         else:
             stored_state = self.lift_states[msg.lift_name]
             if msg.current_floor != stored_state.current_floor or \
                msg.motion_state != stored_state.motion_state or \
                msg.door_state != stored_state.door_state:
                 self.lift_states[msg.lift_name] = msg
-                publish_marker = True
+                self.publish_rviz_markers(self.map_name)
 
-        if publish_marker:
-            marker_array = MarkerArray()
-            marker = self.create_lift_marker(msg.lift_name)
-            text_marker = self.create_lift_text_marker(msg.lift_name)
-            if marker is not None:
-                marker_array.markers.append(marker)
-            if text_marker is not None:
-                marker_array.markers.append(text_marker)
-            self.marker_pub.publish(marker_array)
 
     def param_cb(self, msg):
-        if not self.initialized or \
-          msg.map_name not in self.building_doors:
-            return
-
-        self.map_name = msg.map_name
-        marker_array = MarkerArray()
-        # deleting previously avtive door markers
-        for name, marker in self.active_markers.items():
-            if marker is not None:
-                marker.action = Marker.DELETE
-                marker_array.markers.append(marker)
-        self.active_markers = {}
-        self.marker_pub.publish(marker_array)
-
-        # clearing door states and lift states so that markers can be updated
-        self.door_states[self.map_name] = {}
-        self.lift_states = {}
+        self.publish_rviz_markers(msg.map_name)
 
 
 def main(argv=sys.argv):

rmf_visualization_rviz2_plugins/src/DoorPanel.cpp

@@ -30,9 +30,11 @@ DoorPanel::DoorPanel(QWidget* parent)
 : rviz_common::Panel(parent),
   _requester_id(DoorPanelRequesterId)
 {
+  const auto default_qos =
+    rclcpp::SystemDefaultsQoS().durability_volatile().keep_last(100).reliable();
   _node = std::make_shared<rclcpp::Node>(_requester_id + "_node");
   _door_state_sub = _node->create_subscription<DoorState>(
-    DoorStateTopicName, 10, [&](DoorState::UniquePtr msg)
+    DoorStateTopicName, default_qos, [&](DoorState::UniquePtr msg)
     {
       door_state_callback(std::move(msg));
     });

rmf_visualization_rviz2_plugins/src/LiftPanel.cpp

@@ -30,16 +30,20 @@ LiftPanel::LiftPanel(QWidget* parent)
 : rviz_common::Panel(parent),
   _session_id(LiftPanelSessionId)
 {
+  const auto default_qos =
+    rclcpp::SystemDefaultsQoS().durability_volatile().keep_last(100).reliable();
+  const auto transient_qos =
+    rclcpp::SystemDefaultsQoS().transient_local().keep_last(100).reliable();
   _node = std::make_shared<rclcpp::Node>(_session_id + "_node");
   _lift_state_sub = _node->create_subscription<LiftState>(
-    LiftStateTopicName, 10, [&](LiftState::UniquePtr msg)
+    LiftStateTopicName, default_qos, [&](LiftState::UniquePtr msg)
     {
       lift_state_callback(std::move(msg));
     });
   _lift_request_pub = _node->create_publisher<LiftRequest>(
-    LiftRequestTopicName, rclcpp::QoS(10));
+    LiftRequestTopicName, transient_qos);
   _adapter_lift_request_pub = _node->create_publisher<LiftRequest>(
-    AdapterLiftRequestTopicName, rclcpp::QoS(10));
+    AdapterLiftRequestTopicName, transient_qos);
 
   create_layout();
   create_connections();