From cf9817b7731ed14dd7a6b68804bce25f999a8142 Mon Sep 17 00:00:00 2001
From: mhidalgo-bdai <144129882+mhidalgo-bdai@users.noreply.github.com>
Date: Wed, 27 Mar 2024 11:59:38 -0300
Subject: [PATCH] Add support for Node post-initialization (#81)

Signed-off-by: Michel Hidalgo <mhidalgo@theaiinstitute.com>
---
 .../bdai_ros2_wrappers/scope.py               | 12 +++++++++
 bdai_ros2_wrappers/test/test_node.py          | 27 ++++++++++++++++++-
 2 files changed, 38 insertions(+), 1 deletion(-)

diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/scope.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/scope.py
index 79ad517..159e8f3 100644
--- a/bdai_ros2_wrappers/bdai_ros2_wrappers/scope.py
+++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/scope.py
@@ -309,6 +309,10 @@ def managed(
     def load(self, factory: NodeFactoryCallable, *args: typing.Any, **kwargs: typing.Any) -> rclpy.node.Node:
         """Instantiates and loads a ROS 2 node.
 
+        If a __post_init__ method is defined by the instantiated ROS 2 node, it will be invoked
+        after the node is added to the scope executor. This allows for blocking calls during
+        initialization, provided the scope executor can serve them in the background.
+
         Args:
             factory: callable to instantiate a ROS 2 node.
             It is expected to accept `rclpy.node.Node` arguments.
@@ -333,6 +337,10 @@ def load(
     ) -> typing.List[rclpy.node.Node]:
         """Instantiates and loads a collection (or graph) of ROS 2 nodes.
 
+        For each ROS 2 node instantiated, if a __post_init__ method is defined it will be invoked
+        after the corresponding node has been added to the scope executor. This allows for blocking
+        calls during initialization, provided the scope executor can serve them in the background.
+
         Args:
             factory: callable to instantiate a collection of ROS 2 nodes.
             It is expected to accept `rclpy.node.Node` arguments.
@@ -366,10 +374,14 @@ def load(
                 graph = list(node_or_graph)
                 for node in graph:
                     self._executor.add_node(node)
+                    if hasattr(node, "__post_init__"):
+                        node.__post_init__()
                 self._graph.extend(graph)
                 return graph
             node = node_or_graph
             self._executor.add_node(node)
+            if hasattr(node, "__post_init__"):
+                node.__post_init__()
             self._graph.append(node)
             return node
 
diff --git a/bdai_ros2_wrappers/test/test_node.py b/bdai_ros2_wrappers/test/test_node.py
index df97259..51b643a 100644
--- a/bdai_ros2_wrappers/test/test_node.py
+++ b/bdai_ros2_wrappers/test/test_node.py
@@ -1,15 +1,17 @@
 # Copyright (c) 2024 Boston Dynamics AI Institute Inc.  All rights reserved.
 
 import threading
-from typing import Generator
+from typing import Any, Generator
 
 import pytest
 import rclpy
+from rcl_interfaces.srv import GetParameters
 from rclpy.context import Context
 from std_srvs.srv import Trigger
 
 from bdai_ros2_wrappers.executors import AutoScalingMultiThreadedExecutor
 from bdai_ros2_wrappers.node import Node
+from bdai_ros2_wrappers.scope import ROSAwareScope
 
 
 @pytest.fixture
@@ -67,3 +69,26 @@ def dummy_server_callback(_: Trigger.Request, response: Trigger.Response) -> Tri
         barrier.reset()
         executor.remove_node(node)
         executor.shutdown()
+
+
+def test_node_post_initialization(ros: ROSAwareScope) -> None:
+    """Asserts that Node post initialization is honored and that it affords blocking calls."""
+
+    class ComplexProxyNode(Node):
+        def __init__(self, *args: Any, remote_node_name: str, **kwargs: Any) -> None:
+            super().__init__(f"proxy_node_for_{remote_node_name}", *args, **kwargs)
+            self._get_remote_node_parameters_client = self.create_client(
+                GetParameters,
+                f"{remote_node_name}/get_parameters",
+            )
+
+        def __post_init__(self) -> None:
+            response = self._get_remote_node_parameters_client.call(
+                GetParameters.Request(names=["verbose"]),
+            )
+            self.verbose = response.values[0].bool_value
+
+    assert ros.node is not None
+    ros.node.declare_parameter("verbose", True)
+    with ros.managed(ComplexProxyNode, remote_node_name=ros.node.get_name()) as node:
+        assert node.verbose