Implement BT sequence model and update test plugin for counting ticks

Signed-off-by: Marco Lampacrescia <marco.lampacrescia@de.bosch.com>

src/as2fm/resources/bt_control_nodes/sequence.scxml

@@ -0,0 +1,56 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<scxml
+    xmlns="http://www.w3.org/2005/07/scxml"
+    initial="wait_for_tick"
+    version="1.0"
+    name="Sequence"
+    model_src="https://raw.githubusercontent.com/BehaviorTree/BehaviorTree.CPP/refs/heads/v3.8/src/controls/sequence_node.cpp">
+
+    <!-- A default BT port reporting the amount of children -->
+    <bt_declare_port_in key="CHILDREN_COUNT" type="int8" />
+
+    <datamodel>
+        <data id="current_child_idx" type="int8" expr="0" />
+        <data id="children_count" type="int8">
+            <expr>
+                <bt_get_input key="CHILDREN_COUNT" />
+            </expr>
+        </data>
+    </datamodel>
+
+    <state id="wait_for_tick">
+        <!-- Check if the state is valid. If not, go to error and stop -->
+        <transition target="error" cond="children_count &lt; 1" />
+        <!-- React to an incoming BT Tick -->
+        <bt_tick target="tick_children"/>
+    </state>
+
+    <state id="tick_children">
+        <onentry>
+            <if cond="current_child_idx &lt; children_count">
+                <bt_tick_child id="current_child_idx"/>
+            </if>
+        </onentry>
+        <bt_child_status id="current_child_idx" cond="_bt.status == SUCCESS" target="tick_children">
+            <assign location="current_child_idx" expr="current_child_idx + 1" />
+        </bt_child_status>
+        <bt_child_status id="current_child_idx" cond="_bt.status == FAILURE" target="wait_for_tick">
+            <assign location="current_child_idx" expr="0" />
+            <bt_return_status status="FAILURE" />
+        </bt_child_status>
+        <bt_child_status id="current_child_idx" cond="_bt.status == RUNNING" target="wait_for_tick">
+            <bt_return_status status="RUNNING" />
+        </bt_child_status>
+        <!-- All children returned success -->
+        <transition target="wait_for_tick" cond="current_child_idx == children_count">
+            <assign location="current_child_idx" expr="0" />
+            <bt_return_status status="SUCCESS" />
+        </transition>
+        <!-- The current_child_idx is out of bounds -->
+        <transition target="error" cond="current_child_idx &lt; 0 || current_child_idx > children_count" />
+    </state>
+
+    <!-- A state to transition to when something did not work -->
+    <state id="error" />
+
+</scxml>

test/jani_generator/_test_data/bt_test_models/bt_count_ticks.scxml

@@ -5,13 +5,27 @@
     version="1.0"
     name="BtCountTicks"
     model_src="">
+    <!--
+        A BT plugin that, upon tick, increases a counter, publishes its value on a topic and
+        * n_ticks_running times, returns running.
+        * After n_ticks_running times, returns success and waits for n_ticks_running ticks before returning success again.
+    -->
+
+    <!-- The topic used to publish the tick count -->
+    <bt_declare_port_in key="topic_name" type="string" />
+    <!-- How many ticks to get before sending out a success -->
+    <bt_declare_port_in key="n_ticks_running" type="int8" />
 
     <datamodel>
         <data id="counter" type="int16" expr="0" />
+        <!-- Assign this for accessing the port value in conditions... -->
+        <data id="ticks_before_success" type="int8">
+            <expr>
+                <bt_get_input key="n_ticks_running" />
+            </expr>
+        </data>
     </datamodel>
 
-    <bt_declare_port_in key="topic_name" type="string" />
-
     <ros_topic_publisher type="std_msgs/Int16" name="counter_pub">
         <topic>
             <bt_get_input key="topic_name" />
@@ -24,7 +38,13 @@
             <ros_topic_publish name="counter_pub">
                 <field name="data" expr="counter" />
             </ros_topic_publish>
-            <bt_return_status status="SUCCESS" />
+            <if cond="ticks_before_success &lt;= 0">
+                <bt_return_status status="SUCCESS" />
+            <elseif cond="counter % ticks_before_success &gt; 0"/>
+                <bt_return_status status="RUNNING" />
+            <else/>
+                <bt_return_status status="SUCCESS" />
+            </if>
         </bt_tick>
     </state>
 

test/jani_generator/_test_data/bt_test_models/bt_test_reactive_fallback.xml

@@ -3,10 +3,10 @@
     <BehaviorTree ID="MainTree">
         <ReactiveFallback name="root_sequence">
             <Inverter>
-                <Action ID="BtCountTicks" topic_name="tick_count_0" />
+                <Action ID="BtCountTicks" topic_name="tick_count_0" n_ticks_running="0" />
             </Inverter>
             <Action ID="AlwaysSuccess" />
-            <Action ID="BtCountTicks" topic_name="tick_count_1" />
+            <Action ID="BtCountTicks" topic_name="tick_count_1" n_ticks_running="0" />
         </ReactiveFallback>
     </BehaviorTree>
 </root>

test/jani_generator/_test_data/bt_test_models/bt_test_reactive_sequence.xml

@@ -2,9 +2,9 @@
 <root BTCPP_format="4" >
     <BehaviorTree ID="MainTree">
         <ReactiveSequence name="root_sequence">
-            <Action ID="BtCountTicks" topic_name="tick_count_0" />
+            <Action ID="BtCountTicks" topic_name="tick_count_0" n_ticks_running="0" />
             <Action ID="AlwaysFailure" />
-            <Action ID="BtCountTicks" topic_name="tick_count_1" />
+            <Action ID="BtCountTicks" topic_name="tick_count_1" n_ticks_running="0" />
         </ReactiveSequence>
     </BehaviorTree>
 </root>