diff --git a/rmf_visualization_building_systems/rmf_visualization_building_systems/rmf_visualization_building_systems.py b/rmf_visualization_building_systems/rmf_visualization_building_systems/rmf_visualization_building_systems.py index ad6c1a8..734ddac 100644 --- a/rmf_visualization_building_systems/rmf_visualization_building_systems/rmf_visualization_building_systems.py +++ b/rmf_visualization_building_systems/rmf_visualization_building_systems/rmf_visualization_building_systems.py @@ -34,32 +34,27 @@ 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, @@ -67,11 +62,10 @@ def __init__(self, map_name): 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,9 +210,9 @@ 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: @@ -202,19 +220,16 @@ def create_lift_marker(self, lift_name): 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): diff --git a/rmf_visualization_rviz2_plugins/src/DoorPanel.cpp b/rmf_visualization_rviz2_plugins/src/DoorPanel.cpp index cc88140..f56708b 100644 --- a/rmf_visualization_rviz2_plugins/src/DoorPanel.cpp +++ b/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(_requester_id + "_node"); _door_state_sub = _node->create_subscription( - DoorStateTopicName, 10, [&](DoorState::UniquePtr msg) + DoorStateTopicName, default_qos, [&](DoorState::UniquePtr msg) { door_state_callback(std::move(msg)); }); diff --git a/rmf_visualization_rviz2_plugins/src/LiftPanel.cpp b/rmf_visualization_rviz2_plugins/src/LiftPanel.cpp index 7779c17..1fe8a4d 100644 --- a/rmf_visualization_rviz2_plugins/src/LiftPanel.cpp +++ b/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(_session_id + "_node"); _lift_state_sub = _node->create_subscription( - LiftStateTopicName, 10, [&](LiftState::UniquePtr msg) + LiftStateTopicName, default_qos, [&](LiftState::UniquePtr msg) { lift_state_callback(std::move(msg)); }); _lift_request_pub = _node->create_publisher( - LiftRequestTopicName, rclcpp::QoS(10)); + LiftRequestTopicName, transient_qos); _adapter_lift_request_pub = _node->create_publisher( - AdapterLiftRequestTopicName, rclcpp::QoS(10)); + AdapterLiftRequestTopicName, transient_qos); create_layout(); create_connections();