Skip to content

Commit

Permalink
Fix perception modules (#68)
Browse files Browse the repository at this point in the history
* 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
  • Loading branch information
lukeschmitt-tr committed Apr 23, 2024
1 parent 0d80fb3 commit 6645d3c
Show file tree
Hide file tree
Showing 7 changed files with 143 additions and 98 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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()
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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
Original file line number Diff line number Diff line change
@@ -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:
Expand Down Expand Up @@ -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]
Expand All @@ -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

Expand All @@ -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,
)
Expand All @@ -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:
"""
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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

Expand All @@ -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
Expand Down
Original file line number Diff line number Diff line change
@@ -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:
Expand Down Expand Up @@ -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

Expand All @@ -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:
"""
Expand All @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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)
Expand Down
Original file line number Diff line number Diff line change
@@ -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:
Expand Down Expand Up @@ -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
Expand All @@ -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:
Expand All @@ -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] = {}
Expand Down Expand Up @@ -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)
Expand Down
Loading

0 comments on commit 6645d3c

Please sign in to comment.