merged PR #41 to allow testing of ros_bt_py

technologies/ros_bt_py/README.md

@@ -0,0 +1,91 @@
+# ros\_bt\_py Setup for ROSCon 2024 Deliberation Technologies Workshop
+---
+
+## Installation and Setup
+
+For the ROSCon '24 Deliberation Technologies workshop all required software is included in the Docker container.
+To build locally at home see the [installation instructions](https://fzi-forschungszentrum-informatik.github.io/ros2_ros_bt_py/index.html).
+
+## Getting Started
+
+To get started with `ros_bt_py` as part of the workshop no additonal packages would be required.
+We provide generic nodes for interacting with ROS Topics, Services and Actions, which only require having the messages available in your environment when starting `ros_bt_py`.
+Nevertheless we provided the `ros_bt_py_pyrobosim` package containing some additonal nodes, providing convinience wrappers, making interacting with `pyrobosim` simpler.
+
+First run one of the problem scenarios from the workshop:
+
+```bash
+ros2 run delib_ws_worlds run --ros-args -p problem_number:=1
+```
+
+To run `ros_bt_py` with these convinience nodes you need to run:
+```bash
+ros2 launch ros_bt_py_pyrobosim ros_bt_py_pyrobosim.launch.py 
+```
+
+Alternativly, to launch the regular `ros_bt_py` without any additional nodes run:
+```bash
+ros2 launch ros_bt_py ros_bt_py.launch.py
+```
+
+After launching you can click one of the shown URLs to open our WebUI in a browser.
+Commonly this is just `http://localhost:8085`.
+
+## Usage
+
+The WebUI has four distinct sections:
+* On the left you have the list of available nodes.
+  These can be moved into the main editing area at the center to add them to the tree.
+  Some nodes (e.g. EnumFields) require you to click on the node in the list and add required details at the bottom of the editing window before they can be added to  the tree via the `AddToTree` button.
+* In the center there is the main editing window.
+  By moving nodes into this area their position within the tree can be adjusted.
+  When connecting nodes from the data terminals at either the left (inputs) or right (outputs) side of a node you can created data connections between nodes.
+  The color of the node will indicate its current state:
+    * Dark Red - Shutdown
+    * Blue - Idle
+    * Yellow - Running
+    * Red - Failure
+    * Green - Succeed
+* The top bar provides execution controls and reports the current status of the tree.
+  Also the load / save controls for trees is located here.
+  Trees can be loaded from ROS packages and from the `.ros` directory.
+  Saving is unfortunately only possible in the `.ros` directory.
+* The center bottom area is where attributes on a clicked node can be edited.
+  ROS message types and builtin python types are automatically completed when a type field is edited.
+> 
+> BUG: When updating node attributs and clicking apply, a popup will say that changes are discarded. This a bug and the changes will apply fully as intended.
+> This will be fixed with the next GUI update.
+>
+
+This explains the basics of the `ros_bt_py` UI. 
+In the following some more details on specific features are given:
+
+### Data Flow
+
+`ros_bt_py` uses a typed datagraph to share data between nodes.
+Each node can expose inputs and outputs which can be connected to other nodes.
+
+All inputs are mandatory, meaning that if an input is not connected or at runtime no value is present, a runtime error will be raised.
+Outputs are populated on the tick the node succeeds.
+
+Data connections are strongly typed for ROS messages and primitive python types.
+While container types (e.g arrays, lists, tuple) are checked, their content types are not.
+Connecting an `list[int]` output to an `list[string]` input will result in a runtime error. 
+
+### ROS Actions & Servuces
+
+`ros_bt_py` has generic nodes for interacting with ROS services, topics and actions.
+The `Serivce` and `Action` nodes allow to call an arbitrary ROS service/action.
+Within both nodes options, the endpoint and type must be specified.
+The `FieldsToMessage` and `MessageToFields` nodes can be used to construct and destruct the `Request/Goal` and `Result` messages from these nodes.
+`Constant` nodes can be used to create primitve python datatype constants.
+>
+> BUG: It should also be possible to generate full ROS message types in constants, but unfortunately due to changes to ROS/Python type introspection
+> this is not possible. ROS messages need to be constructed using the `FieldsToMessage` node.
+> 
+
+## ToDo's
+
+- [ ] Provide example solutiont trees for all problems.
+- [ ] Add convinience nodes for pyrobosim interaction.
+

technologies/ros_bt_py/ros_bt_py_pyrobosim/launch/ros_bt_py_pyrobosim.launch.py

@@ -0,0 +1,25 @@
+import os
+
+from ament_index_python import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+
+    # include default bt_py launch file
+    bt_py_include = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory('ros_bt_py'),
+                'launch/ros_bt_py.launch.py')),
+        launch_arguments={
+            'enable_web_interface': 'True',
+            'node_modules': "['ros_bt_py.nodes','ros_bt_py.ros_nodes','ros_bt_py_pyrobosim.nodes']",
+        }.items()
+    )
+
+    return LaunchDescription([bt_py_include])
+

