[watcher] bugfix broken stream cleanup in humble

Can no longer cleanup on pre-shutdown from the watcher. Do it
from the blackboard itself.

Related to #185.

py_trees_ros/blackboard.py

@@ -355,6 +355,11 @@ def post_tick_handler(self, visited_client_ids: typing.List[uuid.UUID]=None):
                         else:
                             msg.data = "{}".format(view.sub_blackboard)
                         view.publisher.publish(msg)
+                else:
+                    # Workaround for https://github.com/splintered-reality/py_trees_ros/issues/185
+                    # Might not be the best approach for wifi connections that can't be aware
+                    # they've been ruthlessly pruned.
+                    self._close_view(view)
 
         # manage the activity stream
         if py_trees.blackboard.Blackboard.activity_stream is not None:
@@ -371,16 +376,19 @@ def register_activity_stream_client(self):
     def unregister_activity_stream_client(self):
         self.activity_stream_clients -= 1
 
+    def _close_view(self, view: BlackboardView):
+        if view.with_activity_stream:
+            self.unregister_activity_stream_client()
+        view.shutdown()  # that node.destroy_publisher call makes havoc
+        self.views[:] = [v for v in self.views if v.topic_name != view.topic_name]
+
     def _close_service(self, request, response):
         response.result = False
         for view in self.views:
             if view.topic_name == request.topic_name:
-                if view.with_activity_stream:
-                    self.unregister_activity_stream_client()
-                view.shutdown()  # that node.destroy_publisher call makes havoc
+                self._close_view(view)
                 response.result = True
                 break
-        self.views[:] = [view for view in self.views if view.topic_name != request.topic_name]
         return response
 
     def _get_variables_service(self, unused_request, response):

py_trees_ros/programs/blackboard_watcher.py

@@ -24,11 +24,14 @@
 ##############################################################################
 
 import argparse
-import py_trees.console as console
-import py_trees_ros
+import functools
+import sys
+
 import rclpy
 import std_msgs.msg as std_msgs
-import sys
+
+import py_trees.console as console
+import py_trees_ros
 
 ##############################################################################
 # Classes
@@ -190,25 +193,34 @@ def main(command_line_args=sys.argv[1:]):
             # stream
             try:
                 rclpy.spin(blackboard_watcher.node)
-            except KeyboardInterrupt:
+            except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
                 pass
-            # close connection
-            request, client = blackboard_watcher.create_service_client('close')
-            request.topic_name = watcher_topic_name
-            future = client.call_async(request)
-            rclpy.spin_until_future_complete(blackboard_watcher.node, future)
-            if future.result() is None:
-                raise py_trees_ros.exceptions.ServiceError(
-                    "service call to close connection failed [{}]".format(future.exception())
-                )
+            finally:
+                # no pre-shutdown hooks from fumble
+                #   https://github.com/ros2/rclpy/issues/1077
+                # instead, letting the blackboard clean up it's own blackboard views
+                # when the subscriber count goes to zero
+                #   https://github.com/splintered-reality/py_trees_ros/issues/185
+                pass
+                # close connection
+                # request, client = blackboard_watcher.create_service_client('close')
+                # request.topic_name = watcher_topic_name
+                # future = client.call_async(request)
+                # rclpy.spin_until_future_complete(blackboard_watcher.node, future)
+                # if future.result() is None:
+                #     raise py_trees_ros.exceptions.ServiceError(
+                #         "service call to close connection failed [{}]".format(future.exception())
+                #     )
+
     # connection problems
     except (py_trees_ros.exceptions.NotReadyError,
             py_trees_ros.exceptions.ServiceError,
             py_trees_ros.exceptions.TimedOutError) as e:
         print(console.red + "\nERROR: {}".format(str(e)) + console.reset)
         result = 1
-    if subscription is not None:
-        blackboard_watcher.node.destroy_subscription(subscription)
-    blackboard_watcher.shutdown()
-    rclpy.shutdown()
-    sys.exit(result)
+    finally:
+        if subscription is not None:
+            blackboard_watcher.node.destroy_subscription(subscription)
+        blackboard_watcher.shutdown()
+        rclpy.try_shutdown()
+        sys.exit(result)