Add flag to enable tick of non-running BT + UC2 in tests

.github/workflows/test.yml

@@ -55,9 +55,17 @@ jobs:
               if: ${{ matrix.os == 'ubuntu-22.04' }}
             # lint packages
             # TODO: add linting
+            # Prepare ROS WS for the tests
+            - name: Install ROS support pkgs
+              run: |
+                source /opt/ros/${{ matrix.ros-distro }}/setup.bash
+                mkdir ros_interfaces_ws
+                cd ros_interfaces_ws
+                ln -s ../ros_support_interfaces src
+                colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
             # run the tests
             - name: Run tests
               run: |
                 export PATH=$PATH:${{ steps.get_smc_storm.outputs.SMC_STORM_PATH }}
-                source /opt/ros/${{ matrix.ros-distro }}/setup.bash
+                source ros_interfaces_ws/install/setup.bash
                 pytest-3 -vs test/

ros_support_interfaces/uc2_interfaces/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.5)
+project(uc2_interfaces)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+find_package(std_msgs REQUIRED)
+
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/BlockStatus.msg"
+  "srv/GetBlockStatus.srv"
+  "action/MoveBlock.action"
+  "action/MoveBlockImplem.action"
+  "action/RecoverBlock.action"
+  "action/RecoverBlockImplem.action"
+  "action/InspectBlock.action"
+  "action/InspectBlockImplem.action"
+  DEPENDENCIES std_msgs
+ )
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

ros_support_interfaces/uc2_interfaces/README.md

@@ -0,0 +1,7 @@
+# Current status
+This repository stores message, service, action definitions for all the moonshot runs prepared until 31/07/2023, namely `RUN 2` and `RUN 3`.
+
+In the current version, interface definitions from all runs are mixed together. Some of them, though, have become obsolete; others need to undergo a complete re-examination and/or refactoring.
+
+# Future versions
+Next versions of the repository will re-organize the interface definitions to be compatible with the developments from `RUN 3` on.

ros_support_interfaces/uc2_interfaces/action/InspectBlock.action

@@ -0,0 +1,7 @@
+# action pattern
+# attributes of goal
+int32 block_id
+---
+# attributes of result
+---
+# attributes of feedback

ros_support_interfaces/uc2_interfaces/action/InspectBlockImplem.action

@@ -0,0 +1,7 @@
+# action pattern
+# attributes of goal
+int32 block_id
+---
+# attributes of result
+---
+# attributes of feedback

ros_support_interfaces/uc2_interfaces/action/MoveBlock.action

@@ -0,0 +1,7 @@
+# action pattern
+# attributes of goal
+int32 block_id
+---
+# attributes of result
+---
+# attributes of feedback

ros_support_interfaces/uc2_interfaces/action/MoveBlockImplem.action

@@ -0,0 +1,7 @@
+# action pattern
+# attributes of goal
+int32 block_id
+---
+# attributes of result
+---
+# attributes of feedback

ros_support_interfaces/uc2_interfaces/action/RecoverBlock.action

@@ -0,0 +1,7 @@
+# action pattern
+# attributes of goal
+int32 block_id
+---
+# attributes of result
+---
+# attributes of feedback

ros_support_interfaces/uc2_interfaces/action/RecoverBlockImplem.action

@@ -0,0 +1,7 @@
+# action pattern
+# attributes of goal
+int32 block_id
+---
+# attributes of result
+---
+# attributes of feedback

ros_support_interfaces/uc2_interfaces/msg/BlockStatus.msg

@@ -0,0 +1 @@
+int32 block_down

ros_support_interfaces/uc2_interfaces/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>uc2_interfaces</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>std_msgs</depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ros_support_interfaces/uc2_interfaces/srv/GetBlockStatus.srv

@@ -0,0 +1,2 @@
+---
+int32 block_down

src/as2fm/jani_generator/scxml_helpers/top_level_interpreter.py

@@ -42,13 +42,23 @@
 
 @dataclass()
 class FullModel:
+    # The maximum time the model is allowed to run, in nanoseconds
     max_time: Optional[int] = None
+    # Max size of "dynamic" arrays defined in the SCXML models
     max_array_size: int = field(default=100)
+    # Tick rate for the loaded BT
     bt_tick_rate: float = field(default=1.0)
+    # Whether to keep ticking the BT after it returns SUCCESS / FAILURE
+    bt_tick_not_running: bool = field(default=False)
+    # Path to the behavior tree loaded in the model
     bt: Optional[str] = None
+    # Paths to the SCXML models of the BT nodes used in the model
     plugins: List[str] = field(default_factory=list)
+    # Paths to the SCXML models of the non-BT nodes in the model
     skills: List[str] = field(default_factory=list)
+    # Similar to the skills, currently unused
     components: List[str] = field(default_factory=list)
+    # Path to the properties definition, currently in JANI
     properties: List[str] = field(default_factory=list)
 
 
