Add Reload Tree button to reload current namespace

.gitignore

@@ -2,3 +2,4 @@ __pycache__
 *doc/html
 *egg-info
 *.md.html
+**.vscode

py_trees_ros_viewer/backend.py

@@ -95,6 +95,7 @@ def __init__(
 
         self.topic_name = None
         self.subscriber = None
+        self.connected = False
 
         self.services = {
             'open': None,
@@ -119,6 +120,8 @@ def __init__(
         }
         # create service clients
         self.services["open"] = self.create_service_client(key="open")
+        if self.services["open"] is None:
+            return  # Prevent from waiting more time for other services and exit earlier.
         self.services["close"] = self.create_service_client(key="close")
         self.services["reconfigure"] = self.create_service_client(key="reconfigure")
 
@@ -160,15 +163,23 @@ def _connect_on_init(self, timeout_sec=1.0):
         request.parameters.snapshot_period = self.parameters.snapshot_period
         console.logdebug("establishing a snapshot stream connection [{}][backend]".format(self.namespace))
         future = self.services["open"].call_async(request)
-        rclpy.spin_until_future_complete(self.node, future)
+        start_time = time.monotonic()
+        while not future.done():
+            elapsed_time = time.monotonic() - start_time
+            if elapsed_time > timeout_sec:
+                console.logerror(f"Timed out waiting to open a connection")
+                return
+            rclpy.spin_once(self.node, timeout_sec=0.1)
         response = future.result()
         self.topic_name = response.topic_name
         # connect to a snapshot stream
         start_time = time.monotonic()
         while True:
             elapsed_time = time.monotonic() - start_time
             if elapsed_time > timeout_sec:
-                raise exceptions.TimedOutError("timed out waiting for a snapshot stream publisher [{}]".format(self.topic_name))
+                console.logerror(f"Timed out waiting for a snapshot stream publisher [{self.topic_name}]")
+                self.connected = False
+                return
             if self.node.count_publishers(self.topic_name) > 0:
                 break
             time.sleep(0.1)
@@ -178,10 +189,11 @@ def _connect_on_init(self, timeout_sec=1.0):
             callback=self.callback,
             qos_profile=utilities.qos_profile_latched()
         )
+        self.connected = True
         console.logdebug("  ...ok [backend]")
 
     def shutdown(self):
-        if rclpy.ok() and self.services["close"] is not None:
+        if rclpy.ok() and self.services["close"] is not None and self.topic_name is not None:
             request = self.service_types["close"].Request()
             request.topic_name = self.topic_name
             future = self.services["close"].call_async(request)
@@ -197,6 +209,9 @@ def create_service_client(self, key: str):
 
         Args:
             key: one of 'open', 'close'.
+
+        Returns:
+            A client if successful, else None.
 
         Raises:
             :class:`~py_trees_ros.exceptions.NotReadyError`: if setup() wasn't called to identify the relevant services to connect to.
@@ -213,9 +228,8 @@ def create_service_client(self, key: str):
         )
         # hardcoding timeouts will get us into trouble
         if not client.wait_for_service(timeout_sec=3.0):
-            raise exceptions.TimedOutError(
-                "timed out waiting for {}".format(self.service_names['close'])
-            )
+            console.logdebug(f"Timed out waiting for {self.service_names['close']}")
+            return None
         return client
 
 ##############################################################################
@@ -253,10 +267,12 @@ def spin(self):
                 if self.parameters != old_parameters:
                     if self.snapshot_stream is not None:
                         self.snapshot_stream.reconfigure(self.parameters)
-                old_parameters = copy.copy(self.parameters)
+                    old_parameters = copy.copy(self.parameters)
                 if self.enqueued_connection_request_namespace is not None:
                     self.connect(self.enqueued_connection_request_namespace)
-                    self.enqueued_connection_request_namespace = None
+                    # If connection failed, keep retrying with the latest enqueued namespace.
+                    if self.snapshot_stream.connected:
+                        self.enqueued_connection_request_namespace = None
             rclpy.spin_once(self.node, timeout_sec=0.1)
         if self.snapshot_stream is not None:
             self.snapshot_stream.shutdown()

