ActionServerBT Overhaul and Add Static TF Object

ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py

@@ -2,7 +2,7 @@
 # -*- coding: utf-8 -*-
 """
 This module defines the ComputeFoodFrame behavior, which computes the
-food frame from 
+food frame from the Mask provided from a perception algorithm.
 """
 # Standard imports
 from typing import Union, Optional
@@ -17,12 +17,16 @@
 from rclpy.node import Node
 from sensor_msgs.msg import CameraInfo
 import tf2_ros
-from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
 
 # Local imports
 from ada_feeding_msgs.msg import Mask
 from ada_feeding_msgs.srv import AcquisitionSelect
-from ada_feeding.helpers import BlackboardKey, quat_between_vectors, get_tf_object
+from ada_feeding.helpers import (
+    BlackboardKey,
+    quat_between_vectors,
+    get_tf_object,
+    add_update_static_tf,
+)
 from ada_feeding.behaviors import BlackboardBehavior
 from ada_feeding_perception.helpers import ros_msg_to_cv2_image
 
@@ -47,8 +51,8 @@ def blackboard_inputs(
         ros2_node: Union[BlackboardKey, Node],
         camera_info: Union[BlackboardKey, CameraInfo],
         mask: Union[BlackboardKey, Mask],
+        food_frame_id: Union[BlackboardKey, str] = "food",
         world_frame: Union[BlackboardKey, str] = "world",
-        debug_food_frame: Union[BlackboardKey, str] = "food",
     ) -> None:
         """
         Blackboard Inputs
@@ -58,9 +62,9 @@ def blackboard_inputs(
         ros2_node (Node): ROS2 Node for reading/writing TFs
         camera_info (geometry_msgs/CameraInfo): camera intrinsics matrix
         mask (ada_feeding_msgs/Mask): food context, see Mask.msg
+        food_frame_id (string): If len>0, TF frame to publish static transform
+                                   (relative to world_frame)
         world_frame (string): ID of the TF frame to represent the food frame in
-        debug_food_frame (string): If len>0, TF frame to publish static transform
-                                   (relative to world_frame) for debugging purposes
         """
         # pylint: disable=unused-argument
         # Arguments are handled generically in base class.
@@ -72,9 +76,6 @@ def blackboard_outputs(
         self,
         action_select_request: Optional[BlackboardKey],  # AcquisitionSelect.Request
         food_frame: Optional[BlackboardKey],  # TransformStamped
-        debug_tf_publisher: Optional[
-            BlackboardKey
-        ] = None,  # StaticTransformBroadcaster
     ) -> None:
         """
         Blackboard Outputs
@@ -84,10 +85,7 @@ def blackboard_outputs(
         ----------
         action_select_request (AcquisitionSelect.Request): request to send to AcquisitionSelect
                                                            (copies mask input)
-        food_frame (geometry_msgs/TransformStamped): transform from world_frame to food frame
-        debug_tf_publisher (StaticTransformBroadcaster): If set, store
-                                                        static broadcaster here to keep it alive
-                                                        for debugging purposes.
+        food_frame (geometry_msgs/TransformStamped): transform from world_frame to food_frame
         """
         # pylint: disable=unused-argument
         # Arguments are handled generically in base class.
@@ -182,7 +180,7 @@ def update(self) -> py_trees.common.Status:
         world_to_food_transform = TransformStamped()
         world_to_food_transform.header.stamp = node.get_clock().now().to_msg()
         world_to_food_transform.header.frame_id = world_frame
-        world_to_food_transform.child_frame_id = self.blackboard_get("debug_food_frame")
+        world_to_food_transform.child_frame_id = self.blackboard_get("food_frame_id")
 
         # De-project center of ROI
         mask = self.blackboard_get("mask")
@@ -251,10 +249,8 @@ def update(self) -> py_trees.common.Status:
         )
 
         # Write to blackboard outputs
-        if len(self.blackboard_get("debug_food_frame")) > 0:
-            stb = StaticTransformBroadcaster(self.blackboard_get("ros2_node"))
-            stb.sendTransform(world_to_food_transform)
-            self.blackboard_set("debug_tf_publisher", stb)
+        if len(self.blackboard_get("food_frame_id")) > 0:
+            add_update_static_tf(world_to_food_transform, self.blackboard, node)
         self.blackboard_set("food_frame", world_to_food_transform)
         request = AcquisitionSelect.Request()
         request.food_context = mask

