Fix perception modules (#68)

* Update perception modules with new API

* Fix bug in apriltag module

* Add interbotix_is_up function to robot module

* Align slate module with API wait_until_future_complete

* Lint

interbotix_common_toolbox/interbotix_common_modules/interbotix_common_modules/common_robot/robot.py

@@ -59,19 +59,19 @@ def wait_until_future_complete(self, future: Future, timeout_sec: float = None)
         :param future: future to complete
         :param timeout_sec: timeout in seconds after which this will continue. if `None` or
             negative, will wait infinitely. defaults to `None`.
-
-        :note: this function assumes that rclpy is spinning in a separate thread. if this is not
-            the case, calling this function with an infinite timeout may block forever.
         """
-        rate = self.create_rate(20.0)
-        if timeout_sec is None or timeout_sec < 0:
-            while not future.done():
-                rate.sleep()
+        if interbotix_is_up():
+            rate = self.create_rate(20.0)
+            if timeout_sec is None or timeout_sec < 0:
+                while not future.done():
+                    rate.sleep()
+            else:
+                start = self.get_clock().now()
+                timeout_duration = Duration(seconds=timeout_sec)
+                while not future.done() and self.get_clock().now() - start < timeout_duration:
+                    rate.sleep()
         else:
-            start = self.get_clock().now()
-            timeout_duration = Duration(seconds=timeout_sec)
-            while not future.done() and self.get_clock().now() - start < timeout_duration:
-                rate.sleep()
+            rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec)
 
     def logdebug(self, message: str, **kwargs):
         self.get_logger().debug(message, **kwargs)
@@ -82,12 +82,17 @@ def loginfo(self, message: str, **kwargs):
     def logwarn(self, message: str, **kwargs):
         self.get_logger().warning(message, **kwargs)
 
+    def logerror(self, message: str, **kwargs):
+        self.get_logger().error(message, **kwargs)
+
     def logfatal(self, message: str, **kwargs):
         self.get_logger().fatal(message, **kwargs)
 
 
 def __start(node: InterbotixRobotNode, daemon: bool = True) -> None:
     """Start a background thread that spins the rclpy global executor."""
+    global __interbotix_is_up
+    __interbotix_is_up = True
     global __interbotix_execution_thread
     __interbotix_execution_thread = Thread(target=rclpy.spin, args=(node,), daemon=daemon)
     __interbotix_execution_thread.start()
@@ -131,6 +136,8 @@ def robot_shutdown(node: Optional[InterbotixRobotNode] = None) -> None:
     node.destroy_node()
     rclpy.shutdown()
     __interbotix_execution_thread.join()
+    if '__interbotix_is_up' in globals():
+        __interbotix_is_up = False  # noqa: F841
 
 
 def create_interbotix_global_node(
@@ -169,3 +176,15 @@ def create_interbotix_global_node(
 def get_interbotix_global_node() -> InterbotixRobotNode:
     """Return the global Interbotix node."""
     return __interbotix_global_node
+
+
+def interbotix_is_up() -> bool:
+    """
+    Return the state of the Interbotix Python-ROS API.
+
+    :return: `True` if the API is up, `False` otherwise
+    """
+    if '__interbotix_is_up' in globals():
+        return __interbotix_is_up
+    else:
+        return False

interbotix_perception_toolbox/interbotix_perception_modules/interbotix_perception_modules/apriltag.py

@@ -1,4 +1,4 @@
-# Copyright 2022 Trossen Robotics
+# Copyright 2024 Trossen Robotics
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
@@ -32,14 +32,14 @@
 from apriltag_ros.msg import AprilTagDetection, AprilTagDetectionArray
 from apriltag_ros.srv import AnalyzeSingleImage
 from geometry_msgs.msg import Pose, TransformStamped
+from interbotix_common_modules.common_robot.robot import InterbotixRobotNode
 from interbotix_perception_msgs.srv import SnapPicture
 import rclpy
-from rclpy.node import Node
 from rclpy.time import Time
 from sensor_msgs.msg import CameraInfo
 
 
-class InterbotixAprilTagInterface(Node):
+class InterbotixAprilTagInterface:
     """Python API to snap the pose of an AprilTag."""
 
     valid_tags = [5, 413, 820, 875, 1050]
@@ -50,24 +50,19 @@ def __init__(
         apriltag_ns: str = 'apriltag',
         full_img_get_path: str = '/tmp/get_image.png',
         full_img_save_path: str = '/tmp/save_image.png',
-        node_inf: Node = None,
-        args=None
+        node_inf: InterbotixRobotNode = None,
+        args=None,
     ) -> None:
         """
         Construct the InterbotixAprilTagInterface node.
 
         :param apriltag_ns: the namespace that the AprilTag node and other related parameters are
             in
-        :param node_inf: reference to the rclpy.node.Node on which to build this interface. Leave
-            as `None` to let this interface serve as its own Node
+        :param node_inf: reference to the InterbotixRobotNode on which to build this interface.
         """
         if node_inf is None:
-            # start apriltag interface node
-            rclpy.init(args=args)
-            super().__init__(f"{apriltag_ns.strip('/')}_interface")
-            self.node_inf = self
-        else:
-            self.node_inf = node_inf
+            raise NotImplementedError('Passing node_inf as None is not implemented.')
+        self.node_inf = node_inf
 
         self.image_frame_id: str = None
 
@@ -87,12 +82,12 @@ def __init__(
             f'/{apriltag_ns}/single_image_tag_detection'
         )
         while (not self.srv_snap_picture.wait_for_service(timeout_sec=1) and rclpy.ok()):
-            self.node_inf.get_logger().warn(
+            self.node_inf.logwarn(
                 f"Service '/{apriltag_ns}/snap_picture' not yet ready.",
                 throttle_duration_sec=5,
             )
         while (not self.srv_analyze_image.wait_for_service(timeout_sec=1) and rclpy.ok()):
-            self.node_inf.get_logger().warn(
+            self.node_inf.logwarn(
                 f"Service '/{apriltag_ns}/single_image_tag_detection' not yet ready.",
                 throttle_duration_sec=5,
             )
@@ -116,17 +111,18 @@ def __init__(
         # wait to receive camera info (means that we are properly subscribed to the topic)
         rclpy.spin_once(self.node_inf, timeout_sec=3)  # spin once to process any callbacks
         while (self.request.camera_info == CameraInfo() and rclpy.ok()):
-            self.get_logger().warn(
+            self.node_inf.logwarn(
                 (
                     f"No CameraInfo messages received yet on topic '{camera_info_topic}' or "
-                    'CameraInfo message is empty.'
+                    'CameraInfo message is empty. Will wait until received.'
                 ),
                 throttle_duration_sec=5,
             )
-            rclpy.spin_once(self.node_inf, timeout_sec=1)
+            rclpy.spin_once(self.node_inf, timeout_sec=1.0)
+        self.node_inf.loginfo('CameraInfo message received! Continuing...')
         self.node_inf.destroy_subscription(self.sub_camera_info)
 
-        self.node_inf.get_logger().info('Initialized InterbotixAprilTagInterface!')
+        self.node_inf.loginfo('Initialized InterbotixAprilTagInterface!')
 
     def camera_info_cb(self, msg: CameraInfo) -> None:
         """
@@ -159,7 +155,7 @@ def find_pose(
         if len(detections) == 0:
             pose = Pose()
             if self.v:
-                self.node_inf.get_logger().warning(
+                self.node_inf.logwarn(
                     f"Could not find '{ar_tag_name}'. Returning a 'zero' Pose..."
                 )
         else:
@@ -189,14 +185,14 @@ def _snap(self) -> AprilTagDetectionArray:
         future_snap = self.srv_snap_picture.call_async(
             SnapPicture.Request(filename=self.request.full_path_where_to_get_image)
         )
-        rclpy.spin_until_future_complete(self.node_inf, future=future_snap)
+        self.node_inf.wait_until_future_complete(future_snap)
         # Check if the SnapPicture result was successful
         if not future_snap.result().success:
             # If it was not, return an empty detection array
             return AprilTagDetectionArray()
         # Call the AnalyzeSingleImage service on the snapped image
         future_analyze = self.srv_analyze_image.call_async(self.request)
-        rclpy.spin_until_future_complete(self.node_inf, future=future_analyze)
+        self.node_inf.wait_until_future_complete(future_analyze)
         # Return the results of the analysis - the detected tags
         return future_analyze.result().tag_detections
 
@@ -216,7 +212,7 @@ def find_pose_id(self) -> Tuple[List[Pose], List[int]]:
         :return: poses of the tags relative to camera, corresponding ids
         """
         if self.valid_tags is None:
-            self.node_inf.get_logger().warning(
+            self.node_inf.logwarn(
                 'Tried to find pose of valid tags but valid ids are not set'
             )
         detections: Sequence[AprilTagDetection] = self._snap().detections

interbotix_perception_toolbox/interbotix_perception_modules/interbotix_perception_modules/armtag.py

@@ -1,4 +1,4 @@
-# Copyright 2022 Trossen Robotics
+# Copyright 2024 Trossen Robotics
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
@@ -28,10 +28,10 @@
 
 from geometry_msgs.msg import Point, Pose, Quaternion, TransformStamped
 from interbotix_common_modules import angle_manipulation as ang
+from interbotix_common_modules.common_robot.robot import InterbotixRobotNode
 from interbotix_perception_modules.apriltag import InterbotixAprilTagInterface
 import numpy as np
 from rclpy.duration import Duration
-from rclpy.node import Node
 from rclpy.time import Time
 import tf2_ros
 
@@ -48,7 +48,7 @@ def __init__(
         ref_frame: str = None,
         arm_tag_frame: str = None,
         arm_base_frame: str = None,
-        node_inf: Node = None,
+        node_inf: InterbotixRobotNode = None,
         args=None,
     ) -> None:
         """
@@ -59,9 +59,9 @@ def __init__(
             InterbotixAprilTagInterface module are located
         :param ref_frame: Reference frame from which the image was taken
         :param arm_tag_frame: Frame on the robot that should serve as the arm_tag's frame
-        :param arm_base_frame: Frame on the robot that should servce as the base frame
-        :param node_inf: reference to the rclcpy.node.Node on which to build the AprilTag
-            Interface. Leave as `None` to let this object serve as its own Node
+        :param arm_base_frame: Frame on the robot that should serve as the base frame
+        :param node_inf: reference to the InterbotixRobotNode on which to build the AprilTag
+            Interface.
         """
         self.apriltag_inf = InterbotixAprilTagInterface(
             apriltag_ns=apriltag_ns,
@@ -108,7 +108,7 @@ def __init__(
         self.tfBuffer = tf2_ros.Buffer()
         self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, self.apriltag_inf.node_inf)
 
-        self.apriltag_inf.node_inf.get_logger().info('Initialized InterbotixArmTagInterface!')
+        self.apriltag_inf.node_inf.loginfo('Initialized InterbotixArmTagInterface!')
 
     def find_ref_to_arm_base_transform(
         self,
@@ -234,7 +234,7 @@ def get_transform(
             tf2_ros.ConnectivityException,
             tf2_ros.ExtrapolationException
         ) as e:
-            self.apriltag_inf.node_inf.get_logger().error(
+            self.apriltag_inf.node_inf.logerror(
                 f"Failed to look up the transform from '{target_frame}' to '{source_frame}'. {e}"
             )
             return np.identity(4)

interbotix_perception_toolbox/interbotix_perception_modules/interbotix_perception_modules/armtag_tuner_gui.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 
-# Copyright 2022 Trossen Robotics
+# Copyright 2024 Trossen Robotics
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
@@ -35,6 +35,7 @@
 from typing import Dict
 
 from ament_index_python import get_package_share_directory, PackageNotFoundError
+from interbotix_common_modules.common_robot.robot import create_interbotix_global_node
 from interbotix_perception_modules.armtag import InterbotixArmTagInterface
 from python_qt_binding import loadUi
 from python_qt_binding.QtGui import QIcon
@@ -59,12 +60,13 @@ class GUI(QWidget):
 class ArmTagTunerGui(QWidget):
     """GUI to snap AR tag pose on the arm and calculate the 'ref to arm base_link' transform."""
 
-    def __init__(self, ros_args, args):
+    def __init__(self, ros_args, node_inf, args):
         """Construct ArmTagTunerGUI object."""
         super(ArmTagTunerGui, self).__init__()
         self.armtag_inf = InterbotixArmTagInterface(
             armtag_ns=ros_args.armtag_ns,
             apriltag_ns=ros_args.apriltag_ns,
+            node_inf=node_inf,
             args=args,
         )
         try:
@@ -85,12 +87,12 @@ def __init__(self, ros_args, args):
             {'GUI': GUI}
         )
 
-        self.armtag_inf.apriltag_inf.declare_parameter(
+        self.armtag_inf.apriltag_inf.node_inf.declare_parameter(
             'position_only',
             'false'
         )
 
-        self.position_only = self.armtag_inf.apriltag_inf.get_parameter(
+        self.position_only = self.armtag_inf.apriltag_inf.node_inf.get_parameter(
             'position_only').get_parameter_value().bool_value
 
         self.name_map: Dict[str, NameMapElement] = {}
@@ -163,8 +165,10 @@ def main(args=None):
     command_line_args = remove_ros_args(args=sys.argv)[1:]
     ros_args = p.parse_args(command_line_args)
 
+    global_node = create_interbotix_global_node()
+
     app = QApplication(sys.argv)
-    gui = ArmTagTunerGui(ros_args, args)  # noqa: F841
+    gui = ArmTagTunerGui(ros_args, global_node, args)  # noqa: F841
 
     # Only kill the program at node shutdown
     signal.signal(signal.SIGINT, signal.SIG_DFL)