Add BlackboardBehavior and initial AcquireFood tree #102

egordon · 2023-09-21T02:26:34Z

Description

Adds class behaviors.BlackboardBehavior, which exposes a concise way to map blackboard keys to local strings.
Adds and hooks up the AcquireFood tree to the AcquireFood action server
Creates the behavior ComputeFoodFrame which, given the goal, outputs the food reference frame. It also (optionally) publishes this frame to the TF tree for debugging purposes.
Adds ComputeFoodFrame to the AcquireFoodTree
Uses send_goal to write specific goal arguments to the local blackboard.

Proposal:
In general, I think goal and result variables should be read from / written to the current tree blackboard namespace. This better enables subtree inclusion, where previous nodes explicitly write subtree inputs to that subtree's local blackboard and read subtree outputs from that subtree's local blackboard namespace.

For right now, I think it's okay for feedback to utilize the tree root blackboard. However, I think it would be better going forward for us to attach a FeedbackVisitor to construct it every tick instead. Feedback in theory should ideally only be dependent on:

The currently running Behavior
The blackboard attached to the currently running Behavior
The previous Feedback

Note that, for subtrees, get_feedback will never be called, so get_feedback, get_result, send_goal should be free to assume that they are running on the main tree and act accordingly.

The upshot of all of this is that tree_root_name becomes deprecated, and all trees can treat their own self.blackboard as the root scope.

Testing procedure

Use MoveAbovePlate before AcquireFood feeding_web_interface#89 (if not merged)
Run the Mock FT Sensor: ros2 run ada_feeding dummy_ft_sensor.py
Run the action servers: ros2 launch ada_feeding ada_feeding_launch.xml use_estop:=false
Run MoveIt in sim: ros2 launch ada_moveit demo.launch.py sim:=mock
Run this code with the default dummy carrot (oriented horizontally in the camera frame): ros2 launch feeding_web_app_ros2_test feeding_dummy_acquirefood_launch.py
Verify the Goal Succeeds (status=0). Open RViz and add an Axis Frame "food" (rescale to 0.1 for visibility), verify that +Z is against gravity and +X is horizontal relative to the camera.
Run this code with the dummy carrot 2 (oriented ~vertically in the camera frame): ros2 launch feeding_web_app_ros2_test feeding_dummy_acquirefood_launch.py request:=above_plate_2_carrot_request.pkl
Verify the Goal Succeeds (status=0). Open RViz and add an Axis Frame "food" (rescale to 0.1 for visibility), verify that +Z is against gravity and +X is vertical relative to the camera and to the "lower right" in the camera frame.

Before opening a pull request

Format your code using black formatter python3 -m black .
Run your code through pylint and address all warnings/errors. The only warnings that are acceptable to not address is TODOs that should be addressed in a future PR. From the top-level ada_feeding directory, run: pylint --recursive=y --rcfile=.pylintrc ..

Before Merging

Squash & Merge

amalnanavati

LGTM, comments should be addressed, but nothing that requires a re-review.

Re. your idea of writing goals in local namespace as opposed to the tree root namespace, I think the only scenario in which that matters is if we use a tree within another tree, and I'm not sure how we would percolate the goal down into every subtree's blackboard. We should discuss more asynch (e.g., on a GitHub issue). For Feedback and Result, I agree that we can create a visitor for those.

amalnanavati · 2023-09-22T18:47:37Z

ada_feeding/ada_feeding/behaviors/blackboard_behavior.py

+
+class BlackboardBehavior(py_trees.behaviour.Behaviour):
+    """
+    TODO


Please add to the class docstring the following:

self.remap, self.inputs, and self.outputs should not be overridden. I was hoping name mangling could allow us to prevent them from being overridden, but unfortunately that won't work (see example).

blackboard values should only be accessed via blackboard_exists, blackboard_get, and blackboard_write, not by accessing self.blackboard directly.

amalnanavati · 2023-09-22T18:55:34Z

ada_feeding/ada_feeding/behaviors/blackboard_behavior.py

+
+    def blackboard_inputs(self, **kwargs) -> None:
+        """
+        Define and register all blackboard input keys.


Can you add an English explanation of what this does, in addition to the pseudocode explanation before? e.g.,

