diff --git a/.gitmodules b/.gitmodules index f5f1d42b..e69de29b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +0,0 @@ -[submodule "dependencies/py_trees_ros"] - path = dependencies/py_trees_ros - url = https://github.com/splintered-reality/py_trees_ros.git diff --git a/dependencies/py_trees_ros b/dependencies/py_trees_ros deleted file mode 160000 index 247cf47d..00000000 --- a/dependencies/py_trees_ros +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 247cf47d509f827bb24464be376e8ed2ddefc0fb diff --git a/dependencies/scenario_execution_py_trees_ros/LICENSE b/dependencies/scenario_execution_py_trees_ros/LICENSE new file mode 100644 index 00000000..2c0a413c --- /dev/null +++ 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 new file mode 100644 index 00000000..90fe0795 --- /dev/null +++ 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. \ No newline at end of file diff --git a/dependencies/scenario_execution_py_trees_ros/package.xml b/dependencies/scenario_execution_py_trees_ros/package.xml new file mode 100644 index 00000000..a3022215 --- /dev/null +++ b/dependencies/scenario_execution_py_trees_ros/package.xml @@ -0,0 +1,16 @@ + + + + scenario_execution_py_trees_ros + 1.2.0 + Fixes in py-trees-ros not yet release + Intel Labs + Intel Labs + BSD-3-Clause + + py_trees_ros + + + ament_python + + 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 new file mode 100644 index 00000000..e69de29b 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 new file mode 100644 index 00000000..4a3632d8 --- /dev/null +++ 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 new file mode 100644 index 00000000..d1412356 --- /dev/null +++ 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 new file mode 100644 index 00000000..3f3f20a9 --- /dev/null +++ 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 new file mode 100644 index 00000000..c8acc7ab --- /dev/null +++ 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='scenario-execution@intel.com', + description='Scenario Execution library for Gazebo', + license='BSD-3-Clause', +) diff --git a/scenario_execution_ros/package.xml b/scenario_execution_ros/package.xml index 494ea898..b1c96158 100644 --- a/scenario_execution_ros/package.xml +++ b/scenario_execution_ros/package.xml @@ -16,6 +16,7 @@ py_trees py_trees_ros py_trees_ros_interfaces + scenario_execution_py_trees_ros visualization_msgs xacro rcl_interfaces diff --git a/scenario_execution_ros/scenario_execution_ros/scenario_execution_ros.py b/scenario_execution_ros/scenario_execution_ros/scenario_execution_ros.py index 79f45f4e..fb823f76 100644 --- a/scenario_execution_ros/scenario_execution_ros/scenario_execution_ros.py +++ b/scenario_execution_ros/scenario_execution_ros/scenario_execution_ros.py @@ -17,7 +17,7 @@ """ Main entry for scenario_execution_ros """ import sys import rclpy # pylint: disable=import-error -import py_trees_ros # pylint: disable=import-error +import scenario_execution_py_trees_ros from py_trees_ros_interfaces.srv import OpenSnapshotStream from scenario_execution import ScenarioExecution, ShutdownHandler from .logging_ros import RosLogger @@ -92,9 +92,9 @@ def setup_behaviour_tree(self, tree): tree [py_trees.behaviour.Behaviour]: root of the behaviour tree return: - py_trees_ros.trees.BehaviourTree + scenario_execution_py_trees_ros.trees.BehaviourTreeKwargs """ - return py_trees_ros.trees.BehaviourTree(tree) + return scenario_execution_py_trees_ros.trees.BehaviourTreeKwargs(tree) def post_setup(self): request = OpenSnapshotStream.Request()