ActionServerBT Overhaul and Add Static TF Object #114

egordon · 2023-09-28T02:09:59Z

Description

As a pre-cursor to the overhaul of the MoveIt2 behaviors, this PR does a smaller overhaul of ActionServerBT (ASBT)

The key idea is that a given ASBT object should act as a generator, where create_tree can be called numerous times with different names and get logically distinct trees. This tree creation should be entirely independent of the action server logic so that it can be used for sub-trees.

Additionally, send_goal, get_result, and get_feedback should be able to be called on any result of create_tree in order to populate / read from the tree's data to implement the action server logic.

Individual behaviors should get their ROS2 Node from setup (as per #105), but the node is currently passed to the ASBT in __init__ in order to:

Be used in all the AS logic (send_goal, get_result, and get_feedback; likely just for the node clock)
Be used in create_tree to handle only behaviors that don't yet get their ROS node in setup.

Once #105 is solved, it may make sense to remove node from init and pass it directly to the AS functions, but I don't think that's necessary.

Testing procedure

Test all MoveTo trees as documented in README (Note: AcquireFood should now fail with a 5s Timeout given an empty message, as there is no valid TF between world and empty string)
Test AcquireFood as documented in Add BlackboardBehavior and initial AcquireFood tree #102
Make sure to run AcquireFood twice to verify that MoveToVisitor is re-initialized (feedback planning_time should start again at 0).

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

…aster logic; Next Step is MoveIt2Plan and MoveIt2Execute Behaviors

amalnanavati · 2023-09-28T02:18:52Z

ada_feeding/ada_feeding/helpers.py

@@ -92,6 +93,87 @@ def quat_between_vectors(vec_from: Vector3, vec_to: Vector3) -> Quaternion:
    return ret


+def add_update_static_tf(


Do you ever intend to let a behavior get the transforms? (e.g., to get the food frame?) If so, I think you should make an analogous getter function for this, as opposed to letting behaviors do it themselves. (To ensure locking logic and such is handled correctly)

Nope, behaviors should get transforms through the TransformListener (get_tf_object() later in the file).

Got it. In that case, why do you need to store a dict of the transforms on the blackboard? If you just publish transforms directly to the Broadcaster as this function gets called, shouldn't the Broadcaster handle overriding old transforms? I'm not sure if the dict functionality is necessary.

We do need the dict.

The STB handles the latching (i.e. holding on to the last published messages), but sendTransform just forwards the transform / list of transforms to the publisher.

Actually handling adding transforms to a list is a feature present in CPP but not Python

I'll make an issue upstream

See ros2/geometry2#631

amalnanavati · 2023-09-28T02:20:11Z

ada_feeding/ada_feeding/helpers.py

@@ -92,6 +93,87 @@ def quat_between_vectors(vec_from: Vector3, vec_to: Vector3) -> Quaternion:
    return ret


+def add_update_static_tf(


Name nit: add_update_static_tf seems confusing to me, why not just set_static_tf? And in the docstring you can document that if there is already a static tf for that key, it will be overridden (or you can make override a boolean flag)

Okay on the name, no need for override boolean.

amalnanavati · 2023-09-28T02:21:30Z

ada_feeding/ada_feeding/visitors/__init__.py

+project. All of these extend VisitorBase.
+"""
+
+from .moveit2_visitor import MoveIt2Visitor


I think this should be called MoveToVisitor, since it is specifically for all the MoveTo trees, right?

I agree specifically because it is for the MoveTo-style action interface.

amalnanavati · 2023-09-28T02:26:12Z

ada_feeding/ada_feeding/visitors/moveit2_visitor.py

Generally looks good, but how is initialization handled for visitors?

Related to the above point, when you test this as part of AcquireFood, have at least one test that involves running AcquireFood twice, to ensure the visitor is reset properly.

Good call, I'll test this.

Added a reinit function that will be called if the tree already has a MoveToVisitor on send_goal.

…r, and separate actionserver functionality out of create_tree.

amalnanavati

LGTM

amalnanavati · 2023-09-28T18:38:52Z

ada_feeding/ada_feeding/trees/acquire_food_tree.py

            memory=True,
            children=[
-                compute_food_frame,
+                py_trees.decorators.Timeout(


I feel like the default duration of 5secs is probably too long to wait. What do you think about 1s?

[WIP] Working MoveIt2Visitor, DebugVisitor, and StaticTransformBroadc…

317cffc

…aster logic; Next Step is MoveIt2Plan and MoveIt2Execute Behaviors

egordon requested a review from amalnanavati September 28, 2023 02:09

egordon self-assigned this Sep 28, 2023

amalnanavati reviewed Sep 28, 2023

View reviewed changes

egordon added 4 commits September 28, 2023 09:32

[WIP] Formatted overhaul of ActionServerBT to be a true tree generato…

2da5dcf

…r, and separate actionserver functionality out of create_tree.

[WIP] Ran pylint

a1b7b7d

Addressed Draft PR comments

7d2d79c

Fixed bug, tested AcquireFood, TODO test other trees

1b8d985

egordon changed the title ~~MoveTo Action Overhaul~~ ActionServerBT Overhaul Sep 28, 2023

Tested all MoveTo actions w/o perception

51cf7ba

This was referenced Sep 28, 2023

[ROS2] Behaviors should set node in setup not __init__ #105

Open

[ROS2] MoveTo Trees Feedback Revamp #59

Open

egordon changed the title ~~ActionServerBT Overhaul~~ ActionServerBT Overhaul and Add Static TF Object Sep 28, 2023

Switch ComputeFoodFrame to get node from setup()

b47d892

egordon requested a review from amalnanavati September 28, 2023 18:14

egordon marked this pull request as ready for review September 28, 2023 18:14

Fixed comment nit

478163e

amalnanavati approved these changes Sep 28, 2023

View reviewed changes

Final nit

d17ad78

egordon merged commit fb19d55 into ros2-devel Sep 28, 2023

egordon deleted the egordon/ros2_movetooverhaul branch September 28, 2023 19:04

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

ActionServerBT Overhaul and Add Static TF Object #114

ActionServerBT Overhaul and Add Static TF Object #114

egordon commented Sep 28, 2023 •

edited

Loading

amalnanavati Sep 28, 2023

egordon Sep 28, 2023

amalnanavati Sep 28, 2023

egordon Sep 28, 2023

egordon Sep 28, 2023

amalnanavati Sep 28, 2023

egordon Sep 28, 2023

amalnanavati Sep 28, 2023

egordon Sep 28, 2023

amalnanavati Sep 28, 2023

egordon Sep 28, 2023

amalnanavati left a comment

amalnanavati Sep 28, 2023

		@@ -92,6 +93,87 @@ def quat_between_vectors(vec_from: Vector3, vec_to: Vector3) -> Quaternion:
		return ret


		def add_update_static_tf(

ActionServerBT Overhaul and Add Static TF Object #114

ActionServerBT Overhaul and Add Static TF Object #114

Conversation

egordon commented Sep 28, 2023 • edited Loading

Description

Testing procedure

Before opening a pull request

Before Merging

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

amalnanavati left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

egordon commented Sep 28, 2023 •

edited

Loading