Each key of the kwargs is a str specifying the location on the blackboard for that variable to be stored. Each value of kwargs is either `BlackboardKey`, in which case it specifies a blackboard location to remap the key to, or another type, in which case it specifies a constant value to store at that key. Note that as opposed to setting constants on the blackboard, this behavior stores it in a local dict.

amalnanavati · 2023-09-22T19:07:40Z

ada_feeding/ada_feeding/behaviors/blackboard_behavior.py

+
+        return self.blackboard.get(self.remap[key])
+
+    def blackboard_write(self, key: str, output: Any) -> None:


Why blackboard_write instead of blackboard_set (which would seem to match better with blackboard_get)?

Or blackboard_read instead of blackboard_get. Basically I think we should either use read/write or get/set, not both.

Switch to blackboard_set, using get/set to match the underlying API

amalnanavati · 2023-09-22T19:10:51Z

ada_feeding/ada_feeding/behaviors/blackboard_behavior.py

+
+    def blackboard_exists(self, key: str) -> bool:
+        """
+        Check if a key is set in the blackboard or available locally.


Add to blackboard_exists, blackboard_get, blackboard_write docstrings: Raises KeyError if the key has not been defined in blackboard_inputs/outputs

amalnanavati · 2023-09-22T19:16:01Z

ada_feeding/ada_feeding/behaviors/blackboard_behavior.py

+        <key>: BlackboardKey = <default>
+
+        Call from the subclass using:
+        super().blackboard_inputs(**{key: value for key,


outputs, not inputs

amalnanavati · 2023-09-22T19:40:20Z

ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py

    """
-    TODO
+    Computes the food reference frame.


Please define the food reference frame here, or point to the file where it is defined (I believe in one of the message definitions).

amalnanavati · 2023-09-22T19:42:50Z

ada_feeding/ada_feeding/behaviors/blackboard_behavior.py

+            raise KeyError(f"{key} is not a registered output: {self.outputs.keys()}")
+
+        if self.outputs[key] is not None:
+            self.blackboard.set(self.outputs[key], output)


I think we should log an error/warning if blackboard_write is called when the default value was None. A programmer may have just forgotten to change the default output value, and a warning can help surface that issue.

It's expected behavior, so I'll instead write it to debug()

amalnanavati · 2023-09-22T19:44:38Z

ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py

+        request.food_context = mask
+        self.blackboard_write("action_select_request", request)
+
+        return py_trees.common.Status.SUCCESS


In practice, once we have the transform how long does it take to do the computations? Because the tree is blocked until the computations are done. If it takes a while, we should instead make it an asynch function that does the computation, and return RUNNING until that asynch function succeeds.

Timed: 0.0005796703s (i.e. 580us)
I think we're fine

amalnanavati · 2023-09-22T19:46:04Z

ada_feeding/ada_feeding/helpers.py

@@ -35,6 +54,42 @@
 MOVE_TO_NAMESPACE_PREFIX = "move_to"


+def quat_between_vectors(vec_from: Vector3, vec_to: Vector3) -> Quaternion:


Trusting that this is correct, I didn't check the math on this. In the future we can consider writing a unit test for this to check it.

Added issue #110

amalnanavati · 2023-09-22T19:49:40Z

ada_feeding/ada_feeding/trees/acquire_food_tree.py

