Update jazzy branch (#203)

.github/workflows/test_build.yml

@@ -26,6 +26,10 @@ jobs:
         shell: bash
         run: |
           source /opt/ros/${{ github.event.pull_request.base.ref == 'main' && 'humble' || github.event.pull_request.base.ref }}/setup.bash
+          git clone https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git
+          cd nav2_minimal_turtlebot_simulation
+          git checkout f4eefd049359b3f0becabd7a0870005f4140ec08
+          cd ..
           colcon build --continue-on-error
           source install/setup.bash
       - name: Cache Build

.gitmodules

@@ -1,6 +0,0 @@
-[submodule "dependencies/py_trees_ros"]
-	path = dependencies/py_trees_ros
-	url = https://github.com/splintered-reality/py_trees_ros.git
-[submodule "dependencies/nav2_minimal_turtlebot_simulation"]
-	path = dependencies/nav2_minimal_turtlebot_simulation
-	url = https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git

CITATION.cff

@@ -17,7 +17,7 @@ title: "Scenario Execution for Robotics"
 date-released: "2024-03-05"
 url: "https://github.com/intellabs/scenario_execution"
 preferred-citation:
-  type: unpublished
+  type: article
   authors:
     - family-names: "Pasch"
       given-names: "Frederik"
@@ -33,4 +33,4 @@ preferred-citation:
       orcid: "https://orcid.org/0000-0001-6337-1798"
   title: "Scenario Execution for Robotics: A generic, backend-agnostic library for running reproducible robotics experiments and tests"
   year: 2024
-  notes: "Submitted for publication, currently under review"
+  url: "https://arxiv.org/abs/2409.07080"

README.md

@@ -17,6 +17,23 @@ Upon arrival of the goal position, the scenario ends and the simulation gets cle
 
 Please find the documentation [here](https://intellabs.github.io/scenario_execution).
 
+## How to cite
+
+If you use Scenario Execution for Robotics in your scientific work, please cite our paper [Scenario Execution for Robotics: A generic, backend-agnostic library for running reproducible robotics experiments and tests](https://arxiv.org/pdf/2409.07080)
+
+```bibtex
+@Article{Pasch2024,
+  author = {Pasch, Frederik and Mirus, Florian and Zhang, Yongzhou and Scholl, Kay-Ulrich},
+  title = {Scenario Execution for Robotics: A generic, backend-agnostic library for running reproducible robotics experiments and tests},
+  journal = {Computing Research Repository (CoRR)},
+  year = {2024},
+  eprint = {2409.07080},
+  archivePrefix = {arXiv},
+  primaryClass = {cs.RO},
+  url = {https://arxiv.org/abs/2409.07080},
+}
+```
+
 ## Setup
 
 ### Installation from source as ROS 2 workspace
@@ -54,3 +71,4 @@ To launch a scenario with ROS2:
 ```bash
 ros2 launch scenario_execution_ros scenario_launch.py scenario:=examples/example_scenario/hello_world.osc live_tree:=True
 ```
+

dependencies/scenario_execution_py_trees_ros/LICENSE

@@ -0,0 +1,29 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2024 Intel Labs
+Copyright (c) 2015 Daniel Stonier
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

dependencies/scenario_execution_py_trees_ros/README.md

@@ -0,0 +1,3 @@
+# Scenario Execution Py Trees ROS
+
+This package provides the latest [`py_trees_ros.trees.BehaviourTree`](https://github.com/splintered-reality/py_trees_ros/blob/247cf47d509f827bb24464be376e8ed2ddefc0fb/py_trees_ros/trees.py#L216) that is not yet part of a release package.

dependencies/scenario_execution_py_trees_ros/package.xml

@@ -0,0 +1,16 @@
+<?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>scenario_execution_py_trees_ros</name>
+  <version>1.2.0</version>
+  <description>Fixes in py-trees-ros not yet release</description>
+  <author email="[email protected]">Intel Labs</author>
+  <maintainer email="[email protected]">Intel Labs</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <exec_depend>py_trees_ros</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

dependencies/scenario_execution_py_trees_ros/scenario_execution_py_trees_ros/__init__.py

@@ -0,0 +1,32 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2024 Intel Labs
+
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+
+# * Redistributions of source code must retain the above copyright notice, this
+#   list of conditions and the following disclaimer.
+
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+
+# * Neither the name of the copyright holder nor the names of its
+#   contributors may be used to endorse or promote products derived from
+#   this software without specific prior written permission.
+
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+from . import trees

dependencies/scenario_execution_py_trees_ros/scenario_execution_py_trees_ros/trees.py

@@ -0,0 +1,224 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2024 Intel Labs
+# Copyright (c) 2015 Daniel Stonier
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#        notice, this list of conditions and the following disclaimer.
+#    * Redistributions in binary form must reproduce the above
+#        copyright notice, this list of conditions and the following
+#        disclaimer in the documentation and/or other materials provided
+#        with the distribution.
+#    * Neither the name of Yujin Robot nor the names of its
+#        contributors may be used to endorse or promote products derived
+#        from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+import typing
+
+import py_trees
+import py_trees_ros_interfaces.srv as py_trees_srvs  # noqa
+import rcl_interfaces.msg as rcl_interfaces_msgs
+import rclpy
+
+from py_trees_ros import blackboard
+from py_trees_ros import exceptions
+from py_trees_ros import utilities
+from py_trees_ros import visitors
+
+from py_trees_ros.trees import BehaviourTree
+from py_trees_ros.trees import SnapshotStream
+
+
+class BehaviourTreeKwargs(BehaviourTree):
+    """
+    Extend the :class:`py_trees_ros.trees.BehaviourTree` class with a required feature that is not yet available
+    as a release.
+    
+    For details, see:
+    https://github.com/splintered-reality/py_trees_ros/commit/247cf47d509f827bb24464be376e8ed2ddefc0fb
+    """
+
+    def setup(
+            self,
+            node: typing.Optional[rclpy.node.Node]=None,
+            node_name: str="tree",
+            timeout: float=py_trees.common.Duration.INFINITE,
+            visitor: typing.Optional[py_trees.visitors.VisitorBase]=None,
+            **kwargs: int
+    ):
+        """
+        Copy of py_trees_ros.trees.BehaviourTree plus kwargs forwarding.
+        """
+        # node creation - can raise rclpy.exceptions.NotInitializedException
+        if node:
+            # Use existing node if one is passed in, and is of the correct type.
+            if isinstance(node, rclpy.node.Node):
+                self.node = node
+            else:
+                raise TypeError(f"invalid node object [received: {type(node)}][expected: rclpy.node.Node]")
+        else:
+            # Node creation - can raise rclpy.exceptions.NotInitializedException
+            self.node = rclpy.create_node(node_name=node_name)
+        if visitor is None:
+            visitor = visitors.SetupLogger(node=self.node)
+        self.default_snapshot_stream_topic_name = SnapshotStream.expand_topic_name(
+            node=self.node,
+            topic_name="~/snapshots"
+        )
+
+        ########################################
+        # ROS Comms
+        ########################################
+        self.snapshot_stream_services = utilities.Services(
+            node=self.node,
+            service_details=[
+                ("close", "~/snapshot_streams/close", py_trees_srvs.CloseSnapshotStream, self._close_snapshot_stream),
+                ("open", "~/snapshot_streams/open", py_trees_srvs.OpenSnapshotStream, self._open_snapshot_stream),
+                ("reconfigure", "~/snapshot_streams/reconfigure", py_trees_srvs.ReconfigureSnapshotStream, self._reconfigure_snapshot_stream),
+            ],
+            introspection_topic_name="snapshot_streams/services"
+        )
+        self.blackboard_exchange = blackboard.Exchange()
+        self.blackboard_exchange.setup(self.node)
+
+        ################################################################################
+        # Parameters
+        ################################################################################
+        self.node.add_on_set_parameters_callback(
+            callback=self._set_parameters_callback
+        )
+
+        ########################################
+        # default_snapshot_stream
+        ########################################
+        self.node.declare_parameter(
+            name='default_snapshot_stream',
+            value=False,
+            descriptor=rcl_interfaces_msgs.ParameterDescriptor(
+                name="default_snapshot_stream",
+                type=rcl_interfaces_msgs.ParameterType.PARAMETER_BOOL,  # noqa
+                description="enable/disable the default snapshot stream in ~/snapshots",
+                additional_constraints="",
+                read_only=False,
+            )
+        )
+
+        ########################################
+        # default_snapshot_period
+        ########################################
+        self.node.declare_parameter(
+            name='default_snapshot_period',
+            value=2.0,  # DJS: py_trees.common.Duration.INFINITE.value,
+            descriptor=rcl_interfaces_msgs.ParameterDescriptor(
+                name="default_snapshot_period",
+                type=rcl_interfaces_msgs.ParameterType.PARAMETER_DOUBLE,  # noqa
+                description="time between snapshots, set to math.inf to only publish tree state changes",
+                additional_constraints="",
+                read_only=False,
+                floating_point_range=[rcl_interfaces_msgs.FloatingPointRange(
+                    from_value=0.0,
+                    to_value=py_trees.common.Duration.INFINITE.value)]
+            )
+        )
+
+        ########################################
+        # default_snapshot_blackboard_data
+        ########################################
+        self.node.declare_parameter(
+            name='default_snapshot_blackboard_data',
+            value=True,
+            descriptor=rcl_interfaces_msgs.ParameterDescriptor(
+                name="default_snapshot_blackboard_data",
+                type=rcl_interfaces_msgs.ParameterType.PARAMETER_BOOL,  # noqa
+                description="append blackboard data (tracking status, visited variables) to the default snapshot stream",
+                additional_constraints="",
+                read_only=False,
+            )
+        )
+
+        ########################################
+        # default_snapshot_blackboard_activity
+        ########################################
+        self.node.declare_parameter(
+            name='default_snapshot_blackboard_activity',
+            value=False,
+            descriptor=rcl_interfaces_msgs.ParameterDescriptor(
+                name="default_snapshot_blackboard_activity",
+                type=rcl_interfaces_msgs.ParameterType.PARAMETER_BOOL,  # noqa
+                description="append the blackboard activity stream to the default snapshot stream",
+                additional_constraints="",
+                read_only=False,
+            )
+        )
+
+        ########################################
+        # setup_timeout
+        ########################################
+        self.node.declare_parameter(
+            name='setup_timeout',
+            value=timeout if timeout != py_trees.common.Duration.INFINITE else py_trees.common.Duration.INFINITE.value,
+            descriptor=rcl_interfaces_msgs.ParameterDescriptor(
+                name="setup_timeout",
+                type=rcl_interfaces_msgs.ParameterType.PARAMETER_DOUBLE,  # noqa
+                description="timeout for ROS tree setup (node, pubs, subs, ...)",
+                additional_constraints="",
+                read_only=True,
+                floating_point_range=[rcl_interfaces_msgs.FloatingPointRange(
+                    from_value=0.0,
+                    to_value=py_trees.common.Duration.INFINITE.value)]
+            )
+        )
+        # Get the resulting timeout
+        setup_timeout = self.node.get_parameter("setup_timeout").value
+        # Ugly workaround to accomodate use of the enum (TODO: rewind this)
+        #   Need to pass the enum for now (instead of just a float) in case
+        #   there are behaviours out in the wild that apply logic around the
+        #   use of the enum
+        if setup_timeout == py_trees.common.Duration.INFINITE.value:
+            setup_timeout = py_trees.common.Duration.INFINITE
+
+        ########################################
+        # Behaviours
+        ########################################
+        try:
+            py_trees.trees.BehaviourTree.setup(
+                self,
+                timeout=setup_timeout,
+                visitor=visitor,
+                node=self.node,
+                **kwargs
+            )
+        except RuntimeError as e:
+            if str(e) == "tree setup interrupted or timed out":
+                raise exceptions.TimedOutError(str(e))
+            else:
+                raise
+
+        ########################################
+        # Setup Handlers
+        ########################################
+        # set a handler to publish future modifications whenever the tree is modified
+        # (e.g. pruned). The tree_update_handler method is in the base class, set this
+        # to the callback function here.
+        self.tree_update_handler = self._on_tree_update_handler
+        self.post_tick_handlers.append(self._snapshots_post_tick_handler)

dependencies/scenario_execution_py_trees_ros/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/scenario_execution_py_trees_ros
+[install]
+install_scripts=$base/lib/scenario_execution_py_trees_ros