From 83e98517b7f459d0d711ac35004e65b8e30272ea Mon Sep 17 00:00:00 2001
From: Yifei Cheng <ycheng517@gmail.com>
Date: Mon, 22 Apr 2024 20:28:29 -0400
Subject: [PATCH 1/2] wait for aruco marker to be present before init

---
 .../easy_handeye2/handeye_sampler.py          |  2 +-
 easy_handeye2/easy_handeye2/handeye_server.py | 26 +++++++++++++++----
 2 files changed, 22 insertions(+), 6 deletions(-)

diff --git a/easy_handeye2/easy_handeye2/handeye_sampler.py b/easy_handeye2/easy_handeye2/handeye_sampler.py
index 3f8fbbc..873bb78 100644
--- a/easy_handeye2/easy_handeye2/handeye_sampler.py
+++ b/easy_handeye2/easy_handeye2/handeye_sampler.py
@@ -84,7 +84,7 @@ def wait_for_tf_init(self) -> bool:
         self.node.get_logger().info('All expected transforms are available on tf; ready to take samples')
         return True
 
-    def _get_transforms(self, time: Optional[rclpy.time.Time] = None) -> easy_handeye2_msgs.msg.Sample:
+    def _get_transforms(self, time: Optional[rclpy.time.Time] = None) -> Sample:
         """
         Samples the transforms at the given time.
         """
diff --git a/easy_handeye2/easy_handeye2/handeye_server.py b/easy_handeye2/easy_handeye2/handeye_server.py
index f55f3df..8b421c8 100644
--- a/easy_handeye2/easy_handeye2/handeye_server.py
+++ b/easy_handeye2/easy_handeye2/handeye_server.py
@@ -3,9 +3,7 @@
 import rclpy
 import std_msgs
 import easy_handeye2_msgs as ehm
-from easy_handeye2_msgs import msg, srv
 from rclpy.executors import ExternalShutdownException
-from std_msgs import msg
 
 import easy_handeye2 as hec
 from easy_handeye2.handeye_calibration import save_calibration, HandeyeCalibrationParametersProvider
@@ -23,12 +21,31 @@ def __init__(self):
         self.get_logger().info(f'Read parameters for calibration "{self.parameters.name}"')
 
         self.sampler = HandeyeSampler(self, handeye_parameters=self.parameters)
-        self.sampler.wait_for_tf_init()
+        self.setup_timer = self.create_timer(2.0, self.setup_services_and_topics)
 
         self.calibration_backends = {'OpenCV': HandeyeCalibrationBackendOpenCV()}
         self.calibration_algorithm = 'OpenCV/Tsai-Lenz'
 
         # setup calibration services and topics
+        self.list_algorithms_service = None
+        self.set_algorithm_service = None
+        self.get_current_transforms_service = None
+        self.get_sample_list_service = None
+        self.take_sample_service = None
+        self.remove_sample_service = None
+        self.save_samples_service = None
+        self.load_samples_service = None
+        self.compute_calibration_service = None
+        self.save_calibration_service = None
+        self.take_sample_topic = None
+        self.remove_last_sample_topic = None
+
+        self.last_calibration = None
+
+    def setup_services_and_topics(self):
+        if not self.sampler.wait_for_tf_init():
+            self.get_logger().warn('Waiting for TF initialization...')
+            return
 
         self.list_algorithms_service = self.create_service(ehm.srv.ListAlgorithms, hec.LIST_ALGORITHMS_TOPIC,
                                                            self.list_algorithms)
@@ -55,8 +72,7 @@ def __init__(self):
                                                           10)
         self.remove_last_sample_topic = self.create_subscription(std_msgs.msg.Empty, hec.REMOVE_SAMPLE_TOPIC,
                                                                   self.remove_last_sample, 10)
-
-        self.last_calibration = None
+        self.setup_timer.cancel()
 
     # algorithm
 

From 20dea21c0115ff783e28d4ece6cc08c09ec59cc1 Mon Sep 17 00:00:00 2001
From: Yifei Cheng <ycheng517@gmail.com>
Date: Mon, 22 Apr 2024 22:00:48 -0400
Subject: [PATCH 2/2] fix import statements

---
 easy_handeye2/easy_handeye2/handeye_server.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/easy_handeye2/easy_handeye2/handeye_server.py b/easy_handeye2/easy_handeye2/handeye_server.py
index 8b421c8..a461b00 100644
--- a/easy_handeye2/easy_handeye2/handeye_server.py
+++ b/easy_handeye2/easy_handeye2/handeye_server.py
@@ -3,7 +3,9 @@
 import rclpy
 import std_msgs
 import easy_handeye2_msgs as ehm
+from easy_handeye2_msgs import msg, srv
 from rclpy.executors import ExternalShutdownException
+from std_msgs import msg
 
 import easy_handeye2 as hec
 from easy_handeye2.handeye_calibration import save_calibration, HandeyeCalibrationParametersProvider