sea-bass / turtlebot3_behavior_demos

Example repository for autonomous behaviors using TurtleBot3, as well as Docker workflows in ROS based projects.
MIT License
270 stars 53 forks source link

Decorator node inconsistency issue #23

Closed ShrutheeshIR closed 3 months ago

ShrutheeshIR commented 1 year ago

Thanks for the great set of examples! They're super helpful. I'm looking to use py_trees for my setup and I'm following the examples that you've given, especially with the go to locations decorator node.

Let us say I have a fallback node : OR that can either do action 1 or action 2. Action1 is a searchAndGoto node that has your decorator node. It is a sequence that first searches for a location, and goes to that location, inside a decorator node that performs this until it is successful. Action2 is just another action (that always succeeds, let's say). As per this setup, I would like the fallback node to complete Action1, i.e. exhaust all the locations, and then try action2 only if action1 fails. Here's the associated code snippet:

`

    sel = py_trees.composites.Selector(name = "or", memory=True)
    seq = py_trees.composites.Sequence(name="search", memory=True)
    dec = py_trees.decorators.OneShot(
        name='root', child=seq, policy=py_trees.common.OneShotPolicy.ON_SUCCESSFUL_COMPLETION
    )
    tree = py_trees_ros.trees.BehaviourTree(sel, record_rosbag=False)

    tree.setup(timeout = 15.0)
    sel.add_child(dec)
    sel.add_child(GoToPose(
            name = "action2"
        ))

    seq.add_children([
        GetFVFromQueue(
            name = "get FV", blackboard_key="loc_list", fv='loc'
        ),
        GoToPose(
            name = "gotopose"
        )
    ])

`

Here's the output for the same.

('Go to loc choice is : ', 2)
[ INFO] get FV               : Terminated with status Status.SUCCESS

{o} or [*]
    -^- root [*]
        {-} search [*]
            --> get FV [✓]
            --> gotopose [*]
    --> action2

Blackboard Activity Stream

{o} or [*]
    -^- root [*]
        {-} search [*]
            --> get FV
            --> gotopose [*]
    --> action2

Blackboard Activity Stream

{o} or [*]
    -^- root [✕]
        {-} search [✕]
            --> get FV
            --> gotopose [✕]
    --> action2 [*]

Blackboard Activity Stream
[ INFO] get FV               : Terminated with status Status.INVALID

{o} or [*]
    -^- root
        {-} search
            --> get FV
            --> gotopose
    --> action2 [*]

Blackboard Activity Stream

{o} or [✓]
    -^- root
        {-} search
            --> get FV
            --> gotopose
    --> action2 [✓]

As you can see here, as soon as action1 (gotopose) fails for the first location, it fires action2. I would like for it to be stuck in action1 until the queue is empty. What's the best way to implement this?

Thanks

sea-bass commented 1 year ago

To me, it seems you may want to pull from the either_or idiom available here: https://py-trees.readthedocs.io/en/devel/idioms.html#either-or

So basically the condition is the success of get FV -- does this return success or failure? -- if success, run "action1", else, run "action2".

You'll then probably want to stick a Retry decorator on top of that so you... well, keep retrying after the first "action1" failure.

From your other issue to py_trees, I think you're in an older version so maybe you'll need to implement this yourself?