ada_feeding/ada_feeding/behaviors/acquisition/compute_move_above.py

@@ -0,0 +1,114 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+This module defines the ComputeMoveAbove behavior, which computes the
+food frame from 
+"""
+# Standard imports
+from typing import Union, Optional
+
+# Third-party imports
+from geometry_msgs.msg import TransformStamped
+import numpy as np
+import py_trees
+import rclpy
+from rclpy.node import Node
+import tf2_ros
+
+# Local imports
+from ada_feeding_msgs.srv import AcquisitionSelect
+from ada_feeding.helpers import BlackboardKey
+from ada_feeding.behaviors import BlackboardBehavior
+
+
+class ComputeMoveAbove(BlackboardBehavior):
+    """
+    (1) Selects an action from AcquisitionSelect service response.
+    (2) Computes the MoveAbove Pose in the World Frame
+    """
+
+    # pylint: disable=arguments-differ
+    # We *intentionally* violate Liskov Substitution Princple
+    # in that blackboard config (inputs + outputs) are not
+    # meant to be called in a generic setting.
+
+    # pylint: disable=too-many-arguments
+    # These are effectively config definitions
+    # They require a lot of arguments.
+
+    def blackboard_inputs(
+        self,
+        world_to_food: Union[BlackboardKey, TransformStamped],
+        action_select_response: Union[BlackboardKey, AcquisitionSelect.Response],
+    ) -> None:
+        """
+        Blackboard Inputs
+
+        Parameters
+        ----------
+        world_to_food (geometry_msgs/TransformStamped): transform from world_frame to food_frame
+        action_select_request (AcquisitionSelect.Response): response received from AcquisitionSelect
+        """
+        # pylint: disable=unused-argument
+        # Arguments are handled generically in base class.
+        super().blackboard_inputs(
+            **{key: value for key, value in locals().items() if key != "self"}
+        )
+
+    def blackboard_outputs(
+        self,
+        action: Optional[BlackboardKey],  # AcquisitionSchema
+    ) -> None:
+        """
+        Blackboard Outputs
+        By convention (to avoid collisions), avoid non-None default arguments.
+
+        Parameters
+        ----------
+        TODO
+        """
+        # pylint: disable=unused-argument
+        # Arguments are handled generically in base class.
+        super().blackboard_outputs(
+            **{key: value for key, value in locals().items() if key != "self"}
+        )
+
+    def setup(self, **kwargs):
+        """
+        Middleware (i.e. TF) setup
+        """
+
+        # pylint: disable=attribute-defined-outside-init
+        # It is okay for attributes in behaviors to be
+        # defined in the setup / initialise functions.
+
+        # TODO
+        pass
+
+    def initialise(self):
+        """
+        Behavior initialization
+        """
+
+        # pylint: disable=attribute-defined-outside-init
+        # It is okay for attributes in behaviors to be
+        # defined in the setup / initialise functions.
+
+        # TODO
+        pass
+
+    def update(self) -> py_trees.common.Status:
+        """
+        Behavior tick (DO NOT BLOCK)
+        """
+        # pylint: disable=too-many-locals
+        # I think this is reasonable to understand
+        # the logic of this function.
+
+        # pylint: disable=too-many-statements
+        # We can't get around all the conversions
+        # to ROS2 msg types, which take 3-4 statements each.
+
+        # TODO
+
+        return py_trees.common.Status.SUCCESS

ada_feeding/ada_feeding/helpers.py

@@ -9,14 +9,15 @@
 
 # Third-party imports
 import numpy as np
-from geometry_msgs.msg import Vector3, Quaternion
+from geometry_msgs.msg import TransformStamped, Vector3, Quaternion
 import py_trees
 from py_trees.common import Access
 from pymoveit2 import MoveIt2
 from pymoveit2.robots import kinova
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.node import Node
 from tf2_ros.buffer import Buffer
+from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
 from tf2_ros.transform_listener import TransformListener
 
 
@@ -92,6 +93,87 @@ def quat_between_vectors(vec_from: Vector3, vec_to: Vector3) -> Quaternion:
     return ret
 
 
+def add_update_static_tf(
+    transform_stamped: TransformStamped,
+    blackboard: py_trees.blackboard.Client,
+    node: Optional[Node] = None,
+) -> bool:
+    """
+    Sends a given transform to /tf_static.
+    This uses a StaticTransformBroadcaster on the global backboard.
+    Note this is *not* a resource-intensive operation, as both
+    publisher and subscribers to /tf_static use latching.
+    Do NOT call this function in a fast loop.
+    Since these transforms are assumed static until updated, they cannot be deleted.
+    More Info: https://answers.ros.org/question/226824/using-tf_static-for-almost-static-transforms/
+
+    Parameters
+    ----------
+    transform_stamped: Transform to publish (will overwrite any transform with identical
+                        transform_stamped.header.frame_id and transform_stamped.child_frame_id)
+    blackboard: Client in which to store the static transform broadcaster (STB) and mutex
+    node: The ROS2 node the STB is associated with. If None, this function will not create
+          the STB if it does exist, and will instead raise a KeyError.
+
+    Returns
+    ---------
+    False if the lock is held, else True
+
+    Raises
+    ------
+    KeyError: if the TF objects do not exist and node is None.
+    """
+
+    static_tf_broadcaster_blackboard_key = "/tf_static/stb"
+    static_tf_transforms_blackboard_key = "/tf_static/transforms"
+    static_tf_lock_blackboard_key = "/tf_static/lock"
+
+    # First, register the TF objects and their corresponding lock for READ access
+    if not blackboard.is_registered(static_tf_broadcaster_blackboard_key, Access.READ):
+        blackboard.register_key(static_tf_broadcaster_blackboard_key, Access.READ)
+    if not blackboard.is_registered(static_tf_transforms_blackboard_key, Access.WRITE):
+        blackboard.register_key(static_tf_transforms_blackboard_key, Access.WRITE)
+    if not blackboard.is_registered(static_tf_lock_blackboard_key, Access.READ):
+        blackboard.register_key(static_tf_lock_blackboard_key, Access.READ)
+
+    # Second, check if the MoveIt2 object and its corresponding lock exist on the
+    # blackboard. If they do not, register the blackboard for WRITE access to those
+    # keys and create them.
+    try:
+        stb = blackboard.get(static_tf_broadcaster_blackboard_key)
+        lock = blackboard.get(static_tf_lock_blackboard_key)
+    except KeyError as exc:
+        # If no node is passed in, raise an error.
+        if node is None:
+            raise KeyError("Static TF objects do not exist on the blackboard") from exc
+
+        # If a node is passed in, create a new MoveIt2 object and lock.
+        node.get_logger().info(
+            "Static TF objects and lock do not exist on the blackboard. Creating them now."
+        )
+        blackboard.register_key(static_tf_broadcaster_blackboard_key, Access.WRITE)
+        blackboard.register_key(static_tf_lock_blackboard_key, Access.WRITE)
+        stb = StaticTransformBroadcaster(node)
+        transforms = {}
+        lock = Lock()
+        blackboard.set(static_tf_broadcaster_blackboard_key, stb)
+        blackboard.set(static_tf_transforms_blackboard_key, transforms)
+        blackboard.set(static_tf_lock_blackboard_key, lock)
+
+    # Check and acquire the lock
+    if lock.locked():
+        return False
+
+    with lock:
+        key = f"{transform_stamped.header.frame_id}-{transform_stamped.child_frame_id}"
+        transforms = blackboard.get(static_tf_transforms_blackboard_key)
+        transforms[key] = transform_stamped
+        blackboard.set(static_tf_transforms_blackboard_key, transforms)
+        stb.sendTransform(list(transforms.values()))
+
+    return True
+
+
 def get_tf_object(
     blackboard: py_trees.blackboard.Client,
     node: Optional[Node] = None,
@@ -135,7 +217,7 @@ def get_tf_object(
     if not blackboard.is_registered(tf_lock_blackboard_key, Access.READ):
         blackboard.register_key(tf_lock_blackboard_key, Access.READ)
 
-    # Second, check if the MoveIt2 object and its corresponding lock exist on the
+    # Second, check if the TF objects and its corresponding lock exist on the
     # blackboard. If they do not, register the blackboard for WRITE access to those
     # keys and create them.
     try:
@@ -147,7 +229,7 @@ def get_tf_object(
         if node is None:
             raise KeyError("TF objects do not exist on the blackboard") from exc
 
-        # If a node is passed in, create a new MoveIt2 object and lock.
+        # If a node is passed in, create new TF objects and lock.
         node.get_logger().info(
             "TF objects and lock do not exist on the blackboard. Creating them now."
         )