From 19f95cdb3f0e658cf1bd816858023192ce2102aa Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 23 Jan 2024 11:14:11 +0800 Subject: [PATCH 1/7] First draft of event based state update Signed-off-by: Luca Della Vedova --- .../rmf_visualization_building_systems.py | 171 ++++++++++-------- 1 file changed, 98 insertions(+), 73 deletions(-) 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 d8e691f..07e146b 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 @@ -1,6 +1,8 @@ import rclpy from rclpy.node import Node +from rclpy.event_handler import PublisherEventCallbacks +from rclpy.event_handler import QoSPublisherMatchedInfo from rclpy.qos import qos_profile_system_default from rclpy.qos import QoSProfile from rclpy.qos import QoSHistoryPolicy as History @@ -34,32 +36,30 @@ def __init__(self, map_name): super().__init__('building_systems_visualizer') self.get_logger().info('Building systems visualizer started...') - qos = QoSProfile( - history=History.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + map_qos = QoSProfile( + history=History.KEEP_LAST, depth=1, - reliability=Reliability.RMW_QOS_POLICY_RELIABILITY_RELIABLE, - durability=Durability.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) + reliability=Reliability.RELIABLE, + durability=Durability.TRANSIENT_LOCAL) + + pub_event_callback = \ + PublisherEventCallbacks(matched=self.__pub_matched_event_callback) + 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, + event_callbacks=pub_event_callback) 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 +67,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 +80,20 @@ def __init__(self, map_name): self.door_states = {} self.door_states[self.map_name] = {} + # Tracks when all the necessary nodes are ready + self.map_received = False + self.rviz_ready = False + self.subscriptions_initialized = False + # markers currently being displayed self.active_markers = {} + def __pub_matched_event_callback(self, info: QoSPublisherMatchedInfo): + if info.current_count_change > 0: + self.rviz_ready = True + if self.map_received is True: + self.init_subscriptions() + def create_door_marker(self, msg): door_marker = Marker() door_marker.header.frame_id = 'map' @@ -162,18 +172,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 +190,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 +199,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 +209,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 +229,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,38 +257,61 @@ 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.map_received = True + if self.rviz_ready is True: + self.init_subscriptions() + + def init_subscriptions(self): + if self.subscriptions_initialized is True: + return + 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) + self.subscriptions_initialized = True 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 + marker_array.markers.append(marker) + marker_array.markers.append(text_marker) + self.active_markers[msg.door_name] = marker + self.active_markers[f'{msg.door_name}_text'] = text_marker self.marker_pub.publish(marker_array) 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: @@ -301,32 +327,31 @@ def lift_cb(self, msg): 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) + 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 param_cb(self, msg): - if not self.initialized or \ - msg.map_name not in self.building_doors: + if msg.map_name not in self.building_doors: return self.map_name = msg.map_name marker_array = MarkerArray() - # deleting previously avtive door markers + # deleting previously active door markers for name, marker in self.active_markers.items(): - if marker is not None: - marker.action = Marker.DELETE - marker_array.markers.append(marker) + 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 = {} + # Send all the markers for this level + for msg in self.door_states[self.map_name].values(): + marker = self.create_door_marker(msg) + text_marker = self.create_door_text_marker(msg) + marker_array.markers.append(marker) + self.active_markers[msg.door_name] = marker + marker_array.markers.append(text_marker) + self.active_markers[f'{msg.door_name}_text'] = text_marker + self.marker_pub.publish(marker_array) def main(argv=sys.argv): From 5b302d4882d059f80bf8af95712eb8a63e5a8c5e Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 15 Mar 2024 17:43:45 +0800 Subject: [PATCH 2/7] Fix publishing of building markers on non existing level Signed-off-by: Luca Della Vedova --- .../rmf_visualization_building_systems.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) 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 07e146b..6cb730c 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 @@ -332,10 +332,6 @@ def lift_cb(self, msg): self.marker_pub.publish(marker_array) def param_cb(self, msg): - if msg.map_name not in self.building_doors: - return - - self.map_name = msg.map_name marker_array = MarkerArray() # deleting previously active door markers for name, marker in self.active_markers.items(): @@ -343,6 +339,12 @@ def param_cb(self, msg): marker_array.markers.append(marker) self.active_markers = {} + if msg.map_name not in self.building_doors: + self.marker_pub.publish(marker_array) + return + + self.map_name = msg.map_name + # Send all the markers for this level for msg in self.door_states[self.map_name].values(): marker = self.create_door_marker(msg) From fd971c4bcfc77cdbe76538c0514dcfd88968b13c Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 15 Mar 2024 17:56:20 +0800 Subject: [PATCH 3/7] Fix QoS for door and lift states Signed-off-by: Luca Della Vedova --- rmf_visualization_rviz2_plugins/src/DoorPanel.cpp | 4 +++- rmf_visualization_rviz2_plugins/src/LiftPanel.cpp | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) 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 3b10ce8..2b1c8d4 100644 --- a/rmf_visualization_rviz2_plugins/src/LiftPanel.cpp +++ b/rmf_visualization_rviz2_plugins/src/LiftPanel.cpp @@ -30,9 +30,11 @@ LiftPanel::LiftPanel(QWidget* parent) : rviz_common::Panel(parent), _session_id(LiftPanelSessionId) { + const auto default_qos = + rclcpp::SystemDefaultsQoS().durability_volatile().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)); }); From 83a404b14be41ff2f45119e2a3e0a1d409134255 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 21 Mar 2024 18:12:38 +0800 Subject: [PATCH 4/7] Always publish whole state to rviz Signed-off-by: Luca Della Vedova --- .../rmf_visualization_building_systems.py | 92 +++++++------------ 1 file changed, 33 insertions(+), 59 deletions(-) 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 6cb730c..0fc3e6b 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 @@ -42,8 +42,6 @@ def __init__(self, map_name): reliability=Reliability.RELIABLE, durability=Durability.TRANSIENT_LOCAL) - pub_event_callback = \ - PublisherEventCallbacks(matched=self.__pub_matched_event_callback) marker_qos = QoSProfile( history=History.KEEP_LAST, depth=5, @@ -53,8 +51,7 @@ def __init__(self, map_name): self.marker_pub = self.create_publisher( MarkerArray, 'building_systems_markers', - qos_profile=marker_qos, - event_callbacks=pub_event_callback) + qos_profile=marker_qos) self.create_subscription( BuildingMap, @@ -80,19 +77,35 @@ def __init__(self, map_name): self.door_states = {} self.door_states[self.map_name] = {} - # Tracks when all the necessary nodes are ready - self.map_received = False - self.rviz_ready = False - self.subscriptions_initialized = False - - # markers currently being displayed + # door markers currently being displayed self.active_markers = {} - def __pub_matched_event_callback(self, info: QoSPublisherMatchedInfo): - if info.current_count_change > 0: - self.rviz_ready = True - if self.map_received is True: - self.init_subscriptions() + 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() @@ -258,13 +271,9 @@ def map_cb(self, msg): for door in level.doors: self.building_doors[level.name][door.name] = door self.door_to_level_name[door.name] = level.name - self.map_received = True - if self.rviz_ready is True: - self.init_subscriptions() + self.init_subscriptions() def init_subscriptions(self): - if self.subscriptions_initialized is True: - return state_qos = QoSProfile( history=History.KEEP_LAST, depth=100, @@ -281,7 +290,6 @@ def init_subscriptions(self): 'lift_states', self.lift_cb, qos_profile=state_qos) - self.subscriptions_initialized = True def door_cb(self, msg): if msg.door_name not in self.door_to_level_name: @@ -301,59 +309,25 @@ def door_cb(self, msg): 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) - marker_array.markers.append(marker) - marker_array.markers.append(text_marker) - self.active_markers[msg.door_name] = 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 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 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_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 param_cb(self, msg): - marker_array = MarkerArray() - # deleting previously active door markers - for name, marker in self.active_markers.items(): - marker.action = Marker.DELETE - marker_array.markers.append(marker) - self.active_markers = {} - - if msg.map_name not in self.building_doors: - self.marker_pub.publish(marker_array) - return - - self.map_name = msg.map_name - - # Send all the markers for this level - for msg in self.door_states[self.map_name].values(): - marker = self.create_door_marker(msg) - text_marker = self.create_door_text_marker(msg) - marker_array.markers.append(marker) - self.active_markers[msg.door_name] = marker - 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(msg.map_name) def main(argv=sys.argv): From 52667a237f9e0317d326c513ac5ab1fb56314485 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 21 Mar 2024 18:13:49 +0800 Subject: [PATCH 5/7] Remove unused imports Signed-off-by: Luca Della Vedova --- .../rmf_visualization_building_systems.py | 2 -- 1 file changed, 2 deletions(-) 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 0fc3e6b..545e771 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 @@ -1,8 +1,6 @@ import rclpy from rclpy.node import Node -from rclpy.event_handler import PublisherEventCallbacks -from rclpy.event_handler import QoSPublisherMatchedInfo from rclpy.qos import qos_profile_system_default from rclpy.qos import QoSProfile from rclpy.qos import QoSHistoryPolicy as History From 2b96b4361120c597db2c92a058890893bcdd8aeb Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 2 Apr 2024 07:57:59 +0800 Subject: [PATCH 6/7] Add missing lift state publish Signed-off-by: Luca Della Vedova --- .../rmf_visualization_building_systems.py | 1 + 1 file changed, 1 insertion(+) 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 545e771..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 @@ -315,6 +315,7 @@ def lift_cb(self, msg): if msg.lift_name not in self.lift_states: self.lift_states[msg.lift_name] = msg + 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 \ From 1f331cd7e9c3e92fddc1a6bb7fb7ae5126091007 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 5 Apr 2024 16:50:34 +0800 Subject: [PATCH 7/7] Fix lift request QoS to be transient local Signed-off-by: Luca Della Vedova --- rmf_visualization_rviz2_plugins/src/LiftPanel.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rmf_visualization_rviz2_plugins/src/LiftPanel.cpp b/rmf_visualization_rviz2_plugins/src/LiftPanel.cpp index 4c6bbb1..1fe8a4d 100644 --- a/rmf_visualization_rviz2_plugins/src/LiftPanel.cpp +++ b/rmf_visualization_rviz2_plugins/src/LiftPanel.cpp @@ -32,6 +32,8 @@ LiftPanel::LiftPanel(QWidget* parent) { 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, default_qos, [&](LiftState::UniquePtr msg) @@ -39,9 +41,9 @@ LiftPanel::LiftPanel(QWidget* parent) 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();