From 317cffc17531af0a3799f27376b8941dbbf243cc Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Wed, 27 Sep 2023 19:01:52 -0700 Subject: [PATCH 1/9] [WIP] Working MoveIt2Visitor, DebugVisitor, and StaticTransformBroadcaster logic; Next Step is MoveIt2Plan and MoveIt2Execute Behaviors --- .../acquisition/compute_food_frame.py | 32 +++-- .../acquisition/compute_move_above.py | 114 ++++++++++++++++++ ada_feeding/ada_feeding/helpers.py | 88 +++++++++++++- .../ada_feeding/trees/acquire_food_tree.py | 99 ++++++++++----- ada_feeding/ada_feeding/visitors/__init__.py | 6 + .../ada_feeding/visitors/moveit2_visitor.py | 86 +++++++++++++ ada_feeding/package.xml | 1 + ada_feeding/scripts/create_action_servers.py | 5 + 8 files changed, 377 insertions(+), 54 deletions(-) create mode 100644 ada_feeding/ada_feeding/behaviors/acquisition/compute_move_above.py create mode 100644 ada_feeding/ada_feeding/visitors/__init__.py create mode 100644 ada_feeding/ada_feeding/visitors/moveit2_visitor.py diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py b/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py index b159defc..d1732b8b 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py +++ b/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 diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/compute_move_above.py b/ada_feeding/ada_feeding/behaviors/acquisition/compute_move_above.py new file mode 100644 index 00000000..ba9eeaac --- /dev/null +++ b/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 diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index 2bac5fc3..1630496c 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -9,7 +9,7 @@ # 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 @@ -17,6 +17,7 @@ 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." ) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index af69b758..467932a5 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -13,19 +13,25 @@ from rclpy.node import Node # Local imports +from ada_feeding import ActionServerBT from ada_feeding.behaviors.acquisition import ComputeFoodFrame from ada_feeding.helpers import BlackboardKey -from ada_feeding.trees import MoveToTree +from ada_feeding.visitors import MoveIt2Visitor from ada_feeding_msgs.action import AcquireFood -class AcquireFoodTree(MoveToTree): +class AcquireFoodTree(ActionServerBT): """ A behvaior tree to select and execute an acquisition action (see ada_feeding_msgs.action.AcquisitionSchema) for a given food mask in ada_feeding_msgs.action.AcquireFood. """ + def __init__(self): + """ + Initializes tree-specific parameters. + """ + # pylint: disable=too-many-instance-attributes, too-many-arguments # Many arguments is fine for this class since it has to be able to configure all parameters # of its constraints. @@ -33,30 +39,26 @@ class AcquireFoodTree(MoveToTree): # pylint: disable=too-many-locals # Unfortunately, many local variables are required here to isolate the keys # of similar constraints in the blackboard. - def create_move_to_tree( + # pylint: disable=attribute-defined-outside-init + # It is reasonable for attributes that are tree-specific, or only relevant + # after the tree is created, to be defined in `create_tree`. + @override + def create_tree( self, name: str, - tree_root_name: str, + action_type: type, + tree_root_name: str, # DEPRECATED node: Node, ) -> py_trees.trees.BehaviourTree: - """ - Creates the AcquireFood behavior tree. - - Parameters - ---------- - name: The name of the behavior tree. (DEPRECATED) - tree_root_name: The name of the tree. This is necessary because sometimes - trees create subtrees, but still need to track the top-level tree - name to read/write the correct blackboard variables. (DEPRECATED) - node: The ROS2 node that this tree is associated with. Necessary for - behaviors within the tree connect to ROS topics/services/actions. - - Returns - ------- - tree: The behavior tree that acquires food - """ - # TODO: remove name and tree_root_name - # We have access to them implicitly via self.blackboard / self.blackboard_tree_root + # Docstring copied by @override + + # TODO: remove tree_root_name + # Sub-trees in general should not need knowledge of their parent. + + ## Create Local Variables + self.blackboard = py_trees.blackboard.Client(name=name, namespace=name) + self.action_type = action_type + self.node = node ### Inputs we expect on the blackboard on initialization # Note: WRITE access since send_goal could write to these @@ -77,15 +79,10 @@ def create_move_to_tree( "ros2_node": node, "camera_info": BlackboardKey("camera_info"), "mask": BlackboardKey("mask") + # Default food_frame_id = "food" # Default world_frame = "world" - # Default debug_tf_frame = "food" - }, - outputs={ - "action_select_request": None, - "food_frame": None, - "debug_tf_publisher": BlackboardKey("debug_tf_publisher") - # ^^ Set to None to disable }, + outputs={"action_select_request": None, "food_frame": None}, ) ### Define Tree Logic @@ -106,7 +103,7 @@ def create_move_to_tree( @override def send_goal(self, tree: py_trees.trees.BehaviourTree, goal: object) -> bool: # Docstring copied by @override - super().send_goal(tree, goal) + # Note: if here, tree is root, not a subtree # Check goal type if not isinstance(goal, AcquireFood.Goal): @@ -116,22 +113,58 @@ def send_goal(self, tree: py_trees.trees.BehaviourTree, goal: object) -> bool: self.blackboard.mask = goal.detected_food self.blackboard.camera_info = goal.camera_info + # Top level tree + # Add MoveIt2Visitor for Feedback: + self.feedback_visitor = MoveIt2Visitor(self.node) + tree.visitors.append(self.feedback_visitor) + return True # Override result to handle timing outside MoveTo Behaviors @override def get_feedback(self, tree: py_trees.trees.BehaviourTree) -> object: # Docstring copied by @override - feedback_msg = super().get_feedback(tree) + # Note: if here, tree is root, not a subtree + feedback_msg = self.action_type.Feedback() + + # Copy everything from the visitor + # TODO: This Feedback/Result logic w/ MoveIt2Visitor can exist in MoveToTree right now + feedback = self.feedback_visitor.get_feedback() + feedback_msg.is_planning = feedback.is_planning + feedback_msg.planning_time = feedback.planning_time + feedback_msg.motion_time = feedback.motion_time + feedback_msg.motion_initial_distance = feedback.motion_initial_distance + feedback_msg.motion_curr_distance = feedback.motion_curr_distance - # TODO: fix is_planning / planning_time in non-MoveTo Behavior return feedback_msg # Override result to add other elements to result msg @override def get_result(self, tree: py_trees.trees.BehaviourTree) -> object: # Docstring copied by @override - result_msg = super().get_result(tree) + # Note: if here, tree is root, not a subtree + result_msg = self.action_type.Result() + + # If the tree succeeded, return success + if tree.root.status == py_trees.common.Status.SUCCESS: + result_msg.status = result_msg.STATUS_SUCCESS + # If the tree failed, detemine whether it was a planning or motion failure + elif tree.root.status == py_trees.common.Status.FAILURE: + feedback = self.feedback_visitor.get_feedback() + if feedback.is_planning: + result_msg.status = result_msg.STATUS_PLANNING_FAILED + else: + result_msg.status = result_msg.STATUS_MOTION_FAILED + # If the tree has an invalid status, return unknown + elif tree.root.status == py_trees.common.Status.INVALID: + result_msg.status = result_msg.STATUS_UNKNOWN + # If the tree is running, the fact that `get_result` was called is + # indicative of an error. Return unknown error. + else: + tree.root.logger.error( + f"Called get_result with status RUNNING: {tree.root.status}" + ) + result_msg.status = result_msg.STATUS_UNKNOWN # TODO: add action_index, posthoc, action_select_hash return result_msg diff --git a/ada_feeding/ada_feeding/visitors/__init__.py b/ada_feeding/ada_feeding/visitors/__init__.py new file mode 100644 index 00000000..37f0326a --- /dev/null +++ b/ada_feeding/ada_feeding/visitors/__init__.py @@ -0,0 +1,6 @@ +""" +This package contains custom BT Visitors that are used in the Ada Feeding +project. All of these extend VisitorBase. +""" + +from .moveit2_visitor import MoveIt2Visitor diff --git a/ada_feeding/ada_feeding/visitors/moveit2_visitor.py b/ada_feeding/ada_feeding/visitors/moveit2_visitor.py new file mode 100644 index 00000000..7b225272 --- /dev/null +++ b/ada_feeding/ada_feeding/visitors/moveit2_visitor.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +This module defines the MoveIt2 Visitor. + +This generally collects variables used for feedback/result in +all actions based on MoveTo.action. +""" + +# Standard imports +from overrides import override + +# Third-party imports +import py_trees +from py_trees.visitors import VisitorBase +from rclpy.node import Node + +# Local imports +import ada_feeding.behaviors +from ada_feeding_msgs.action import MoveTo + + +class MoveIt2Visitor(VisitorBase): + """ + A BT Visitor that computes the feedback used in MoveTo.action. + Can be used in all actions that return similar information. + """ + + def __init__(self, node: Node) -> None: + super().__init__(full=False) + + # Just need the node's clock for timing + self.node = node + + # Used for planning/motion time calculations + self.start_time = None + + # To return with get_feedback + self.feedback = MoveTo.Feedback() + self.feedback.is_planning = True + + @override + def run(self, behaviour: py_trees.behaviour.Behaviour) -> None: + # Docstring copied by @override + + # Record Start Time + if self.start_time is None: + self.start_time = self.node.get_clock().now() + + if isinstance(behaviour, ada_feeding.behaviors.MoveTo): + # If in MoveTo action, copy from there + self.feedback.motion_initial_distance = ( + behaviour.tree_blackboard.motion_initial_distance + ) + self.feedback.motion_curr_distance = ( + behaviour.tree_blackboard.motion_curr_distance + ) + + # Check for flip between planning/motion + if behaviour.tree_blackboard.is_planning != self.feedback.is_planning: + self.start_time = self.node.get_clock().now() + self.feedback.is_planning = behaviour.tree_blackboard.is_planning + else: + # Else just update planning time + if not self.feedback.is_planning: + self.start_time = self.node.get_clock().now() + self.feedback.is_planning = True + + # Calculate updated planning/motion time + if self.feedback.is_planning: + self.feedback.planning_time = ( + self.node.get_clock().now() - self.start_time + ).to_msg() + else: + self.feedback.motion_time = ( + self.node.get_clock().now() - self.start_time + ).to_msg() + + def get_feedback(self) -> MoveTo.Feedback: + """ + + Returns + ------- + MoveTo Feedback message, see MoveTo.action for more info. + """ + return self.feedback diff --git a/ada_feeding/package.xml b/ada_feeding/package.xml index a6d4f905..130c7fa9 100644 --- a/ada_feeding/package.xml +++ b/ada_feeding/package.xml @@ -25,6 +25,7 @@ pyrealsense2-pip overrides-pip + ros2_numpy-pip ament_cmake diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index 0ef714da..e77b87fa 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -12,6 +12,7 @@ # Third-party imports from ada_watchdog_listener import ADAWatchdogListener import py_trees +from py_trees.visitors import DebugVisitor from rcl_interfaces.msg import ParameterDescriptor, ParameterType import rclpy from rclpy.action import ActionServer, CancelResponse, GoalResponse @@ -326,6 +327,10 @@ def setup_tree(self, tree: py_trees.trees.BehaviourTree) -> None: for node in tree.root.iterate(): node.logger = self.get_logger() + # Add a DebugVisitor to catch behavior debug messages + # Set --log-level create_action_servers:=info (or higher) to quiet. + tree.visitors.append(DebugVisitor()) + # Call the tree's setup function # TODO: consider adding a timeout here tree.setup(node=self) From 2da5dcf09aeaa8b855b2c77c88c3684a1e495975 Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Thu, 28 Sep 2023 09:32:09 -0700 Subject: [PATCH 2/9] [WIP] Formatted overhaul of ActionServerBT to be a true tree generator, and separate actionserver functionality out of create_tree. --- ada_feeding/ada_feeding/action_server_bt.py | 34 ++++--- .../ada_feeding/trees/acquire_food_tree.py | 61 +++++------- .../ada_feeding/trees/move_from_mouth_tree.py | 37 +++----- .../trees/move_to_configuration_tree.py | 30 ++---- ...o_configuration_with_ft_thresholds_tree.py | 22 ++--- ...uration_with_pose_path_constraints_tree.py | 27 ++---- .../ada_feeding/trees/move_to_dummy_tree.py | 92 ++++--------------- .../ada_feeding/trees/move_to_mouth_tree.py | 3 +- .../ada_feeding/trees/move_to_pose_tree.py | 35 +++---- ...to_pose_with_pose_path_constraints_tree.py | 22 +---- ada_feeding/ada_feeding/trees/move_to_tree.py | 45 +++------ ada_feeding/scripts/create_action_servers.py | 18 ++-- 12 files changed, 139 insertions(+), 287 deletions(-) diff --git a/ada_feeding/ada_feeding/action_server_bt.py b/ada_feeding/ada_feeding/action_server_bt.py index 48df019e..71c91f12 100644 --- a/ada_feeding/ada_feeding/action_server_bt.py +++ b/ada_feeding/ada_feeding/action_server_bt.py @@ -19,16 +19,19 @@ class ActionServerBT(ABC): `create_action_server.py` """ - # pylint: disable=too-many-arguments - # One over is fine. + def __init__(self, node: Node, **kwargs) -> None: + """ + Store the ROS2 Node that created trees is associated with. Necessary for + behaviors within the tree to connect to ROS topics/services/actions. + + Subclasses should overwrite kwargs with tree-agnostic + input parameters. + """ + self._node = node @abstractmethod def create_tree( - self, - name: str, - action_type: type, - tree_root_name: str, - node: Node, + self, name: str, tree_root_name: str # DEPRECATED ) -> py_trees.trees.BehaviourTree: """ Create the behavior tree that will be executed by this action server. @@ -38,12 +41,9 @@ def create_tree( Parameters ---------- name: The name of the behavior tree. - action_type: the type for the action, as a class tree_root_name: The name of the tree. This is necessary because sometimes trees create subtrees, but still need to track the top-level tree name to read/write the correct blackboard variables. - node: The ROS2 node that this tree is associated with. Necessary for - behaviors within the tree connect to ROS topics/services/actions. """ raise NotImplementedError("create_tree not implemented") @@ -91,7 +91,9 @@ def preempt_goal(self, tree: py_trees.trees.BehaviourTree) -> bool: return False @abstractmethod - def get_feedback(self, tree: py_trees.trees.BehaviourTree) -> object: + def get_feedback( + self, tree: py_trees.trees.BehaviourTree, action_type: type + ) -> object: """ Creates the ROS feedback message corresponding to this action. @@ -100,15 +102,18 @@ def get_feedback(self, tree: py_trees.trees.BehaviourTree) -> object: Parameters ---------- tree: The behavior tree that is being executed. + action_type: the type for the action, as a class Returns ------- - feedback: The ROS feedback message to be sent to the action client. + The ROS feedback message to be sent to the action client, type action_type.Feedback() """ raise NotImplementedError("get_feedback not implemented") @abstractmethod - def get_result(self, tree: py_trees.trees.BehaviourTree) -> object: + def get_result( + self, tree: py_trees.trees.BehaviourTree, action_type: type + ) -> object: """ Creates the ROS result message corresponding to this action. @@ -117,9 +122,10 @@ def get_result(self, tree: py_trees.trees.BehaviourTree) -> object: Parameters ---------- tree: The behavior tree that is being executed. + action_type: the type for the action, as a class Returns ------- - result: The ROS result message to be sent to the action client. + The ROS feedback message to be sent to the action client, type action_type.Result() """ raise NotImplementedError("get_result not implemented") diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 467932a5..fe303cdc 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -27,56 +27,25 @@ class AcquireFoodTree(ActionServerBT): for a given food mask in ada_feeding_msgs.action.AcquireFood. """ - def __init__(self): - """ - Initializes tree-specific parameters. - """ - - # pylint: disable=too-many-instance-attributes, too-many-arguments - # Many arguments is fine for this class since it has to be able to configure all parameters - # of its constraints. - - # pylint: disable=too-many-locals - # Unfortunately, many local variables are required here to isolate the keys - # of similar constraints in the blackboard. - # pylint: disable=attribute-defined-outside-init - # It is reasonable for attributes that are tree-specific, or only relevant - # after the tree is created, to be defined in `create_tree`. @override def create_tree( self, name: str, - action_type: type, tree_root_name: str, # DEPRECATED - node: Node, ) -> py_trees.trees.BehaviourTree: # Docstring copied by @override # TODO: remove tree_root_name # Sub-trees in general should not need knowledge of their parent. - ## Create Local Variables - self.blackboard = py_trees.blackboard.Client(name=name, namespace=name) - self.action_type = action_type - self.node = node - - ### Inputs we expect on the blackboard on initialization - # Note: WRITE access since send_goal could write to these - # Mask for ComputeFoodFrame - self.blackboard.register_key(key="mask", access=py_trees.common.Access.WRITE) - # CameraInfo for ComputeFoodFrame - self.blackboard.register_key( - key="camera_info", access=py_trees.common.Access.WRITE - ) - ### Define Tree Leaves and Subtrees # Add ComputeFoodFrame compute_food_frame = ComputeFoodFrame( "ComputeFoodFrame", - self.blackboard.namespace, + name, inputs={ - "ros2_node": node, + "ros2_node": self._node, "camera_info": BlackboardKey("camera_info"), "mask": BlackboardKey("mask") # Default food_frame_id = "food" @@ -89,7 +58,7 @@ def create_tree( # Root Sequence root_seq = py_trees.composites.Sequence( - name="RootSequence", + name=name, memory=True, children=[ compute_food_frame, @@ -110,22 +79,29 @@ def send_goal(self, tree: py_trees.trees.BehaviourTree, goal: object) -> bool: return False # Write tree inputs to blackboard + name = tree.root.name + blackboard = py_trees.blackboard.Client(name=name, namespace=name) self.blackboard.mask = goal.detected_food self.blackboard.camera_info = goal.camera_info # Top level tree # Add MoveIt2Visitor for Feedback: - self.feedback_visitor = MoveIt2Visitor(self.node) - tree.visitors.append(self.feedback_visitor) + self.feedback_visitor = MoveIt2Visitor(self._node) + tree.add_visitor(self.feedback_visitor) return True # Override result to handle timing outside MoveTo Behaviors @override - def get_feedback(self, tree: py_trees.trees.BehaviourTree) -> object: + def get_feedback( + self, tree: py_trees.trees.BehaviourTree, action_type: type + ) -> object: # Docstring copied by @override # Note: if here, tree is root, not a subtree - feedback_msg = self.action_type.Feedback() + if action_type is not AcquireFood: + return None + + feedback_msg = action_type.Feedback() # Copy everything from the visitor # TODO: This Feedback/Result logic w/ MoveIt2Visitor can exist in MoveToTree right now @@ -140,10 +116,15 @@ def get_feedback(self, tree: py_trees.trees.BehaviourTree) -> object: # Override result to add other elements to result msg @override - def get_result(self, tree: py_trees.trees.BehaviourTree) -> object: + def get_result( + self, tree: py_trees.trees.BehaviourTree, action_type: type + ) -> object: # Docstring copied by @override # Note: if here, tree is root, not a subtree - result_msg = self.action_type.Result() + if action_type is not AcquireFood: + return None + + result_msg = action_type.Result() # If the tree succeeded, return success if tree.root.status == py_trees.common.Status.SUCCESS: diff --git a/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py index 5682738f..e99251bb 100644 --- a/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py +++ b/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py @@ -10,6 +10,7 @@ # Standard imports from typing import List, Tuple +from overrides import override # Third-party imports import py_trees @@ -42,6 +43,7 @@ class MoveFromMouthTree(MoveToTree): def __init__( self, + node: Node, staging_configuration_position: Tuple[float, float, float], staging_configuration_quat_xyzw: Tuple[float, float, float, float], end_configuration: List[float], @@ -112,7 +114,7 @@ def __init__( and to the mouth. """ # Initialize MoveToTree - super().__init__() + super().__init__(node) # Store the parameters self.staging_configuration_position = staging_configuration_position @@ -156,28 +158,13 @@ def __init__( self.force_threshold = force_threshold self.torque_threshold = torque_threshold + @override def create_move_to_tree( self, name: str, tree_root_name: str, - node: Node, ) -> py_trees.trees.BehaviourTree: - """ - Creates the MoveToMouth behaviour tree. - - Parameters - ---------- - name: The name of the behaviour tree. - tree_root_name: The name of the tree. This is necessary because sometimes - trees create subtrees, but still need to track the top-level tree - name to read/write the correct blackboard variables. - node: The ROS2 node that this tree is associated with. Necessary for - behaviours within the tree connect to ROS topics/services/actions. - - Returns - ------- - tree: The behaviour tree that moves the robot above the plate. - """ + # Docstring copied from @override # Separate the namespace of each sub-behavior pre_moveto_config_prefix = "pre_moveto_config" @@ -207,7 +194,7 @@ def create_move_to_tree( allow_wheelchair_collision = get_toggle_collision_object_behavior( name, allow_wheelchair_collision_prefix, - node, + self._node, [self.wheelchair_collision_object_id], True, ) @@ -218,6 +205,7 @@ def create_move_to_tree( ) move_to_staging_configuration = ( MoveToPoseWithPosePathConstraintsTree( + self._node, position_goal=self.staging_configuration_position, quat_xyzw_goal=self.staging_configuration_quat_xyzw, tolerance_position_goal=self.staging_configuration_tolerance, @@ -240,9 +228,7 @@ def create_move_to_tree( ) .create_tree( move_to_staging_configuration_name, - self.action_type, tree_root_name, - node, ) .root ) @@ -252,7 +238,7 @@ def create_move_to_tree( disallow_wheelchair_collision = get_toggle_collision_object_behavior( name, disallow_wheelchair_collision_prefix, - node, + self._node, [self.wheelchair_collision_object_id], False, ) @@ -263,7 +249,7 @@ def create_move_to_tree( name, add_wheelchair_wall_prefix, in_front_of_wheelchair_wall_id, - node, + self._node, self.blackboard, ) @@ -273,6 +259,7 @@ def create_move_to_tree( ) move_to_end_configuration = ( MoveToConfigurationWithPosePathConstraintsTree( + self._node, joint_positions_goal=self.end_configuration, tolerance_joint_goal=self.end_configuration_tolerance, planner_id=self.planner_id, @@ -286,9 +273,7 @@ def create_move_to_tree( ) .create_tree( move_to_end_configuration_name, - self.action_type, tree_root_name, - node, ) .root ) @@ -299,7 +284,7 @@ def create_move_to_tree( name, remove_wheelchair_wall_prefix, in_front_of_wheelchair_wall_id, - node, + self._node, ) ) diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py index c86d176c..f1cd1162 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py @@ -7,6 +7,7 @@ # Standard imports from typing import List, Set +from overrides import override # Third-party imports import py_trees @@ -42,6 +43,7 @@ class MoveToConfigurationTree(MoveToTree): # A mutable default value is okay since we don't change it in this function. def __init__( self, + node: Node, joint_positions: List[float], tolerance: float = 0.001, weight: float = 1.0, @@ -78,7 +80,7 @@ def __init__( tree, this should be False. Else (e.g., if this is a standalone tree), True. """ # Initialize MoveToTree - super().__init__() + super().__init__(node) # Store the parameters self.joint_positions = joint_positions @@ -96,28 +98,14 @@ def __init__( # pylint: disable=too-many-locals # Unfortunately, many local variables are required here to isolate the keys # of similar constraints in the blackboard. + @override def create_move_to_tree( self, name: str, tree_root_name: str, - node: Node, ) -> py_trees.trees.BehaviourTree: - """ - Creates the MoveToConfiguration behavior tree. + # Docstring copied from @override - Parameters - ---------- - name: The name of the behavior tree. - tree_root_name: The name of the tree. This is necessary because sometimes - trees create subtrees, but still need to track the top-level tree - name to read/write the correct blackboard variables. - node: The ROS2 node that this tree is associated with. Necessary for - behaviors within the tree connect to ROS topics/services/actions. - - Returns - ------- - tree: The behavior tree that moves the robot above the plate. - """ # Separate blackboard namespaces for children joint_constraint_namespace_prefix = JOINT_GOAL_CONSTRAINT_NAMESPACE_PREFIX clear_constraints_namespace_prefix = CLEAR_CONSTRAINTS_NAMESPACE_PREFIX @@ -228,14 +216,14 @@ def create_move_to_tree( # Create the MoveTo behavior move_to_name = Blackboard.separator.join([name, move_to_namespace_prefix]) - move_to = MoveTo(move_to_name, tree_root_name, node) + move_to = MoveTo(move_to_name, tree_root_name, self._node) # Add the joint goal constraint to the MoveTo behavior joint_goal_constaint_name = Blackboard.separator.join( [name, joint_constraint_namespace_prefix] ) joint_constraints = SetJointGoalConstraint( - joint_goal_constaint_name, move_to, node + joint_goal_constaint_name, move_to, self._node ) # Clear the constraints @@ -243,7 +231,9 @@ def create_move_to_tree( clear_constraints_name = Blackboard.separator.join( [name, clear_constraints_namespace_prefix] ) - root = ClearConstraints(clear_constraints_name, joint_constraints, node) + root = ClearConstraints( + clear_constraints_name, joint_constraints, self._node + ) else: root = joint_constraints diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py index 519ad25e..ed6a5760 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py @@ -7,6 +7,7 @@ # Standard imports from typing import List, Set +from overrides import override # Third-party imports import py_trees @@ -34,6 +35,7 @@ class MoveToConfigurationWithFTThresholdsTree(MoveToTree): # A mutable default value is okay since we don't change it in this function. def __init__( self, + node: Node, # Required parameters for moving to a configuration joint_positions: List[float], # Optional parameters for moving to a configuration @@ -98,7 +100,7 @@ def __init__( # One over is okay # Initialize MoveToTree - super().__init__() + super().__init__(node) # Store the parameters for the joint goal constraint self.joint_positions = joint_positions @@ -126,31 +128,21 @@ def __init__( self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard self.clear_constraints = clear_constraints + @override def create_move_to_tree( self, name: str, tree_root_name: str, - node: Node, ) -> py_trees.trees.BehaviourTree: - """ - Creates the MoveToConfigurationWithFTThresholdsTree behavior tree. + # Docstring copied from @override - Parameters - ---------- - name: The name of the behavior tree. - node: The ROS2 node that this tree is associated with. Necessary for - behaviors within the tree connect to ROS topics/services/actions. - - Returns - ------- - tree: The behavior tree that moves the robot above the plate. - """ turn_watchdog_listener_on_prefix = "turn_watchdog_listener_on" # First, create the MoveToConfiguration behavior tree, in the same # namespace as this tree move_to_configuration_root = ( MoveToConfigurationTree( + self._node, joint_positions=self.joint_positions, tolerance=self.tolerance_joint, weight=self.weight_joint, @@ -162,7 +154,7 @@ def create_move_to_tree( keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard, clear_constraints=self.clear_constraints, ) - .create_tree(name, self.action_type, tree_root_name, node) + .create_tree(name, tree_root_name) .root ) diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py index 55d9c051..ed34a2ca 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py @@ -7,6 +7,7 @@ # Standard imports from typing import List, Optional, Tuple, Set, Union +from overrides import override # Third-party imports import py_trees @@ -35,6 +36,7 @@ class MoveToConfigurationWithPosePathConstraintsTree(MoveToTree): # A mutable default value is okay since we don't change it in this function. def __init__( self, + node: Node, # Required parameters for moving to a configuration joint_positions_goal: List[float], # Optional parameters for moving to a configuration @@ -98,7 +100,7 @@ def __init__( tree, this should be False. Else (e.g., if this is a standalone tree), True. """ # Initialize MoveToTree - super().__init__() + super().__init__(node) # Store the parameters for the joint goal constraint self.joint_positions_goal = joint_positions_goal @@ -125,32 +127,19 @@ def __init__( self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard self.clear_constraints = clear_constraints + @override def create_move_to_tree( self, name: str, tree_root_name: str, - node: Node, ) -> py_trees.trees.BehaviourTree: - """ - Creates the MoveToConfigurationWithPosePathConstraintsTree behavior tree. + # Docstring copied from @override - Parameters - ---------- - name: The name of the behavior tree. - tree_root_name: The name of the tree. This is necessary because sometimes - trees create subtrees, but still need to track the top-level tree - name to read/write the correct blackboard variables. - node: The ROS2 node that this tree is associated with. Necessary for - behaviors within the tree connect to ROS topics/services/actions. - - Returns - ------- - tree: The behavior tree that moves the robot above the plate. - """ # First, create the MoveToConfiguration behavior tree, in the same # namespace as this tree move_to_configuration_root = ( MoveToConfigurationTree( + self._node, joint_positions=self.joint_positions_goal, tolerance=self.tolerance_joint_goal, weight=self.weight_joint_goal, @@ -162,7 +151,7 @@ def create_move_to_tree( keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard, clear_constraints=False, ) - .create_tree(name, self.action_type, tree_root_name, node) + .create_tree(name, tree_root_name) .root ) @@ -181,7 +170,7 @@ def create_move_to_tree( parameterization_orientation_path=self.parameterization_orientation_path, weight_position_path=self.weight_position_path, weight_orientation_path=self.weight_orientation_path, - node=node, + node=self._node, clear_constraints=self.clear_constraints, ) diff --git a/ada_feeding/ada_feeding/trees/move_to_dummy_tree.py b/ada_feeding/ada_feeding/trees/move_to_dummy_tree.py index 2aee4699..f1c43768 100644 --- a/ada_feeding/ada_feeding/trees/move_to_dummy_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_dummy_tree.py @@ -6,6 +6,7 @@ """ # Standard imports +from overrides import override # Third-party imports import py_trees @@ -28,6 +29,7 @@ class MoveToDummyTree(ActionServerBT): def __init__( self, + node: Node, dummy_plan_time: float = 2.5, dummy_motion_time: float = 7.5, ) -> None: @@ -39,6 +41,7 @@ def __init__( dummy_plan_time: How many seconds this dummy node should spend in planning. dummy_motion_time: How many seconds this dummy node should spend in motion. """ + super().__init__(node) # Set the dummy motion parameters self.dummy_plan_time = dummy_plan_time @@ -48,43 +51,13 @@ def __init__( self.tree = None self.blackboard = None - # pylint: disable=attribute-defined-outside-init - # It is reasonable for attributes that are tree-specific, or only relevant - # after the tree is created, to be defined in `create_tree`. + @override def create_tree( self, name: str, - action_type: type, tree_root_name: str, - node: Node, ) -> py_trees.trees.BehaviourTree: - """ - Creates the MoveAbovePlate behavior tree. - - Currently, this only has one behavior in it, MoveToDummy. Eventually, - this should be replaced with a behavior tree that actually moves the - robot arm above the plate. - - Parameters - ---------- - name: The name of the behavior tree. - action_type: the type for the action, as a class. - tree_root_name: The name of the tree. This is necessary because sometimes - trees create subtrees, but still need to track the top-level tree - name to read/write the correct blackboard variables. - node: The ROS2 node that this tree is associated with. Necessary for - behaviors within the tree connect to ROS topics/services/actions. - - Returns - ------- - tree: The behavior tree that moves the robot above the plate. - """ - - # pylint: disable=too-many-arguments - # One over is fine. - - # Store the action type - self.action_type = action_type + # Docstring copied from @override # Create the behaviors in the tree if self.tree is None: @@ -116,40 +89,21 @@ def create_tree( return self.tree + @override def send_goal(self, tree: py_trees.trees.BehaviourTree, goal: object) -> bool: - """ - Sends the goal from the action client to the behavior tree. - - This function is called before the behavior tree is executed. - - Parameters - ---------- - tree: The behavior tree that is being executed. - goal: The ROS goal message to be sent to the behavior tree. + # Docstring copied from @override - Returns - ------- - success: Whether the goal was sent successfully. - """ # Write the goal to blackboard self.blackboard.goal = goal return True - def get_feedback(self, tree: py_trees.trees.BehaviourTree) -> object: - """ - Traverses the tree to generate a feedback message for the MoveTo action. - - This function is used as a post-tick handler for the behavior tree. - - Parameters - ---------- - tree: The behavior tree that is being executed. + @override + def get_feedback( + self, tree: py_trees.trees.BehaviourTree, action_type: type + ) -> object: + # Docstring copied from @override - Returns - ------- - feedback: The ROS feedback message to be sent to the action client. - """ - feedback_msg = self.action_type.Feedback() + feedback_msg = action_type.Feedback() if self.blackboard.exists("is_planning"): feedback_msg.is_planning = self.blackboard.is_planning planning_time = self.blackboard.planning_time @@ -169,21 +123,13 @@ def get_feedback(self, tree: py_trees.trees.BehaviourTree) -> object: feedback_msg.motion_curr_distance = self.blackboard.motion_curr_distance return feedback_msg - def get_result(self, tree: py_trees.trees.BehaviourTree) -> object: - """ - Traverses the tree to generate a result message for the MoveTo action. - - This function is called after the behavior tree terminates. + @override + def get_result( + self, tree: py_trees.trees.BehaviourTree, action_type: type + ) -> object: + # Docstring copied from @override - Parameters - ---------- - tree: The behavior tree that is being executed. - - Returns - ------- - result: The ROS result message to be sent to the action client. - """ - result = self.action_type.Result() + result = action_type.Result() # If the tree succeeded, return success if tree.root.status == py_trees.common.Status.SUCCESS: result.status = result.STATUS_SUCCESS diff --git a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py index ed54fc63..2592f810 100644 --- a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py @@ -55,6 +55,7 @@ class MoveToMouthTree(MoveToTree): def __init__( self, + node: Node, staging_configuration: List[float], staging_configuration_tolerance: float = 0.001, mouth_pose_tolerance: float = 0.001, @@ -120,7 +121,7 @@ def __init__( # These are all necessary due to all the behaviors MoveToMouth contains # Initialize MoveToTree - super().__init__() + super().__init__(node) # Store the parameters self.staging_configuration = staging_configuration diff --git a/ada_feeding/ada_feeding/trees/move_to_pose_tree.py b/ada_feeding/ada_feeding/trees/move_to_pose_tree.py index 7cf77d3c..89f02d58 100644 --- a/ada_feeding/ada_feeding/trees/move_to_pose_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_pose_tree.py @@ -6,6 +6,7 @@ """ # Standard imports +from overrides import override from typing import Optional, Set, Tuple, Union # Third-party imports @@ -47,6 +48,7 @@ class MoveToPoseTree(MoveToTree): # A mutable default value is okay since we don't change it in this function. def __init__( self, + node: Node, position: Optional[Tuple[float, float, float]] = None, quat_xyzw: Optional[Tuple[float, float, float, float]] = None, frame_id: Optional[str] = None, @@ -105,7 +107,7 @@ def __init__( tree, this should be False. Else (e.g., if this is a standalone tree), True. """ # Initialize MoveTo - super().__init__() + super().__init__(node) # Store the parameters for the move to pose behavior self.position = position @@ -129,31 +131,18 @@ def __init__( self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard self.clear_constraints = clear_constraints - # pylint: disable=too-many-locals, too-many-statements - # Unfortunately, many locals/statements are required here to isolate the keys - # of similar constraints in the blackboard. + @override def create_move_to_tree( self, name: str, tree_root_name: str, - node: Node, ) -> py_trees.trees.BehaviourTree: - """ - Creates the MoveToPose behavior tree. + # Docstring copied from @override - Parameters - ---------- - name: The name of the behavior tree. - tree_root_name: The name of the tree. This is necessary because sometimes - trees create subtrees, but still need to track the top-level tree - name to read/write the correct blackboard variables. - node: The ROS2 node that this tree is associated with. Necessary for - behaviors within the tree connect to ROS topics/services/actions. + # pylint: disable=too-many-locals, too-many-statements + # Unfortunately, many locals/statements are required here to isolate the keys + # of similar constraints in the blackboard. - Returns - ------- - tree: The behavior tree that moves the robot above the plate. - """ # Separate blackboard namespaces for children if self.position is not None: position_goal_constraint_namespace_prefix = ( @@ -423,7 +412,7 @@ def create_move_to_tree( # Create the MoveTo behavior move_to_name = Blackboard.separator.join([name, move_to_namespace_prefix]) - move_to = MoveTo(move_to_name, tree_root_name, node) + move_to = MoveTo(move_to_name, tree_root_name, self._node) # Add the position goal constraint to the MoveTo behavior if self.position is not None: @@ -431,7 +420,7 @@ def create_move_to_tree( [name, position_goal_constraint_namespace_prefix] ) position_constraint = SetPositionGoalConstraint( - position_goal_constaint_name, move_to, node + position_goal_constaint_name, move_to, self._node ) else: position_constraint = move_to @@ -442,7 +431,7 @@ def create_move_to_tree( [name, orientation_goal_constraint_namespace_prefix] ) orientation_constraint = SetOrientationGoalConstraint( - orientation_goal_constraint_name, position_constraint, node + orientation_goal_constraint_name, position_constraint, self._node ) else: orientation_constraint = position_constraint @@ -453,7 +442,7 @@ def create_move_to_tree( [name, clear_constraints_namespace_prefix] ) root = ClearConstraints( - clear_constraints_name, orientation_constraint, node + clear_constraints_name, orientation_constraint, self._node ) else: root = orientation_constraint diff --git a/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py b/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py index 2aedfff0..35ff1d56 100644 --- a/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py @@ -7,6 +7,7 @@ # Standard imports from typing import Optional, Set, Tuple, Union +from overrides import override # Third-party imports import py_trees @@ -35,6 +36,7 @@ class MoveToPoseWithPosePathConstraintsTree(MoveToTree): # A mutable default value is okay since we don't change it in this function. def __init__( self, + node: Node, # Required parameters for moving to a pose position_goal: Optional[Tuple[float, float, float]] = None, quat_xyzw_goal: Optional[Tuple[float, float, float, float]] = None, @@ -118,7 +120,7 @@ def __init__( tree, this should be False. Else (e.g., if this is a standalone tree), True. """ # Initialize MoveToTree - super().__init__() + super().__init__(node) # Store the parameters for the move to pose behavior self.position_goal = position_goal @@ -154,28 +156,14 @@ def __init__( self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard self.clear_constraints = clear_constraints + @override def create_move_to_tree( self, name: str, tree_root_name: str, - node: Node, ) -> py_trees.trees.BehaviourTree: - """ - Creates the MoveToPoseWithPosePathConstraintsTree behavior tree. + # Docstring Copied from @override - Parameters - ---------- - name: The name of the behavior tree. - tree_root_name: The name of the tree. This is necessary because sometimes - trees create subtrees, but still need to track the top-level tree - name to read/write the correct blackboard variables. - node: The ROS2 node that this tree is associated with. Necessary for - behaviors within the tree connect to ROS topics/services/actions. - - Returns - ------- - tree: The behavior tree that moves the robot above the plate. - """ # First, create the MoveToPose behavior tree, in the same # namespace as this tree move_to_pose_root = ( diff --git a/ada_feeding/ada_feeding/trees/move_to_tree.py b/ada_feeding/ada_feeding/trees/move_to_tree.py index de32d310..836ce76e 100644 --- a/ada_feeding/ada_feeding/trees/move_to_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_tree.py @@ -11,6 +11,7 @@ # Standard imports from abc import ABC, abstractmethod +from overrides import override # Third-party imports import py_trees @@ -30,40 +31,20 @@ class MoveToTree(ActionServerBT, ABC): # pylint: disable=attribute-defined-outside-init # It is reasonable for attributes that are tree-specific, or only relevant # after the tree is created, to be defined in `create_tree`. + @override def create_tree( self, name: str, - action_type: type, tree_root_name: str, - node: Node, ) -> py_trees.trees.BehaviourTree: - """ - Create the behavior tree. - - Parameters - ---------- - name: The name of the behavior tree. - action_type: the type for the action, as a class. - tree_root_name: The name of the tree. This is necessary because sometimes - trees create subtrees, but still need to track the top-level tree - name to read/write the correct blackboard variables. - node: The ROS2 node that this tree is associated with. Necessary for - behaviors within the tree connect to ROS topics/services/actions. - - Returns - ------- - tree: The behavior tree that moves the robot above the plate. - """ + # Docstring copied from @override # pylint: disable=too-many-arguments # One over is fine for this function. self.create_blackboard(name, tree_root_name) - # Import the action type - self.action_type = action_type - - return self.create_move_to_tree(name, tree_root_name, node) + return self.create_move_to_tree(name, tree_root_name) def create_blackboard(self, name: str, tree_root_name: str) -> None: """ @@ -111,7 +92,6 @@ def create_move_to_tree( self, name: str, tree_root_name: str, - node: Node, ) -> py_trees.trees.BehaviourTree: """ Create the behavior tree. By the time this function is called, the @@ -126,8 +106,6 @@ def create_move_to_tree( tree_root_name: The name of the tree. This is necessary because sometimes trees create subtrees, but still need to track the top-level tree name to read/write the correct blackboard variables. - node: The ROS2 node that this tree is associated with. Necessary for - behaviors within the tree connect to ROS topics/services/actions. Returns ------- @@ -135,6 +113,7 @@ def create_move_to_tree( """ raise NotImplementedError("create_move_to_tree not implemented") + @override def send_goal(self, tree: py_trees.trees.BehaviourTree, goal: object) -> bool: """ Sends the goal from the action client to the behavior tree. @@ -154,7 +133,10 @@ def send_goal(self, tree: py_trees.trees.BehaviourTree, goal: object) -> bool: self.blackboard_tree_root.goal = goal return True - def get_feedback(self, tree: py_trees.trees.BehaviourTree) -> object: + @override + def get_feedback( + self, tree: py_trees.trees.BehaviourTree, action_type: type + ) -> object: """ Traverses the tree to generate a feedback message for the MoveTo action. @@ -168,7 +150,7 @@ def get_feedback(self, tree: py_trees.trees.BehaviourTree) -> object: ------- feedback: The ROS feedback message to be sent to the action client. """ - feedback_msg = self.action_type.Feedback() + feedback_msg = action_type.Feedback() if self.blackboard_tree_root.exists("is_planning"): feedback_msg.is_planning = self.blackboard_tree_root.is_planning planning_time = self.blackboard_tree_root.planning_time @@ -192,7 +174,10 @@ def get_feedback(self, tree: py_trees.trees.BehaviourTree) -> object: ) return feedback_msg - def get_result(self, tree: py_trees.trees.BehaviourTree) -> object: + @override + def get_result( + self, tree: py_trees.trees.BehaviourTree, action_type: type + ) -> object: """ Traverses the tree to generate a result message for the MoveTo action. @@ -206,7 +191,7 @@ def get_result(self, tree: py_trees.trees.BehaviourTree) -> object: ------- result: The ROS result message to be sent to the action client. """ - result = self.action_type.Result() + result = action_type.Result() # If the tree succeeded, return success if tree.root.status == py_trees.common.Status.SUCCESS: result.status = result.STATUS_SUCCESS diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index e77b87fa..7442d789 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -364,11 +364,9 @@ def get_execute_callback( execute_callback: The callback function for the action server. """ # Initialize the ActionServerBT object once - tree_action_server = self._tree_classes[tree_class](**tree_kwargs) + tree_action_server = self._tree_classes[tree_class](self, **tree_kwargs) # Create and setup the tree once - tree = tree_action_server.create_tree( - server_name, action_type, server_name, self - ) + tree = tree_action_server.create_tree(server_name, server_name, self) self.setup_tree(tree) self._trees.append(tree) @@ -407,7 +405,7 @@ async def execute_callback(goal_handle: ServerGoalHandle) -> Awaitable: tree ) # blocks until the preempt succeeds goal_handle.canceled() - result = tree_action_server.get_result(tree) + result = tree_action_server.get_result(tree, action_type) break # Check if the watchdog has failed @@ -417,12 +415,14 @@ async def execute_callback(goal_handle: ServerGoalHandle) -> Awaitable: tree ) # blocks until the preempt succeeds goal_handle.abort() - result = tree_action_server.get_result(tree) + result = tree_action_server.get_result(tree, action_type) break # Tick the tree once and publish feedback tree.tick() - feedback_msg = tree_action_server.get_feedback(tree) + feedback_msg = tree_action_server.get_feedback( + tree, action_type + ) goal_handle.publish_feedback(feedback_msg) self.get_logger().debug(f"Publishing feedback {feedback_msg}") @@ -430,7 +430,7 @@ async def execute_callback(goal_handle: ServerGoalHandle) -> Awaitable: if tree.root.status == py_trees.common.Status.SUCCESS: self.get_logger().info("Goal succeeded") goal_handle.succeed() - result = tree_action_server.get_result(tree) + result = tree_action_server.get_result(tree, action_type) break if tree.root.status in set( ( @@ -440,7 +440,7 @@ async def execute_callback(goal_handle: ServerGoalHandle) -> Awaitable: ): self.get_logger().info("Goal failed") goal_handle.abort() - result = tree_action_server.get_result(tree) + result = tree_action_server.get_result(tree, action_type) break # Sleep From a1b7b7d436dcd1440593d469fb066754e9fc146c Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Thu, 28 Sep 2023 09:46:17 -0700 Subject: [PATCH 3/9] [WIP] Ran pylint --- ada_feeding/ada_feeding/action_server_bt.py | 4 +- .../acquisition/compute_move_above.py | 114 ------------------ .../ada_feeding/trees/acquire_food_tree.py | 52 +++++--- .../ada_feeding/trees/move_from_mouth_tree.py | 2 +- .../trees/move_to_configuration_tree.py | 2 +- ...o_configuration_with_ft_thresholds_tree.py | 2 +- ...uration_with_pose_path_constraints_tree.py | 2 +- .../ada_feeding/trees/move_to_dummy_tree.py | 2 +- .../ada_feeding/trees/move_to_mouth_tree.py | 39 ++---- .../ada_feeding/trees/move_to_pose_tree.py | 2 +- ...to_pose_with_pose_path_constraints_tree.py | 7 +- ada_feeding/ada_feeding/trees/move_to_tree.py | 3 +- 12 files changed, 58 insertions(+), 173 deletions(-) delete mode 100644 ada_feeding/ada_feeding/behaviors/acquisition/compute_move_above.py diff --git a/ada_feeding/ada_feeding/action_server_bt.py b/ada_feeding/ada_feeding/action_server_bt.py index 71c91f12..da358f74 100644 --- a/ada_feeding/ada_feeding/action_server_bt.py +++ b/ada_feeding/ada_feeding/action_server_bt.py @@ -19,12 +19,12 @@ class ActionServerBT(ABC): `create_action_server.py` """ - def __init__(self, node: Node, **kwargs) -> None: + def __init__(self, node: Node) -> None: """ Store the ROS2 Node that created trees is associated with. Necessary for behaviors within the tree to connect to ROS topics/services/actions. - Subclasses should overwrite kwargs with tree-agnostic + Subclasses should add kwargs with tree-agnostic input parameters. """ self._node = node diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/compute_move_above.py b/ada_feeding/ada_feeding/behaviors/acquisition/compute_move_above.py deleted file mode 100644 index ba9eeaac..00000000 --- a/ada_feeding/ada_feeding/behaviors/acquisition/compute_move_above.py +++ /dev/null @@ -1,114 +0,0 @@ -#!/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 diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index fe303cdc..0353fc44 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -6,11 +6,10 @@ """ # Standard imports -from overrides import override # Third-party imports +from overrides import override import py_trees -from rclpy.node import Node # Local imports from ada_feeding import ActionServerBT @@ -81,13 +80,13 @@ def send_goal(self, tree: py_trees.trees.BehaviourTree, goal: object) -> bool: # Write tree inputs to blackboard name = tree.root.name blackboard = py_trees.blackboard.Client(name=name, namespace=name) - self.blackboard.mask = goal.detected_food - self.blackboard.camera_info = goal.camera_info + blackboard.register_key(key="mask", access=py_trees.common.Access.WRITE) + blackboard.mask = goal.detected_food + blackboard.register_key(key="camera_info", access=py_trees.common.Access.WRITE) + blackboard.camera_info = goal.camera_info - # Top level tree - # Add MoveIt2Visitor for Feedback: - self.feedback_visitor = MoveIt2Visitor(self._node) - tree.add_visitor(self.feedback_visitor) + # Add MoveIt2Visitor for Feedback + tree.add_visitor(MoveIt2Visitor(self._node)) return True @@ -98,19 +97,26 @@ def get_feedback( ) -> object: # Docstring copied by @override # Note: if here, tree is root, not a subtree + # TODO: This Feedback/Result logic w/ MoveIt2Visitor can exist in MoveToTree right now if action_type is not AcquireFood: return None feedback_msg = action_type.Feedback() + # Get Feedback Visitor + feedback_visitor = None + for visitor in tree.visitors: + if isinstance(visitor, MoveIt2Visitor): + feedback_visitor = visitor + # Copy everything from the visitor - # TODO: This Feedback/Result logic w/ MoveIt2Visitor can exist in MoveToTree right now - feedback = self.feedback_visitor.get_feedback() - feedback_msg.is_planning = feedback.is_planning - feedback_msg.planning_time = feedback.planning_time - feedback_msg.motion_time = feedback.motion_time - feedback_msg.motion_initial_distance = feedback.motion_initial_distance - feedback_msg.motion_curr_distance = feedback.motion_curr_distance + if feedback_visitor is not None: + feedback = feedback_visitor.get_feedback() + feedback_msg.is_planning = feedback.is_planning + feedback_msg.planning_time = feedback.planning_time + feedback_msg.motion_time = feedback.motion_time + feedback_msg.motion_initial_distance = feedback.motion_initial_distance + feedback_msg.motion_curr_distance = feedback.motion_curr_distance return feedback_msg @@ -131,11 +137,19 @@ def get_result( result_msg.status = result_msg.STATUS_SUCCESS # If the tree failed, detemine whether it was a planning or motion failure elif tree.root.status == py_trees.common.Status.FAILURE: - feedback = self.feedback_visitor.get_feedback() - if feedback.is_planning: - result_msg.status = result_msg.STATUS_PLANNING_FAILED + # Get Feedback Visitor to investigate failure cause + feedback_visitor = None + for visitor in tree.visitors: + if isinstance(visitor, MoveIt2Visitor): + feedback_visitor = visitor + if feedback_visitor is None: + result_msg.status = result_msg.STATUS_UNKNOWN else: - result_msg.status = result_msg.STATUS_MOTION_FAILED + feedback = feedback_visitor.get_feedback() + if feedback.is_planning: + result_msg.status = result_msg.STATUS_PLANNING_FAILED + else: + result_msg.status = result_msg.STATUS_MOTION_FAILED # If the tree has an invalid status, return unknown elif tree.root.status == py_trees.common.Status.INVALID: result_msg.status = result_msg.STATUS_UNKNOWN diff --git a/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py index e99251bb..9a5378ff 100644 --- a/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py +++ b/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py @@ -10,9 +10,9 @@ # Standard imports from typing import List, Tuple -from overrides import override # Third-party imports +from overrides import override import py_trees from py_trees.blackboard import Blackboard from rclpy.node import Node diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py index f1cd1162..4dd7ea4b 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py @@ -7,9 +7,9 @@ # Standard imports from typing import List, Set -from overrides import override # Third-party imports +from overrides import override import py_trees from py_trees.blackboard import Blackboard from rclpy.node import Node diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py index ed6a5760..917e55c2 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py @@ -7,9 +7,9 @@ # Standard imports from typing import List, Set -from overrides import override # Third-party imports +from overrides import override import py_trees from rclpy.node import Node diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py index ed34a2ca..0ceea4b8 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py @@ -7,9 +7,9 @@ # Standard imports from typing import List, Optional, Tuple, Set, Union -from overrides import override # Third-party imports +from overrides import override import py_trees from rclpy.node import Node diff --git a/ada_feeding/ada_feeding/trees/move_to_dummy_tree.py b/ada_feeding/ada_feeding/trees/move_to_dummy_tree.py index f1c43768..ba948e64 100644 --- a/ada_feeding/ada_feeding/trees/move_to_dummy_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_dummy_tree.py @@ -6,9 +6,9 @@ """ # Standard imports -from overrides import override # Third-party imports +from overrides import override import py_trees from rclpy.node import Node diff --git a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py index 2592f810..eacf5ded 100644 --- a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py @@ -12,6 +12,7 @@ from typing import List, Tuple # Third-party imports +from overrides import override import py_trees from py_trees.blackboard import Blackboard import py_trees_ros @@ -181,28 +182,14 @@ def check_face_msg(self, msg: FaceDetection, _: FaceDetection) -> bool: return True return False + @override def create_move_to_tree( self, name: str, tree_root_name: str, - node: Node, ) -> py_trees.trees.BehaviourTree: - """ - Creates the MoveToMouth behaviour tree. - - Parameters - ---------- - name: The name of the behaviour tree. - tree_root_name: The name of the tree. This is necessary because sometimes - trees create subtrees, but still need to track the top-level tree - name to read/write the correct blackboard variables. - node: The ROS2 node that this tree is associated with. Necessary for - behaviours within the tree connect to ROS topics/services/actions. + # Docstring copied from @override - Returns - ------- - tree: The behaviour tree that moves the robot above the plate. - """ # pylint: disable=too-many-locals, too-many-statements # This function creates all the behaviors of the tree, which is why # it has so many locals/statements. @@ -247,7 +234,7 @@ def create_move_to_tree( name, add_wheelchair_wall_prefix, in_front_of_wheelchair_wall_id, - node, + self._node, self.blackboard, ) # Create the behaviour to move the robot to the staging configuration @@ -256,6 +243,7 @@ def create_move_to_tree( ) move_to_staging_configuration = ( MoveToConfigurationWithPosePathConstraintsTree( + self._node, joint_positions_goal=self.staging_configuration, tolerance_joint_goal=self.staging_configuration_tolerance, planner_id=self.planner_id, @@ -269,9 +257,7 @@ def create_move_to_tree( ) .create_tree( move_to_staging_configuration_name, - self.action_type, tree_root_name, - node, ) .root ) @@ -282,7 +268,7 @@ def create_move_to_tree( name, remove_wheelchair_wall_prefix, in_front_of_wheelchair_wall_id, - node, + self._node, ) ) @@ -339,7 +325,7 @@ def create_move_to_tree( target_position_offset = (0.0, -0.05, 0.0) compute_target_position = ComputeMoveToMouthPosition( name=compute_target_position_name, - node=node, + node=self._node, face_detection_input_key="/face_detection", target_position_output_key=target_position_output_key, target_position_frame_id="j2n6s200_link_base", @@ -351,7 +337,7 @@ def create_move_to_tree( # of the wheelchair. move_head = ModifyCollisionObject( name=Blackboard.separator.join([name, move_head_prefix]), - node=node, + node=self._node, operation=ModifyCollisionObjectOperation.MOVE, collision_object_id=self.head_object_id, collision_object_position_input_key=target_position_output_key, @@ -384,7 +370,7 @@ def create_move_to_tree( allow_wheelchair_collision = get_toggle_collision_object_behavior( name, allow_wheelchair_collision_prefix, - node, + self._node, [self.wheelchair_collision_object_id], True, ) @@ -402,6 +388,7 @@ def create_move_to_tree( ) move_to_target_pose = ( MoveToPoseWithPosePathConstraintsTree( + self._node, position_goal=(0.0, 0.0, 0.0), quat_xyzw_goal=(0.0, 0.7071068, 0.7071068, 0.0), tolerance_position_goal=self.mouth_pose_tolerance, @@ -423,9 +410,7 @@ def create_move_to_tree( ), }, ) - .create_tree( - move_to_target_pose_name, self.action_type, tree_root_name, node - ) + .create_tree(move_to_target_pose_name, tree_root_name) .root ) @@ -434,7 +419,7 @@ def create_move_to_tree( disallow_wheelchair_collision = get_toggle_collision_object_behavior( name, disallow_wheelchair_collision_prefix, - node, + self._node, [self.wheelchair_collision_object_id], False, ) diff --git a/ada_feeding/ada_feeding/trees/move_to_pose_tree.py b/ada_feeding/ada_feeding/trees/move_to_pose_tree.py index 89f02d58..50888632 100644 --- a/ada_feeding/ada_feeding/trees/move_to_pose_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_pose_tree.py @@ -6,10 +6,10 @@ """ # Standard imports -from overrides import override from typing import Optional, Set, Tuple, Union # Third-party imports +from overrides import override import py_trees from py_trees.blackboard import Blackboard from rclpy.node import Node diff --git a/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py b/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py index 35ff1d56..de7f849c 100644 --- a/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py @@ -7,9 +7,9 @@ # Standard imports from typing import Optional, Set, Tuple, Union -from overrides import override # Third-party imports +from overrides import override import py_trees from rclpy.node import Node @@ -168,6 +168,7 @@ def create_move_to_tree( # namespace as this tree move_to_pose_root = ( MoveToPoseTree( + self._node, position=self.position_goal, quat_xyzw=self.quat_xyzw_goal, frame_id=self.frame_id_goal, @@ -189,7 +190,7 @@ def create_move_to_tree( keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard, clear_constraints=False, ) - .create_tree(name, self.action_type, tree_root_name, node) + .create_tree(name, tree_root_name) .root ) @@ -207,7 +208,7 @@ def create_move_to_tree( parameterization_orientation_path=self.parameterization_orientation_path, weight_position_path=self.weight_position_path, weight_orientation_path=self.weight_orientation_path, - node=node, + node=self._node, clear_constraints=self.clear_constraints, ) diff --git a/ada_feeding/ada_feeding/trees/move_to_tree.py b/ada_feeding/ada_feeding/trees/move_to_tree.py index 836ce76e..f5a36691 100644 --- a/ada_feeding/ada_feeding/trees/move_to_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_tree.py @@ -11,11 +11,10 @@ # Standard imports from abc import ABC, abstractmethod -from overrides import override # Third-party imports +from overrides import override import py_trees -from rclpy.node import Node # Local imports from ada_feeding import ActionServerBT From 7d2d79c8432c638ba6b40c588de49ead5a806213 Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Thu, 28 Sep 2023 09:58:19 -0700 Subject: [PATCH 4/9] Addressed Draft PR comments --- .../acquisition/compute_food_frame.py | 4 ++-- ada_feeding/ada_feeding/helpers.py | 4 ++-- .../ada_feeding/trees/acquire_food_tree.py | 18 ++++++++++++------ ada_feeding/ada_feeding/visitors/__init__.py | 2 +- .../{moveit2_visitor.py => moveto_visitor.py} | 11 ++++++++++- 5 files changed, 27 insertions(+), 12 deletions(-) rename ada_feeding/ada_feeding/visitors/{moveit2_visitor.py => moveto_visitor.py} (90%) diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py b/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py index d1732b8b..621c85fc 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py @@ -25,7 +25,7 @@ BlackboardKey, quat_between_vectors, get_tf_object, - add_update_static_tf, + set_static_tf, ) from ada_feeding.behaviors import BlackboardBehavior from ada_feeding_perception.helpers import ros_msg_to_cv2_image @@ -250,7 +250,7 @@ def update(self) -> py_trees.common.Status: # Write to blackboard outputs if len(self.blackboard_get("food_frame_id")) > 0: - add_update_static_tf(world_to_food_transform, self.blackboard, node) + set_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 diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index 1630496c..23ea4652 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -93,13 +93,13 @@ def quat_between_vectors(vec_from: Vector3, vec_to: Vector3) -> Quaternion: return ret -def add_update_static_tf( +def set_static_tf( transform_stamped: TransformStamped, blackboard: py_trees.blackboard.Client, node: Optional[Node] = None, ) -> bool: """ - Sends a given transform to /tf_static. + Adds a transform to the list sent 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. diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 0353fc44..b1064e65 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -15,7 +15,7 @@ from ada_feeding import ActionServerBT from ada_feeding.behaviors.acquisition import ComputeFoodFrame from ada_feeding.helpers import BlackboardKey -from ada_feeding.visitors import MoveIt2Visitor +from ada_feeding.visitors import MoveToVisitor from ada_feeding_msgs.action import AcquireFood @@ -85,8 +85,14 @@ def send_goal(self, tree: py_trees.trees.BehaviourTree, goal: object) -> bool: blackboard.register_key(key="camera_info", access=py_trees.common.Access.WRITE) blackboard.camera_info = goal.camera_info - # Add MoveIt2Visitor for Feedback - tree.add_visitor(MoveIt2Visitor(self._node)) + # Add MoveToVisitor for Feedback + feedback_visitor = None + for visitor in tree.visitors: + if isinstance(visitor, MoveToVisitor): + visitor.reinit() + feedback_visitor = visitor + if feedback_visitor is None: + tree.add_visitor(MoveToVisitor(self._node)) return True @@ -97,7 +103,7 @@ def get_feedback( ) -> object: # Docstring copied by @override # Note: if here, tree is root, not a subtree - # TODO: This Feedback/Result logic w/ MoveIt2Visitor can exist in MoveToTree right now + # TODO: This Feedback/Result logic w/ MoveToVisitor can exist in MoveToTree right now if action_type is not AcquireFood: return None @@ -106,7 +112,7 @@ def get_feedback( # Get Feedback Visitor feedback_visitor = None for visitor in tree.visitors: - if isinstance(visitor, MoveIt2Visitor): + if isinstance(visitor, MoveToVisitor): feedback_visitor = visitor # Copy everything from the visitor @@ -140,7 +146,7 @@ def get_result( # Get Feedback Visitor to investigate failure cause feedback_visitor = None for visitor in tree.visitors: - if isinstance(visitor, MoveIt2Visitor): + if isinstance(visitor, MoveToVisitor): feedback_visitor = visitor if feedback_visitor is None: result_msg.status = result_msg.STATUS_UNKNOWN diff --git a/ada_feeding/ada_feeding/visitors/__init__.py b/ada_feeding/ada_feeding/visitors/__init__.py index 37f0326a..acb6ab5e 100644 --- a/ada_feeding/ada_feeding/visitors/__init__.py +++ b/ada_feeding/ada_feeding/visitors/__init__.py @@ -3,4 +3,4 @@ project. All of these extend VisitorBase. """ -from .moveit2_visitor import MoveIt2Visitor +from .moveto_visitor import MoveToVisitor diff --git a/ada_feeding/ada_feeding/visitors/moveit2_visitor.py b/ada_feeding/ada_feeding/visitors/moveto_visitor.py similarity index 90% rename from ada_feeding/ada_feeding/visitors/moveit2_visitor.py rename to ada_feeding/ada_feeding/visitors/moveto_visitor.py index 7b225272..4bd19b98 100644 --- a/ada_feeding/ada_feeding/visitors/moveit2_visitor.py +++ b/ada_feeding/ada_feeding/visitors/moveto_visitor.py @@ -20,7 +20,7 @@ from ada_feeding_msgs.action import MoveTo -class MoveIt2Visitor(VisitorBase): +class MoveToVisitor(VisitorBase): """ A BT Visitor that computes the feedback used in MoveTo.action. Can be used in all actions that return similar information. @@ -39,6 +39,15 @@ def __init__(self, node: Node) -> None: self.feedback = MoveTo.Feedback() self.feedback.is_planning = True + def reinit(self) -> None: + """ + Reset all local variables. + Can be called if a tree is run again. + """ + self.start_time = None + self.feedback = MoveTo.Feedback() + self.feedback.is_planning = True + @override def run(self, behaviour: py_trees.behaviour.Behaviour) -> None: # Docstring copied by @override From 1b8d985c9795f5b83f03cb3c2f92e6ca2494ebc0 Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Thu, 28 Sep 2023 10:05:41 -0700 Subject: [PATCH 5/9] Fixed bug, tested AcquireFood, TODO test other trees --- ada_feeding/scripts/create_action_servers.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index 7442d789..146afe21 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -366,7 +366,7 @@ def get_execute_callback( # Initialize the ActionServerBT object once tree_action_server = self._tree_classes[tree_class](self, **tree_kwargs) # Create and setup the tree once - tree = tree_action_server.create_tree(server_name, server_name, self) + tree = tree_action_server.create_tree(server_name, server_name) self.setup_tree(tree) self._trees.append(tree) From 51cf7ba11ff02f851b7f3918d08b5c6ba94b21eb Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Thu, 28 Sep 2023 10:21:35 -0700 Subject: [PATCH 6/9] Tested all MoveTo actions w/o perception --- .../acquisition/compute_food_frame.py | 3 +- .../ada_feeding/trees/acquire_food_tree.py | 32 +++++++++---------- 2 files changed, 17 insertions(+), 18 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py b/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py index 621c85fc..a80ce198 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py @@ -129,14 +129,15 @@ def initialise(self): self.intrinsics.fy = camera_info.k[4] if camera_info.distortion_model == "plumb_bob": self.intrinsics.model = pyrealsense2.distortion.brown_conrady + self.intrinsics.coeffs = list(camera_info.d) elif camera_info.distortion_model == "equidistant": self.intrinsics.model = pyrealsense2.distortion.kannala_brandt4 + self.intrinsics.coeffs = list(camera_info.d) else: self.logger.warning( f"Unsupported camera distortion model: {camera_info.distortion_model}" ) self.intrinsics.model = pyrealsense2.distortion.none - self.intrinsics.coeffs = list(camera_info.d) def update(self) -> py_trees.common.Status: """ diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index b1064e65..631c84da 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -37,22 +37,6 @@ def create_tree( # TODO: remove tree_root_name # Sub-trees in general should not need knowledge of their parent. - ### Define Tree Leaves and Subtrees - - # Add ComputeFoodFrame - compute_food_frame = ComputeFoodFrame( - "ComputeFoodFrame", - name, - inputs={ - "ros2_node": self._node, - "camera_info": BlackboardKey("camera_info"), - "mask": BlackboardKey("mask") - # Default food_frame_id = "food" - # Default world_frame = "world" - }, - outputs={"action_select_request": None, "food_frame": None}, - ) - ### Define Tree Logic # Root Sequence @@ -60,7 +44,21 @@ def create_tree( name=name, memory=True, children=[ - compute_food_frame, + py_trees.decorators.Timeout( + name="ComputeFoodFrameTimeout", + child=ComputeFoodFrame( + name="ComputeFoodFrame", + ns=name, + inputs={ + "ros2_node": self._node, + "camera_info": BlackboardKey("camera_info"), + "mask": BlackboardKey("mask") + # Default food_frame_id = "food" + # Default world_frame = "world" + }, + outputs={"action_select_request": None, "food_frame": None}, + ), + ) ], ) From b47d8922898f127cd43b0066cbdb3a1ed58f510e Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Thu, 28 Sep 2023 11:14:02 -0700 Subject: [PATCH 7/9] Switch ComputeFoodFrame to get node from setup() --- .../acquisition/compute_food_frame.py | 19 ++++++++++--------- ada_feeding/ada_feeding/helpers.py | 7 +++---- .../ada_feeding/trees/acquire_food_tree.py | 8 +++++++- 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py b/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py index a80ce198..46b1c35d 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py @@ -11,10 +11,10 @@ import cv2 as cv from geometry_msgs.msg import PointStamped, TransformStamped, Vector3Stamped import numpy as np +from overrides import override import py_trees import pyrealsense2 import rclpy -from rclpy.node import Node from sensor_msgs.msg import CameraInfo import tf2_ros @@ -48,7 +48,6 @@ class ComputeFoodFrame(BlackboardBehavior): def blackboard_inputs( self, - ros2_node: Union[BlackboardKey, Node], camera_info: Union[BlackboardKey, CameraInfo], mask: Union[BlackboardKey, Mask], food_frame_id: Union[BlackboardKey, str] = "food", @@ -59,7 +58,6 @@ def blackboard_inputs( Parameters ---------- - 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 @@ -93,6 +91,7 @@ def blackboard_outputs( **{key: value for key, value in locals().items() if key != "self"} ) + @override def setup(self, **kwargs): """ Middleware (i.e. TF) setup @@ -102,11 +101,13 @@ def setup(self, **kwargs): # It is okay for attributes in behaviors to be # defined in the setup / initialise functions. + # Get Node from Kwargs + self.node = kwargs["node"] + # Get TF Listener from blackboard - self.tf_buffer, _, self.tf_lock = get_tf_object( - self.blackboard, self.blackboard_get("ros2_node") - ) + self.tf_buffer, _, self.tf_lock = get_tf_object(self.blackboard, self.node) + @override def initialise(self): """ Behavior initialization @@ -139,6 +140,7 @@ def initialise(self): ) self.intrinsics.model = pyrealsense2.distortion.none + @override def update(self) -> py_trees.common.Status: """ Behavior tick (DO NOT BLOCK) @@ -152,7 +154,6 @@ def update(self) -> py_trees.common.Status: # to ROS2 msg types, which take 3-4 statements each. camera_frame = self.blackboard_get("camera_info").header.frame_id - node = self.blackboard_get("ros2_node") world_frame = self.blackboard_get("world_frame") # Lock TF Buffer @@ -179,7 +180,7 @@ def update(self) -> py_trees.common.Status: # Set up return objects world_to_food_transform = TransformStamped() - world_to_food_transform.header.stamp = node.get_clock().now().to_msg() + world_to_food_transform.header.stamp = self.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("food_frame_id") @@ -251,7 +252,7 @@ def update(self) -> py_trees.common.Status: # Write to blackboard outputs if len(self.blackboard_get("food_frame_id")) > 0: - set_static_tf(world_to_food_transform, self.blackboard, node) + set_static_tf(world_to_food_transform, self.blackboard, self.node) self.blackboard_set("food_frame", world_to_food_transform) request = AcquisitionSelect.Request() request.food_context = mask diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index 23ea4652..af774bc7 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -109,8 +109,8 @@ def set_static_tf( Parameters ---------- - transform_stamped: Transform to publish (will overwrite any transform with identical - transform_stamped.header.frame_id and transform_stamped.child_frame_id) + transform_stamped: Transform to publish (will overwrite a transform with an identical + 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. @@ -165,9 +165,8 @@ def set_static_tf( 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 + transforms[transform_stamped.child_frame_id] = transform_stamped blackboard.set(static_tf_transforms_blackboard_key, transforms) stb.sendTransform(list(transforms.values())) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 631c84da..48b1e765 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -24,6 +24,13 @@ class AcquireFoodTree(ActionServerBT): A behvaior tree to select and execute an acquisition action (see ada_feeding_msgs.action.AcquisitionSchema) for a given food mask in ada_feeding_msgs.action.AcquireFood. + + Tree Blackboard Inputs: + - camera_info: See ComputeFoodFrame + - mask: See ComputeFoodFrame + + Tree Blackboard Outputs: + """ @override @@ -50,7 +57,6 @@ def create_tree( name="ComputeFoodFrame", ns=name, inputs={ - "ros2_node": self._node, "camera_info": BlackboardKey("camera_info"), "mask": BlackboardKey("mask") # Default food_frame_id = "food" From 478163e83a411a71261a239469488dfd7dc59076 Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Thu, 28 Sep 2023 11:17:19 -0700 Subject: [PATCH 8/9] Fixed comment nit --- ada_feeding/ada_feeding/action_server_bt.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding/ada_feeding/action_server_bt.py b/ada_feeding/ada_feeding/action_server_bt.py index da358f74..86639214 100644 --- a/ada_feeding/ada_feeding/action_server_bt.py +++ b/ada_feeding/ada_feeding/action_server_bt.py @@ -126,6 +126,6 @@ def get_result( Returns ------- - The ROS feedback message to be sent to the action client, type action_type.Result() + The ROS result message to be sent to the action client, type action_type.Result() """ raise NotImplementedError("get_result not implemented") From d17ad78c2a0727b1c3da73227a49c874cb3be6ec Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Thu, 28 Sep 2023 12:04:27 -0700 Subject: [PATCH 9/9] Final nit --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 1 + 1 file changed, 1 insertion(+) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 48b1e765..0ee9f63d 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -52,6 +52,7 @@ def create_tree( memory=True, children=[ py_trees.decorators.Timeout( + duration=1.0, name="ComputeFoodFrameTimeout", child=ComputeFoodFrame( name="ComputeFoodFrame",