py_trees_ros_viewer/main_window.py

@@ -60,8 +60,10 @@ def on_discovered_namespaces_changed(self, discovered_namespaces):
         console.logdebug("discovered namespaces changed callback {}[window]".format(discovered_namespaces))
         if (discovered_namespaces):
             self.ui.send_button.setEnabled(False)
+            self.ui.reload_button.setEnabled(True)
         else:
             self.ui.send_button.setEnabled(True)
+            self.ui.reload_button.setEnabled(False)
 
         discovered_namespaces.sort()
 
@@ -96,6 +98,7 @@ def onLoadFinished(self):
         console.logdebug("web page loaded [window]")
         self.web_app_loaded = True
         self.ui.send_button.setEnabled((self.ui.topic_combo_box.currentIndex() == -1))
+        self.ui.reload_button.setEnabled(False)
         self.ui.screenshot_button.setEnabled(True)
         self.ui.blackboard_activity_checkbox.setEnabled(True)
         self.ui.blackboard_data_checkbox.setEnabled(True)

py_trees_ros_viewer/main_window.ui

@@ -61,6 +61,16 @@
          </property>
         </widget>
        </item>
+       <item>
+        <widget class="QPushButton" name="reload_button">
+         <property name="enabled">
+          <bool>false</bool>
+         </property>
+         <property name="text">
+          <string>Reload Tree</string>
+         </property>
+        </widget>
+       </item>
        <item>
         <spacer name="horizontalSpacer">
          <property name="orientation">

py_trees_ros_viewer/main_window_ui.py

@@ -42,6 +42,10 @@ def setupUi(self, MainWindow):
         self.send_button.setEnabled(False)
         self.send_button.setObjectName("send_button")
         self.horizontalLayout.addWidget(self.send_button)
+        self.reload_button = QtWidgets.QPushButton(self.tools_frame)
+        self.reload_button.setEnabled(False)
+        self.reload_button.setObjectName("reload_button")
+        self.horizontalLayout.addWidget(self.reload_button)
         spacerItem = QtWidgets.QSpacerItem(186, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum)
         self.horizontalLayout.addItem(spacerItem)
         self.blackboard_data_checkbox = QtWidgets.QCheckBox(self.tools_frame)
@@ -91,6 +95,7 @@ def retranslateUi(self, MainWindow):
         MainWindow.setWindowTitle(_translate("MainWindow", "PyTrees Viewer"))
         self.label.setText(_translate("MainWindow", "Namespace"))
         self.send_button.setText(_translate("MainWindow", "Send Demo Tree"))
+        self.reload_button.setText(_translate("MainWindow", "Reload Tree"))
         self.blackboard_data_checkbox.setText(_translate("MainWindow", "Blackboard Data"))
         self.blackboard_activity_checkbox.setText(_translate("MainWindow", "Blackboard Activity"))
         self.periodic_checkbox.setText(_translate("MainWindow", "Periodic"))

py_trees_ros_viewer/viewer.py

@@ -139,6 +139,15 @@ def on_connection_request(backend, namespace: str):
     with backend.lock:
         backend.enqueued_connection_request_namespace = namespace
 
+def on_connection_reload_request(backend, window):
+    """
+    Enqueue a connection request.
+
+    Cannot directly make the connection here since this is invariably the qt thread.
+    """
+    namespace = window.ui.topic_combo_box.currentText()
+    on_connection_request(backend, namespace)
+
 ##############################################################################
 # Main
 ##############################################################################
@@ -184,6 +193,13 @@ def on_shutdown(unused_signal, unused_frame):
             demo_trees
         )
     )
+    window.ui.reload_button.clicked.connect(
+        functools.partial(
+            on_connection_reload_request,
+            backend,
+            window,
+        )
+    )
     window.ui.screenshot_button.clicked.connect(
         functools.partial(
             capture_screenshot,