@@ -73,9 +55,69 @@ def create_move_to_tree(
        -------
        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


Good point, feel free to create an issue for this.

Currently, self.blackboard.namespace and self.blackboard_tree_root.namespace are the ones that have the unchanged name. We should consider whether it should instead be .name.

Made issue #111

egordon · 2023-09-22T20:43:34Z

Re. your idea of writing goals in local namespace as opposed to the tree root namespace, I think the only scenario in which that matters is if we use a tree within another tree, and I'm not sure how we would percolate the goal down into every subtree's blackboard. We should discuss more asynch (e.g., on a GitHub issue). For Feedback and Result, I agree that we can create a visitor for those.

We can discuss offline, though I want to flag that we shouldn't conceive of the input of the tree being the action goal specifically. Instead, we can consider a tree to have a set of inputs, like a function, that are implemented as expected variables written to the local blackboard prior to tree execution.

For the root tree, send_goal takes the goal object and writes those variables.

For sub-trees, it is up to the parent tree to write to those variables (by having the keys be the outputs of other nodes or written directly).

egordon added 8 commits September 19, 2023 19:13

[WIP] Added blackboard behavior generic class

c1c0963

[WIP] Added ComputeFoodFrame and formatted

5eb8517

[Untested, WIP] ComputeFoodFrame tree

ff79a7b

Fix pylint errors

94b392e

Constant ros2 node, no need to add to blackboard since it is passed in

f0bd1ab

Flag new Python depends that need to be installed via pip

b7816fc

Tested, working ComputeFoodFrame

0e71139

Re-ran black and Pylint

61db2d6

egordon requested a review from amalnanavati September 21, 2023 23:56

egordon added 2 commits September 21, 2023 16:58

Add minor documentation

28fa75c

Attach blackboard client to behavior

4e19899

egordon marked this pull request as ready for review September 22, 2023 00:30

egordon requested a review from taylorkf September 22, 2023 00:32

amalnanavati approved these changes Sep 22, 2023

View reviewed changes

egordon mentioned this pull request Sep 27, 2023

[ROS2] Face Detection Accuracy Check #67

Closed

egordon added 2 commits September 26, 2023 17:30

[WIP] Addressing comments

d21216c

Merged with ros2-devel

760b1cb

egordon mentioned this pull request Sep 27, 2023

[ROS2] Add tests for quat_between_vectors #110

Open

[WIP] Addressing more comments

3608b26

egordon mentioned this pull request Sep 27, 2023

[ROS2] Deprecate name and tree_root_name in create_move_to_tree #111

Closed

Finished addressing comments and running tests, ready to merge

961baad

egordon merged commit 07b20a5 into ros2-devel Sep 27, 2023

egordon deleted the egordon/ros2_actree branch September 27, 2023 00:51

amalnanavati mentioned this pull request Sep 27, 2023

[ROS2] Update Existing Behaviors to BlackboardBehaviors #113

Closed

This was referenced Sep 28, 2023

ActionServerBT Overhaul and Add Static TF Object #114

Merged

Move2Plan/Execute (MoveTo Behavior Overhaul) #115

Merged

egordon mentioned this pull request Oct 5, 2023

AcquireFood MVP (action 0 only) #120

Merged

3 tasks

amalnanavati mentioned this pull request Oct 24, 2023

Revamp MoveToMouth/MoveFromMouth with MoveIt2Plan/Execute and BlackboardBehavior #124

Merged

17 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add BlackboardBehavior and initial AcquireFood tree #102

Add BlackboardBehavior and initial AcquireFood tree #102

egordon commented Sep 21, 2023 •

edited

Loading

amalnanavati left a comment

amalnanavati Sep 22, 2023

amalnanavati Sep 22, 2023

amalnanavati Sep 22, 2023

amalnanavati Sep 22, 2023

amalnanavati Sep 22, 2023

egordon Sep 26, 2023

amalnanavati Sep 22, 2023

amalnanavati Sep 22, 2023

amalnanavati Sep 22, 2023

amalnanavati Sep 22, 2023

egordon Sep 27, 2023

amalnanavati Sep 22, 2023

egordon Sep 27, 2023

amalnanavati Sep 22, 2023

egordon Sep 27, 2023

amalnanavati Sep 22, 2023

egordon Sep 27, 2023

egordon commented Sep 22, 2023


		return self.blackboard.get(self.remap[key])

		def blackboard_write(self, key: str, output: Any) -> None:

		@@ -35,6 +54,42 @@
		MOVE_TO_NAMESPACE_PREFIX = "move_to"


		def quat_between_vectors(vec_from: Vector3, vec_to: Vector3) -> Quaternion:

Add BlackboardBehavior and initial AcquireFood tree #102

Add BlackboardBehavior and initial AcquireFood tree #102

Conversation

egordon commented Sep 21, 2023 • edited Loading

Description

Testing procedure

Before opening a pull request

Before Merging

amalnanavati left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

egordon commented Sep 22, 2023

egordon commented Sep 21, 2023 •

edited

Loading