Multiple actions issue

[teyssieuman]

This PR is not intended to be integrated as it is, but aim to demonstrate an issue in ros2_planning_system

In this branch, the plansys2_simple_example has been modified to add a patrol action, as the one that already existed in plansys2_patrol_navigation_example.

This example is then executed as described initially but replacing the "terminal commands" with the following ones :

set instance leia robot

set instance ro1 room
set instance ro2 room
set instance ro3 room

set predicate (connected ro1 ro2)
set predicate (connected ro2 ro1)

set predicate (connected ro1 ro3)
set predicate (connected ro3 ro1)

set predicate (connected ro2 ro3)
set predicate (connected ro3 ro2)

set predicate (robot_at leia ro1)

set predicate (battery_full leia)

set goal (and (patrolled ro1) (patrolled ro2) (patrolled ro3)) 

get plan

run

As expected, there is no problem finding a plan:

0       (patrol leia ro1)       2
2.001   (move leia ro1 ro2)     5
7.002   (patrol leia ro2)       2
9.003   (move leia ro2 ro3)     5
14.004  (patrol leia ro3)       2

A problem appears in the plan execution:

• The execution of the patrol leia r01 and the move leia ro1 ro1 start simultaneously
• The execution stops just after those two actions...

Here is the log:

Patroling ... [100%]  
Moving ... [100%]  
[plansys2_node-1] [ERROR] [1610100504.616155368] [executor]: Executor BT finished with FAILURE state
[plansys2_node-1] [INFO] [1610100504.616407292] [executor]: Plan Succeeded

[teyssieuman]

@fmrico : I do not create an issue in order to reference a branch. This PR can be modified if you thing it can be integrated into the master repository, or it can stay a place to discuss...

[fmrico]

@teyssieuman, I have to debug this :thinking:

[fmrico]

The problem is in the creation of the BT:

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Parallel success_threshold="2" failure_threshold="1">
      <Sequence name="(patrol leia ro1):0">
        <WaitAtStartReq action="(patrol leia ro1):0"/>
        <ApplyAtStartEffect action="(patrol leia ro1):0"/>
        <Parallel success_threshold="2" failure_threshold="1">
          <CheckOverAllReq action="(patrol leia ro1):0"/>
          <ExecuteAction action="(patrol leia ro1):0"/>
        </Parallel>
        <CheckAtEndReq action="(patrol leia ro1):0"/>
        <ApplyAtEndEffect action="(patrol leia ro1):0"/>
      </Sequence>
      <Sequence name="(move leia ro1 ro2):2">
        <Sequence name="(move leia ro1 ro2):2">
          <WaitAtStartReq action="(move leia ro1 ro2):2"/>
          <ApplyAtStartEffect action="(move leia ro1 ro2):2"/>
          <Parallel success_threshold="2" failure_threshold="1">
            <CheckOverAllReq action="(move leia ro1 ro2):2"/>
            <ExecuteAction action="(move leia ro1 ro2):2"/>
          </Parallel>
          <CheckAtEndReq action="(move leia ro1 ro2):2"/>
          <ApplyAtEndEffect action="(move leia ro1 ro2):2"/>
        </Sequence>
        <Parallel success_threshold="2"  failure_threshold="1">
          <Sequence name="(move leia ro2 ro3):9">
            <Sequence name="(move leia ro2 ro3):9">
              <WaitAtStartReq action="(move leia ro2 ro3):9"/>
              <ApplyAtStartEffect action="(move leia ro2 ro3):9"/>
              <Parallel success_threshold="2" failure_threshold="1">
                <CheckOverAllReq action="(move leia ro2 ro3):9"/>
                <ExecuteAction action="(move leia ro2 ro3):9"/>
              </Parallel>
              <CheckAtEndReq action="(move leia ro2 ro3):9"/>
              <ApplyAtEndEffect action="(move leia ro2 ro3):9"/>
            </Sequence>
            <Sequence name="(patrol leia ro3):14">
              <WaitAtStartReq action="(patrol leia ro3):14"/>
              <ApplyAtStartEffect action="(patrol leia ro3):14"/>
              <Parallel success_threshold="2" failure_threshold="1">
                <CheckOverAllReq action="(patrol leia ro3):14"/>
                <ExecuteAction action="(patrol leia ro3):14"/>
              </Parallel>
              <CheckAtEndReq action="(patrol leia ro3):14"/>
              <ApplyAtEndEffect action="(patrol leia ro3):14"/>
            </Sequence>
          </Sequence>
          <Sequence name="(patrol leia ro2):7">
            <WaitAtStartReq action="(patrol leia ro2):7"/>
            <ApplyAtStartEffect action="(patrol leia ro2):7"/>
            <Parallel success_threshold="2" failure_threshold="1">
              <CheckOverAllReq action="(patrol leia ro2):7"/>
              <ExecuteAction action="(patrol leia ro2):7"/>
            </Parallel>
            <CheckAtEndReq action="(patrol leia ro2):7"/>
            <ApplyAtEndEffect action="(patrol leia ro2):7"/>
          </Sequence>
        </Parallel>
      </Sequence>
    </Parallel>
  </BehaviorTree>