technologies/ros_bt_py/ros_bt_py_pyrobosim/package.xml

@@ -0,0 +1,21 @@
+<?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>ros_bt_py_pyrobosim</name>
+  <version>1.0.0</version>
+  <description>BT nodes for the ROSCon2024 workshop</description>
+  <maintainer email="[email protected]">David Oberacker</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>pyrobosim_msgs</depend>
+  <depend>ros_bt_py</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/nodes/__init__.py

@@ -0,0 +1,2 @@
+from . import actions
+from . import services

technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/nodes/actions.py

@@ -0,0 +1,11 @@
+import rclpy
+
+from ros_bt_py.node_config import NodeConfig
+from ros_bt_py.node import define_bt_node
+
+from ros_bt_py.ros_nodes.action import ActionForSetType
+from ros_bt_py_interfaces.msg import Node as NodeMsg
+
+from pyrobosim_msgs.action import ExecuteTaskAction
+from pyrobosim_msgs.msg import ExecutionResult
+

technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/nodes/services.py

@@ -0,0 +1,88 @@
+from pyrobosim_msgs.msg import ObjectState, RobotState
+
+from ros_bt_py_interfaces.msg import Node as NodeMsg
+from ros_bt_py_interfaces.msg import UtilityBounds
+
+from ros_bt_py.debug_manager import DebugManager
+from ros_bt_py.node import Leaf, define_bt_node
+from ros_bt_py.node_config import NodeConfig, OptionRef
+from ros_bt_py.exceptions import BehaviorTreeException
+
+from abc import ABC, abstractmethod
+from typing import Optional, Dict, Tuple, List
+from ros_bt_py.ros_nodes.service import ServiceForSetType
+
+from pyrobosim_msgs.srv import RequestWorldState
+
+
+@define_bt_node(
+    NodeConfig(
+        version="0.1.0",
+        options={},
+        inputs={"robot_name": str, "object_name": str},
+        outputs={},
+        max_children=0,
+    )
+)
+class CheckIfCarriesObject(ServiceForSetType):
+
+    def set_service_type(self):
+        self._service_type = RequestWorldState
+
+    # Set all outputs to none (define output key while overwriting)
+    def set_output_none(self):
+        pass
+
+    # Sets the service request message, sent to the service.
+    def set_request(self):
+        self._last_request = RequestWorldState.Request()
+
+    # Sets the output (in relation to the response) (define output key while overwriting)
+    # it should return True, if the node state should be SUCCEEDED after receiving the message and
+    # False. if it should be in the FAILED state
+    def set_outputs(self):
+        response: RequestWorldState.Response = self._service_request_future.result()
+        robot_with_matching_name: List[RobotState] = [robot for robot in response.state.robots if robot.name == self.inputs["robot_name"]]
+        if len(robot_with_matching_name) != 1:
+            self.logerr("Found more then one matching robot!")
+            raise BehaviorTreeException("Found more then one matching robot!")
+        robot = robot_with_matching_name[0]
+        return robot.holding_object and robot.manipulated_object == self.inputs["object_name"]
+
+@define_bt_node(
+    NodeConfig(
+        version="0.1.0",
+        options={},
+        inputs={"object_name": str},
+        outputs={
+            "location": str
+        },
+        max_children=0,
+    )
+)
+class GetObjectPosition(ServiceForSetType):
+
+    def set_service_type(self):
+        self._service_type = RequestWorldState
+
+    # Set all outputs to none (define output key while overwriting)
+    def set_output_none(self):
+        self.outputs["location"] = ""
+        pass
+
+    # Sets the service request message, sent to the service.
+    def set_request(self):
+        self._last_request = RequestWorldState.Request()
+
+    # Sets the output (in relation to the response) (define output key while overwriting)
+    # it should return True, if the node state should be SUCCEEDED after receiving the message and
+    # False. if it should be in the FAILED state
+    def set_outputs(self):
+        response: RequestWorldState.Response = self._service_request_future.result()
+        obj_with_matching_name: List[ObjectState] = [obj for obj in response.state.objects if obj.name == self.inputs["object_name"]]
+        if len(obj_with_matching_name) != 1:
+            self.logerr("Found something other than one matching object!")
+            return False
+        obj = obj_with_matching_name[0]
+        self.outputs["location"] = obj.parent
+        return True

technologies/ros_bt_py/ros_bt_py_pyrobosim/setup.cfg

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

technologies/ros_bt_py/ros_bt_py_pyrobosim/setup.py

@@ -0,0 +1,29 @@
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'ros_bt_py_pyrobosim'
+
+setup(
+    name=package_name,
+    version='1.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
+        (os.path.join('share', package_name, 'trees'), glob(os.path.join('trees', '*.yaml')))
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='David Oberacker',
+    maintainer_email='[email protected]',
+    description='BT nodes for the ROSCon2024 workshop',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
+)