@@ -67,15 +77,7 @@ def _parse_time_element(time_element: ET.Element) -> int:
 
 def parse_main_xml(xml_path: str) -> FullModel:
     """
-    Interpret the top-level XML file as a dictionary.
-
-    The returned dictionary contains the following keys:
-    - max_time: The maximum time in nanoseconds.
-    - bt: The path to the Behavior Tree definition.
-    - plugins: A list of paths to the Behavior Tree plugins.
-    - skills: A list of paths to SCXML files encoding an FSM.
-    - components: Similar to skills, but representing abstract models of existing skills
-    - properties: A list of paths to Jani properties.
+    Interpret the top-level XML file and return it as a FullModel object.
     """
     # Used to generate absolute paths of scxml models
     folder_of_xml = os.path.dirname(xml_path)
@@ -98,6 +100,8 @@ def parse_main_xml(xml_path: str) -> FullModel:
                     model.max_array_size = int(mc_parameter.attrib["value"])
                 elif remove_namespace(mc_parameter.tag) == "bt_tick_rate":
                     model.bt_tick_rate = float(mc_parameter.attrib["value"])
+                elif remove_namespace(mc_parameter.tag) == "bt_tick_if_not_running":
+                    model.bt_tick_not_running = bool(mc_parameter.attrib["value"])
                 else:
                     raise ValueError(
                         error(mc_parameter, f"Invalid mc_parameter tag: {mc_parameter.tag}")
@@ -160,7 +164,9 @@ def generate_plain_scxml_models_and_timers(
         ros_scxmls.append(ScxmlRoot.from_scxml_file(fname))
     # Convert behavior tree and plugins to ROS-SCXML
     if model.bt is not None:
-        ros_scxmls.extend(bt_converter(model.bt, model.plugins, model.bt_tick_rate))
+        ros_scxmls.extend(
+            bt_converter(model.bt, model.plugins, model.bt_tick_rate, model.bt_tick_not_running)
+        )
     # Convert the loaded entries to plain SCXML
     plain_scxml_models = []
     all_timers: List[RosTimer] = []

src/as2fm/scxml_converter/bt_converter.py

@@ -29,6 +29,7 @@
     BtTickChild,
     RosRateCallback,
     RosTimeRate,
+    ScxmlExecutionBody,
     ScxmlRoot,
     ScxmlState,
 )
@@ -60,10 +61,18 @@ def load_available_bt_plugins(bt_plugins_scxml_paths: List[str]) -> Dict[str, Sc
 
 
 def bt_converter(
-    bt_xml_path: str, bt_plugins_scxml_paths: List[str], bt_tick_rate: float
+    bt_xml_path: str,
+    bt_plugins_scxml_paths: List[str],
+    bt_tick_rate: float,
+    tick_if_not_running: bool,
 ) -> List[ScxmlRoot]:
     """
     Generate all Scxml files resulting from a Behavior Tree (BT) in XML format.
+
+    :param bt_xml_path: Path to the xml file implementing the Behavior Tree.
+    :param bt_plugins_scxml_paths: Paths to the scxml files implementing the BT nodes (plugins).
+    :param bt_tick_rate: The rate at which the root of the input BT is ticked.
+    :param tick_if_not_running: If true, keep ticking the BT root after it stops returning RUNNING.
     """
     available_bt_plugins = load_available_bt_plugins(bt_plugins_scxml_paths)
     xml_tree: ET.ElementBase = ET.parse(bt_xml_path, ET.XMLParser(remove_comments=True)).getroot()
@@ -78,36 +87,55 @@ def bt_converter(
     ), f"Error: Expected one BehaviorTree child, found {len(bt_children)}."
     root_child_tick_idx = 1000
     bt_name = os.path.basename(bt_xml_path).replace(".xml", "")
-    bt_scxml_root = generate_bt_root_scxml(bt_name, root_child_tick_idx, bt_tick_rate)
+    bt_scxml_root = generate_bt_root_scxml(
+        bt_name, root_child_tick_idx, bt_tick_rate, tick_if_not_running
+    )
     generated_scxmls = [bt_scxml_root] + generate_bt_children_scxmls(
         bt_children[0], root_child_tick_idx, available_bt_plugins
     )
     return generated_scxmls
 
 
-def generate_bt_root_scxml(scxml_name: str, tick_id: int, tick_rate: float) -> ScxmlRoot:
+def generate_bt_root_scxml(
+    scxml_name: str, tick_id: int, tick_rate: float, tick_if_not_running: bool
+) -> ScxmlRoot:
     """
     Generate the root SCXML for a Behavior Tree.
+
+    :param scxml_name: name of the scxml object to be generated.
+    :param tick_id: A tick ID for the BT Root node.
+    :param tick_rate: The rate at which the root is ticked.
+    :param tick_if_not_running: If true, tick the BT root after it stops returning RUNNING.
     """
     bt_scxml_root = ScxmlRoot(BT_ROOT_PREFIX + scxml_name)
     ros_rate_decl = RosTimeRate(f"{scxml_name}_tick", tick_rate)
     bt_scxml_root.add_ros_declaration(ros_rate_decl)
     idle_state = ScxmlState(
-        "idle", body=[RosRateCallback(ros_rate_decl, "wait_tick_res", None, [BtTickChild(0)])]
+        "idle",
+        body=[
+            RosRateCallback(ros_rate_decl, "wait_tick_res", None, [BtTickChild(0)]),
+            BtChildStatus(0, "error"),
+        ],
+    )
+    tick_res_body: ScxmlExecutionBody = (
+        # In case we keep ticking after BT root finishes running
+        [BtChildStatus(0, "idle")]
+        if tick_if_not_running
+        # In case we stop the BT after the BT root result is not RUNNING
+        else [
+            BtChildStatus(0, "idle", "_bt.status == RUNNING"),
+            # This is the case in which BT-status != RUNNING
+            BtChildStatus(0, "done"),
+        ]
     )
     wait_res_state = ScxmlState(
         "wait_tick_res",
-        body=[
-            # If we allow timer ticks here, the automata will generate timer callbacks and make the
-            # BT automaton transition to error state (since our concept of time is not real).
-            # RosRateCallback(ros_rate_decl, "error"),
-            BtChildStatus(0, "idle")
-        ],
+        body=tick_res_body,
     )
-    error_state = ScxmlState("error")
     bt_scxml_root.add_state(idle_state, initial=True)
     bt_scxml_root.add_state(wait_res_state)
-    bt_scxml_root.add_state(error_state)
+    bt_scxml_root.add_state(ScxmlState("done"))
+    bt_scxml_root.add_state(ScxmlState("error"))
     # The BT root's ID is set to -1 (unused anyway)
     bt_scxml_root.set_bt_plugin_id(-1)
     bt_scxml_root.append_bt_child_id(tick_id)

test/jani_generator/_test_data/bt_test_models/main_test_fallback.xml

@@ -2,6 +2,7 @@
     <mc_parameters>
         <max_time value="100" unit="s" />
         <bt_tick_rate value="1.0" />
+        <bt_tick_if_not_running value="true" />
     </mc_parameters>
 
     <behavior_tree>

test/jani_generator/_test_data/bt_test_models/main_test_reactive_fallback.xml

@@ -2,6 +2,7 @@
     <mc_parameters>
         <max_time value="100" unit="s" />
         <bt_tick_rate value="1.0" />
+        <bt_tick_if_not_running value="true" />
     </mc_parameters>
 
     <behavior_tree>

test/jani_generator/_test_data/bt_test_models/main_test_reactive_sequence.xml

@@ -2,6 +2,7 @@
     <mc_parameters>
         <max_time value="100" unit="s" />
         <bt_tick_rate value="1.0" />
+        <bt_tick_if_not_running value="true" />
     </mc_parameters>
 
     <behavior_tree>

test/jani_generator/_test_data/bt_test_models/main_test_sequence.xml

@@ -2,6 +2,7 @@
     <mc_parameters>
         <max_time value="100" unit="s" />
         <bt_tick_rate value="1.0" />
+        <bt_tick_if_not_running value="true" />
     </mc_parameters>
 
     <behavior_tree>

test/jani_generator/_test_data/ros_example_w_bt/main.xml

@@ -1,6 +1,7 @@
 <convince_mc_tc>
     <mc_parameters>
         <max_time value="100" unit="s" />
+        <bt_tick_if_not_running value="true" />
     </mc_parameters>
 
     <behavior_tree>

test/jani_generator/_test_data/ros_example_w_bt_deprecated/main.xml

@@ -1,6 +1,7 @@
 <convince_mc_tc>
     <mc_parameters>
         <max_time value="100" unit="s" />
+        <bt_tick_if_not_running value="true" />
     </mc_parameters>
 
     <behavior_tree>

test/jani_generator/_test_data/uc1_docking/main.xml

@@ -2,6 +2,7 @@
     <mc_parameters>
         <max_time value="20" unit="s" />
         <bt_tick_rate value="5.0" />
+        <bt_tick_if_not_running value="true" />
     </mc_parameters>
 
     <behavior_tree>

test/jani_generator/_test_data/uc1_docking/main_with_problem.xml

@@ -2,6 +2,7 @@
     <mc_parameters>
         <max_time value="20" unit="s" />
         <bt_tick_rate value="5.0" />
+        <bt_tick_if_not_running value="true" />
     </mc_parameters>
 
     <behavior_tree>

test/jani_generator/_test_data/uc2_assembly/Components/Clock.scxml

@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<scxml
+    initial="advance_time"
+    version="1.0"
+    name="Clock"
+    model_src=""
+    xmlns="http://www.w3.org/2005/07/scxml">
+
+    <datamodel>
+        <data id="time" type="int32" expr="0" />
+    </datamodel>
+
+    <ros_topic_publisher name="clock_pub" topic="/uc2/info/clock" type="std_msgs/Int32" />
+    <ros_time_rate name="clock_timer" rate_hz="1" />
+
+    <state id="advance_time">
+        <onentry>
+            <ros_topic_publish name="clock_pub">
+                <field name="data" expr="time" />
+            </ros_topic_publish>
+        </onentry>
+        <ros_rate_callback name="clock_timer" target="advance_time">
+            <assign location="time" expr="time + 1" />
+        </ros_rate_callback>
+    </state>
+</scxml>