</root>

Let me dig into this today...

[teyssieuman]

Do you think that such a test case shall be included in the tests ?

[fmrico]

Do you think that such a test case shall be included in the tests ?

Yes, but it will take some days to fix this. It is not direct.

[fmrico]

Hi @teyssieuman

After a lot of code, I got a new version of the BTBuilder that fix some issues, this among them.

Now the execution is correct and the BT seems reasonable:

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Sequence name="(patrol leia ro1):0">
      <Sequence name="(patrol leia ro1):0">
        <ApplyAtStartEffect action="(patrol leia ro1):0"/>
        <Parallel success_threshold="2" failure_threshold="1">
          <CheckOverAllReq action="(patrol leia ro1):0"/>
          <ExecuteAction action="(patrol leia ro1):0"/>
        </Parallel>
        <CheckAtEndReq action="(patrol leia ro1):0"/>
        <ApplyAtEndEffect action="(patrol leia ro1):0"/>
      </Sequence>
      <Sequence name="(move leia ro1 ro2):2000">
        <Sequence name="(move leia ro1 ro2):2000">
          <WaitAtStartReq action="(patrol leia ro1):0"/>
          <ApplyAtStartEffect action="(move leia ro1 ro2):2000"/>
          <Parallel success_threshold="2" failure_threshold="1">
            <CheckOverAllReq action="(move leia ro1 ro2):2000"/>
            <ExecuteAction action="(move leia ro1 ro2):2000"/>
          </Parallel>
          <CheckAtEndReq action="(move leia ro1 ro2):2000"/>
          <ApplyAtEndEffect action="(move leia ro1 ro2):2000"/>
        </Sequence>
        <Sequence name="(patrol leia ro2):7002">
          <Sequence name="(patrol leia ro2):7002">
            <WaitAtStartReq action="(move leia ro1 ro2):2000"/>
            <ApplyAtStartEffect action="(patrol leia ro2):7002"/>
            <Parallel success_threshold="2" failure_threshold="1">
              <CheckOverAllReq action="(patrol leia ro2):7002"/>
              <ExecuteAction action="(patrol leia ro2):7002"/>
            </Parallel>
            <CheckAtEndReq action="(patrol leia ro2):7002"/>
            <ApplyAtEndEffect action="(patrol leia ro2):7002"/>
          </Sequence>
          <Sequence name="(move leia ro2 ro3):9003">
            <Sequence name="(move leia ro2 ro3):9003">
              <WaitAtStartReq action="(patrol leia ro2):7002"/>
              <ApplyAtStartEffect action="(move leia ro2 ro3):9003"/>
              <Parallel success_threshold="2" failure_threshold="1">
                <CheckOverAllReq action="(move leia ro2 ro3):9003"/>
                <ExecuteAction action="(move leia ro2 ro3):9003"/>
              </Parallel>
              <CheckAtEndReq action="(move leia ro2 ro3):9003"/>
              <ApplyAtEndEffect action="(move leia ro2 ro3):9003"/>
            </Sequence>
            <Sequence name="(patrol leia ro3):14004">
              <WaitAtStartReq action="(move leia ro2 ro3):9003"/>
              <ApplyAtStartEffect action="(patrol leia ro3):14004"/>
              <Parallel success_threshold="2" failure_threshold="1">
                <CheckOverAllReq action="(patrol leia ro3):14004"/>
                <ExecuteAction action="(patrol leia ro3):14004"/>
              </Parallel>
              <CheckAtEndReq action="(patrol leia ro3):14004"/>
              <ApplyAtEndEffect action="(patrol leia ro3):14004"/>
            </Sequence>
          </Sequence>
        </Sequence>
      </Sequence>
    </Sequence>
  </BehaviorTree>
</root>

[teyssieuman]

Such a great news. How can I test it?

[fmrico]

I have just merged into master after passing the tests. Please, test it ;)

[teyssieuman]

I just looked at the modification... It's quite a big work !

[teyssieuman]

Everything works like a charm ... In case you think this should be integer in the examples... But I can also close this PR.

[fmrico]

I have included your example in the tests. Maybe it is redundant as a new example.

I think you can close the PR 😉