remove py-trees-ros submodule (#199)

IntelLabs · Oct 2, 2024 · 1b699af · 1b699af
1 parent e3a95b1
commit 1b699af
Show file tree

Hide file tree

Showing 12 changed files with 363 additions and 7 deletions.
diff --git a/.gitmodules b/.gitmodules
@@ -1,6 +1,3 @@
-[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
diff --git a/dependencies/py_trees_ros b/dependencies/py_trees_ros
diff --git a/dependencies/scenario_execution_py_trees_ros/LICENSE b/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.
diff --git a/dependencies/scenario_execution_py_trees_ros/README.md b/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.
diff --git a/dependencies/scenario_execution_py_trees_ros/package.xml b/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 file="LICENSE">BSD-3-Clause</license>
+
+  <exec_depend>py_trees_ros</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/dependencies/scenario_execution_py_trees_ros/resource/scenario_execution_py_trees_ros b/dependencies/scenario_execution_py_trees_ros/resource/scenario_execution_py_trees_ros
diff --git a/dependencies/scenario_execution_py_trees_ros/scenario_execution_py_trees_ros/__init__.py b/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
diff --git a/dependencies/scenario_execution_py_trees_ros/scenario_execution_py_trees_ros/trees.py b/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)
diff --git a/dependencies/scenario_execution_py_trees_ros/setup.cfg b/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
diff --git a/dependencies/scenario_execution_py_trees_ros/setup.py b/dependencies/scenario_execution_py_trees_ros/setup.py
@@ -0,0 +1,51 @@
+# 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
+
+""" Setup python package """
+from setuptools import find_namespace_packages, setup
+
+PACKAGE_NAME = 'scenario_execution_py_trees_ros'
+
+setup(
+    name=PACKAGE_NAME,
+    version='1.2.0',
+    packages=find_namespace_packages(),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + PACKAGE_NAME]),
+        ('share/' + PACKAGE_NAME, ['package.xml'])
+    ],
+    zip_safe=True,
+    maintainer='Intel Labs',
+    maintainer_email='[email protected]',
+    description='Scenario Execution library for Gazebo',
+    license='BSD-3-Clause',
+)
diff --git a/scenario_execution_ros/package.xml b/scenario_execution_ros/package.xml
@@ -16,6 +16,7 @@
   <exec_depend>py_trees</exec_depend>
   <exec_depend>py_trees_ros</exec_depend>
   <exec_depend>py_trees_ros_interfaces</exec_depend>
+  <exec_depend>scenario_execution_py_trees_ros</exec_depend>
   <exec_depend>visualization_msgs</exec_depend>
   <exec_depend>xacro</exec_depend>
   <exec_depend>rcl_interfaces</exec_depend>