Merge pull request #1 from ycheng517/handle-no-aruco

Handle no aruco
marcoesposito1988 · Apr 23, 2024 · 3e0fa5c · 3e0fa5c
2 parents c788191 + 20dea21
commit 3e0fa5c
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 4 deletions.
diff --git 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
@@ -23,12 +23,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 +74,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